add slam_toolbox example

crazyflie/scripts/crazyflie_server.py

@@ -506,6 +506,18 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri):
 
  self.swarm._cfs[uri].logging["odom_publisher"].publish(msg)
 
+ t_base = TransformStamped()
+ t_base.header.stamp = self.get_clock().now().to_msg()
+ t_base.header.frame_id = 'odom'
+ t_base.child_frame_id = cf_name
+ t_base.transform.translation.x = x
+ t_base.transform.translation.y = y
+ t_base.transform.rotation.x = q[0]
+ t_base.transform.rotation.y = q[1]
+ t_base.transform.rotation.z = q[2]
+ t_base.transform.rotation.w = q[3]
+ self.tfbr.sendTransform(t_base)
+
  def _log_custom_data_callback(self, timestamp, data, logconf, uri):
  """
  Once custom log block is retrieved from the Crazyflie, 

crazyflie_examples/launch/multiranger_mapping_launch.py

@@ -0,0 +1,57 @@
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch_ros.actions import Node
+from launch.conditions import LaunchConfigurationEquals
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+ # load crazyflies
+ crazyflies_yaml = os.path.join(
+ get_package_share_directory('crazyflie'),
+ 'config',
+ 'crazyflies.yaml')
+
+ with open(crazyflies_yaml, 'r') as ymlfile:
+ crazyflies = yaml.safe_load(ymlfile)
+
+ server_params = crazyflies
+
+ return LaunchDescription([
+ Node(
+ package='crazyflie',
+ executable='crazyflie_server.py',
+ name='crazyflie_server',
+ output='screen',
+ parameters=[server_params],
+ #remappings = [('/cmd_vel','/cf2/cmd_vel_2d')]
+ ),
+ Node(
+ package='crazyflie',
+ executable='vel_mux.py',
+ name='vel_mux',
+ output='screen',
+ parameters=[{"hover_height": 0.3},
+ {"incoming_twist_topic": "/cmd_vel"},
+ {"robot_prefix": "/cf1"}]
+ ),
+ Node(
+ parameters=[
+ {'odom_frame': 'odom'},
+ {'map_frame': 'world'},
+ {'base_frame': 'cf1'},
+ {'scan_topic': '/cf1/scan'},
+ {'use_scan_matching': False},
+ {'max_laser_range': 3.5},
+ {'resolution': 0.1},
+ {'minimum_travel_distance': 0.01},
+ {'minimum_travel_heading': 0.001},
+ {'map_update_interval': 0.1}
+ ],
+ package='slam_toolbox',
+ executable='async_slam_toolbox_node',
+ name='slam_toolbox',
+ output='screen'),
+ ])