def generate_launch_description(): entities = [ ExecuteProcess( cmd=[ 'gzserver', '--verbose', '-s', 'libgazebo_ros_init.so', # Publish /clock '-s', 'libgazebo_ros_factory.so', # Provide injection endpoints world_filename ], output='screen'), # Joystick driver, generates /namespace/joy messages # Node(package='joy', node_executable='joy_node', output='screen'), # Add forward-facing camera to the simulation # Node(package='sim_fiducial', executable='inject_entity.py', output='screen', # arguments=[forward_camera_sdf, '0', '0', '0', '0', '0', '0']), Node(package='sim_fiducial', executable='inject_entity.py', output='screen', arguments=[elemental_camera_sdf, '0', '0', '0', '0', '0', '0']), Node(package='fiducial_vlam', executable='vloc_main', output='screen', parameters=vloc_args, namespace='forward_camera'), Node(package='fiducial_vlam', executable='vmap_main', output='screen', parameters=vmap_args), ] return LaunchDescription(entities)
def generate_launch_description(): # launch paths metrobot_description_dir = get_package_share_directory( 'metrobot_description') metrobot_simulation_dir = get_package_share_directory( 'metrobot_simulation') # Create the launch configuration variables use_sim_time = LaunchConfiguration('use_sim_time', default='True') launch_gazebo_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([ get_package_share_directory('gazebo_ros'), '/launch/gazebo.launch.py' ])) set_use_sim_time_cmd = ExecuteProcess( cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], output='screen') launch_robot_description_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([ metrobot_description_dir, '/launch/metrobot_description.launch.py' ]), launch_arguments={'use_sim_time': use_sim_time}.items()) spawn_robot_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( [metrobot_simulation_dir, '/launch/spawn_metrobot.launch.py'])) ld = LaunchDescription() ld.add_action(launch_gazebo_cmd) ld.add_action(set_use_sim_time_cmd) ld.add_action(launch_robot_description_cmd) ld.add_action(spawn_robot_cmd) return ld
def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') # Simulation settings world = LaunchConfiguration('world') simulator = LaunchConfiguration('simulator') # Declare the launch arguments declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), description='Full path to world file to load') declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', description='The simulator to use (gazebo or gzserver)') # Start Gazebo with plugin providing the robot spawing service start_gazebo_cmd = ExecuteProcess(cmd=[ simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', '-s', 'libgazebo_ros_init.so', world ], output='screen') # Create the launch description and populate ld = LaunchDescription() # Declare the launch options ld.add_action(declare_simulator_cmd) ld.add_action(declare_world_cmd) # Add the actions to start gazebo, robots and simulations ld.add_action(start_gazebo_cmd) return ld
def generate_launch_description(): # launch paths gazebo_worlds_path = get_package_share_directory('gazebo_worlds') # export models path os.environ['GAZEBO_MODEL_PATH'] = os.getenv('GAZEBO_MODEL_PATH') + ":" + gazebo_worlds_path + "/models" world = LaunchConfiguration('world') declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(gazebo_worlds_path, 'worlds', 'house.world'), description='Specify world file name' ) # Create the launch configuration variables use_sim_time = LaunchConfiguration('use_sim_time', default='True') launch_gzserver_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros'), "/launch/gzserver.launch.py"]), launch_arguments={'world': world, 'verbose': 'true'}.items() ) launch_gzclient_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros'), "/launch/gzclient.launch.py"]) ) set_use_sim_time_cmd = ExecuteProcess( cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], output='screen') ld = LaunchDescription() ld.add_action(declare_world_cmd) ld.add_action(launch_gzserver_cmd) ld.add_action(launch_gzclient_cmd) ld.add_action(set_use_sim_time_cmd) return ld
def test_logging_long_messages(): # Set the output format to a "verbose" format that is expected by the executable output os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \ '[{severity}] [{name}]: {message} ({function_name}() at {file_name}:{line_number})' executable = os.path.join(os.getcwd(), 'test_logging_long_messages') if os.name == 'nt': executable += '.exe' ld = LaunchDescription() launch_test = LaunchTestService() action = launch_test.add_fixture_action( ld, ExecuteProcess(cmd=[executable], name='test_logging_long_messages', output='screen')) output_file = os.path.join(os.path.dirname(__file__), 'test_logging_long_messages') launch_test.add_output_test(ld, action, create_output_test_from_file(output_file)) launch_service = LaunchService() launch_service.include_launch_description(ld) return_code = launch_test.run(launch_service) assert return_code == 0, 'Launch failed with exit code %r' % ( return_code, )
def generate_launch_description(): # Must match camera name in URDF file camera_name = 'forward_camera' camera_frame = 'forward_camera_frame' sim_fiducial_path = get_package_share_directory('sim_fiducial') world_path = os.path.join(sim_fiducial_path, 'worlds', 'one_marker.world') map_path = os.path.join(sim_fiducial_path, 'worlds', 'one_marker_map.yaml') sdf_path = os.path.join(sim_fiducial_path, 'sdf', 'forward_camera.sdf') vmap_node_params = { # Publish marker /tf 'psm_publish_tfs': 1, # Load map 'map_load_filename': map_path, # Don't save the map 'map_save_filename': '', } model_params = { # Match orca.urdf (slight positive buoyancy): 'mdl_mass': 9.9, 'mdl_volume': 0.01, 'mdl_fluid_density': 997.0, } pose_filter_node_params = { 'four_dof': True, # Simplify calcs 'predict_accel': False, # No control input, drag or buoyancy in this sim 'predict_accel_control': False, 'predict_accel_drag': False, 'predict_accel_buoyancy': False, 'filter_baro': False, # No depth in this sim 'filter_fcam': True, 'publish_tf': True, # Publish map=>base tf for rviz 'good_pose_dist': 5.0, # Marker is 2-3m away, so make this pretty far 'good_obs_dist': 10.0, # Process noise, similar to /depth noise 'ukf_process_noise': 0.0004, # Turn outlier detection off 'ukf_outlier_distance': -1.0, } pose_filter_node_params.update(model_params) all_entities = [ # Launch Gazebo, loading the world ExecuteProcess( cmd=[ # 'gazebo', 'gzserver', '--verbose', '-s', 'libgazebo_ros_init.so', # Publish /clock '-s', 'libgazebo_ros_factory.so', # Provide injection endpoints world_path ], output='screen'), # Add the sdf to the simulation Node(package='sim_fiducial', node_executable='inject_entity.py', output='screen', arguments=[sdf_path, '0', '0', '0', '0', '0', '0']), # Publish, and possibly build, a map Node(package='fiducial_vlam', node_executable='vmap_main', output='screen', node_name='vmap_node', parameters=[vmap_node_params]), # Localize against the map Node( package='fiducial_vlam', node_executable='vloc_main', output='screen', node_name='vloc_forward', node_namespace=camera_name, parameters=[{ # Localize, don't calibrate 'loc_calibrate_not_loocalize': 0, # 0: OpenCV, 1: GTSAM 'loc_camera_sam_not_cv': 1, 'psl_camera_frame_id': camera_frame, 'psl_publish_tfs': 0, # 0: DICT_4x4_50 (default) # 8: DICT_6X6_250 'loc_aruco_dictionary_id': 8, # Gazebo publishes camera info best-effort 'psl_sub_camera_info_best_effort_not_reliable': 1, # Publish the camera pose 'psl_publish_camera_pose': 1, # Don't publish anything else 'psl_publish_base_pose': 0, 'psl_publish_camera_odom': 0, 'psl_publish_base_odom': 0, # Keep the existing timestamps 'psl_stamp_msgs_with_current_time': 0, }]), # Combine poses and observations into fiducial poses Node(package='orca_filter', node_executable='fp_node', output='screen', node_name='fp_node', node_namespace=camera_name), # Run a pose filter (6dof, 4dof or 1dof) Node(package='orca_filter', node_executable='pose_filter_node', output='screen', node_name='pose_filter_node', parameters=[pose_filter_node_params], remappings=[ ('fcam_fp', '/' + camera_name + '/fp'), ]) ] return LaunchDescription(all_entities)
def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py' ]), ) # NOTE: Load the processed xacro file directly description_location = os.path.join( get_package_share_directory('racecar_description')) xacro_file = os.path.join(description_location, 'urdf', 'racecar.xacro') doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) robot_description_value = doc.toxml() params = {"robot_description": robot_description_value} node_robot_state_publisher = Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[params]) spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=[ '-topic', 'robot_description', '-entity', 'racecar', '-x', '0', '-y', '0', '-z', '0.5' ], output='screen') # NOTE: THIS IS DEPRECATED VERSION OF LOADING load_joint_state_controller = ExecuteProcess(cmd=[ 'ros2', 'control', 'load_start_controller', 'joint_state_broadcaster' ], output='screen') # NOTE: THIS IS DEPRECATED VERSION OF LOADING CONTROLLER controllers = [ 'left_rear_wheel_velocity_controller', 'right_rear_wheel_velocity_controller', 'left_front_wheel_velocity_controller', 'right_front_wheel_velocity_controller', 'left_steering_hinge_position_controller', 'right_steering_hinge_position_controller' ] load_controllers = [ ExecuteProcess(cmd=['ros2', 'control', 'load_start_controller', state], output='screen') for state in controllers ] racecar_controller = os.path.join( get_package_share_directory("racecar_control"), "config", "racecar_control.yaml", ) return LaunchDescription([ RegisterEventHandler(event_handler=OnProcessExit( target_action=spawn_entity, on_exit=[load_joint_state_controller], )), RegisterEventHandler(event_handler=OnProcessExit( target_action=load_joint_state_controller, on_exit=load_controllers, )), gazebo, node_robot_state_publisher, spawn_entity, ])
def generate_launch_description(): # 1 or more drones: drones = ['drone1'] # Starting locations: starting_locations = [ # Face the wall of markers in fiducial.world # ['-2.5', '1.5', '1', '0'], # ['-1.5', '0.5', '2', '0.785'], # ['-0.5', '1.5', '1', '0'], # ['-1.5', '2.5', '.5', '-0.785'] ['-1.5', '1.5', '1', '0'], # Face all 4 directions in f2.world # ['-2.5', '1.5', '1', '0'], # ['-1.5', '0.5', '1', '1.57'], # ['-0.5', '1.5', '1', '3.14'], # ['-1.5', '2.5', '1', '-1.57'] ] fiducial_vlam_path = get_package_share_directory('fiducial_vlam') tello_description_path = get_package_share_directory('tello_description') map_path = os.path.join(fiducial_vlam_path, 'launch', 'fiducial_marker_locations_office.yaml') rviz_config_path = os.path.join(fiducial_vlam_path, 'launch', 'launch_one.rviz') # Global entities entities = [ # Rviz ExecuteProcess(cmd=['rviz2', '-d', rviz_config_path], output='screen'), # # Launch Gazebo, loading tello.world # ExecuteProcess(cmd=[ # 'gazebo', # '--verbose', # '-s', 'libgazebo_ros_init.so', # Publish /clock # '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node # world_path # ], output='screen'), # Load and publish a known map Node( package='fiducial_vlam', node_executable='vmap_node', output='screen', node_name='vmap_node', parameters=[{ 'use_sim_time': False, # Use /clock if available 'publish_tfs': 1, # Publish marker /tf 'marker_length': 0.1778, # Marker length 'marker_map_load_full_filename': map_path, # Load a pre-built map from disk 'make_not_use_map': 0 # Don't save a map to disk }]), # Joystick driver, generates /namespace/joy messages Node( package='joy', node_executable='joy_node', output='screen', node_name='joy_node', parameters=[{ 'use_sim_time': False, # Use /clock if available }]), # Flock controller (basically a joystick multiplexer, also starts/stops missions) Node( package='flock2', node_executable='flock_base', output='screen', node_name='flock_base', parameters=[{ 'use_sim_time': False, # Use /clock if available 'drones': drones }]), # WIP: planner Node( package='flock2', node_executable='planner_node', output='screen', node_name='planner_node', parameters=[{ 'use_sim_time': False, # Use /clock if available 'drones': drones, 'arena_x': -5.0, 'arena_y': -5.0, 'arena_z': 10.0, }]), ] # Per-drone entities for idx, namespace in enumerate(drones): suffix = '_' + str(idx + 1) urdf_path = os.path.join(tello_description_path, 'urdf', 'tello' + suffix + '.urdf') entities.extend([ # # Add a drone to the simulation # Node(package='tello_gazebo', node_executable='inject_entity.py', output='screen', # arguments=[urdf_path]+starting_locations[idx]), # # Publish base_link to camera_link tf # Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', # node_name=namespace+'_tf_pub', arguments=[urdf_path], parameters=[{ # 'use_sim_time': False, # Use /clock if available # }]), # # Localize this drone against the map # Future: need odometry for base_link, not camera_link Node( package='fiducial_vlam', node_executable='vloc_node', output='screen', node_name='vloc_node', node_namespace=namespace, parameters=[{ 'use_sim_time': False, # Use /clock if available 'publish_tfs': 1, # Publish drone and camera /tf 'stamp_msgs_with_current_time': 0, # Use incoming message time, not now() 'base_frame_id': 'base_link' + suffix, 'map_init_pose_z': -0.035, 'camera_frame_id': 'camera_link' + suffix, 'base_odometry_pub_topic': 'filtered_odom', }]), # Odometry filter takes camera pose, generates base_link odom, and publishes map to base_link tf # Node(package='odom_filter', node_executable='filter_node', output='screen', # node_name='filter_node', node_namespace=namespace, parameters=[{ # 'use_sim_time': True, # Use /clock if available # 'map_frame': 'map', # 'base_frame': 'base_link' + suffix # }]), # Driver Node(package='tello_driver', node_executable='tello_driver', output='screen', node_name='tello_driver', node_namespace=namespace), # Drone controller Node( package='flock2', node_executable='drone_base', output='screen', node_name='drone_base', node_namespace=namespace, parameters=[{ 'use_sim_time': False, # Use /clock if available }]), ]) return LaunchDescription(entities)
def generate_launch_description(): # 1 or more drones: # drones = ['drone1', 'drone2'] drones = ['drone1'] tello_gazebo_path = get_package_share_directory('tello_gazebo') tello_description_path = get_package_share_directory('tello_description') world_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial.world') map_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial_map.yaml') # Global entities entities = [ # Launch Gazebo, loading tello.world ExecuteProcess( cmd=[ 'gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', # Publish /clock '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node world_path ], output='screen'), # Load and publish a known map Node( package='fiducial_vlam', node_executable='vmap_node', output='screen', node_name='vmap_node', parameters=[{ 'publish_tfs': 1, # Publish marker /tf 'marker_length': 0.1778, # Marker length 'marker_map_load_full_filename': map_path, # Load a pre-built map from disk 'make_not_use_map': 0 }]), # Don't save a map to disk # Joystick driver, generates /namespace/joy messages # Only controls the first drone Node(package='joy', node_executable='joy_node', output='screen', node_namespace=drones[0]), # Joystick controller, generates /namespace/cmd_vel messages Node(package='tello_driver', node_executable='tello_joy', output='screen', node_namespace=drones[0]), ] # Per-drone entities for idx, namespace in enumerate(drones): suffix = '_' + str(idx + 1) urdf_path = os.path.join(tello_description_path, 'urdf', 'tello' + suffix + '.urdf') entities.extend([ # Add a drone to the simulation Node(package='tello_gazebo', node_executable='inject_entity.py', output='screen', arguments=[urdf_path, '0', str(idx), '1', '0']), # Localize this drone against the map Node(package='fiducial_vlam', node_executable='vloc_node', output='screen', node_name='vloc_node', node_namespace=namespace, parameters=[{ 'publish_tfs': 1, 'base_frame_id': 'base_link' + suffix, 't_camera_base_z': -0.035, 'camera_frame_id': 'camera_link' + suffix }]), ]) return LaunchDescription(entities)
def test_pub_basic(self, launch_service, proc_info, proc_output): params = [('/clitest/topic/pub_basic', False, True), ('/clitest/topic/pub_compatible_qos', True, True), ('/clitest/topic/pub_incompatible_qos', True, False)] for topic, provide_qos, compatible_qos in params: with self.subTest(topic=topic, provide_qos=provide_qos, compatible_qos=compatible_qos): # Check for inconsistent arguments assert provide_qos if not compatible_qos else True received_message_count = 0 expected_minimum_message_count = 1 expected_maximum_message_count = 5 pub_extra_options = [] subscription_qos_profile = 10 if provide_qos: if compatible_qos: # For compatible test, put publisher at very high quality # and subscription at low pub_extra_options = [ '--qos-reliability', 'reliable', '--qos-durability', 'transient_local' ] subscription_qos_profile = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE) else: # For an incompatible example, reverse the quality extremes # and expect no messages to arrive pub_extra_options = [ '--qos-reliability', 'best_effort', '--qos-durability', 'volatile' ] subscription_qos_profile = QoSProfile( depth=10, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL) expected_maximum_message_count = 0 expected_minimum_message_count = 0 future = rclpy.task.Future() def message_callback(msg): """If we receive one message, the test has succeeded.""" nonlocal received_message_count received_message_count += 1 future.set_result(True) subscription = self.node.create_subscription( String, topic, message_callback, subscription_qos_profile) assert subscription try: command_action = ExecuteProcess( cmd=(['ros2', 'topic', 'pub'] + pub_extra_options + [topic, 'std_msgs/String', 'data: hello']), additional_env={'PYTHONUNBUFFERED': '1'}, output='screen') with launch_testing.tools.launch_process( launch_service, command_action, proc_info, proc_output, output_filter=launch_testing_ros.tools. basic_output_filter( filtered_rmw_implementation= get_rmw_implementation_identifier( ))) as command: self.executor.spin_until_future_complete( future, timeout_sec=10) command.wait_for_shutdown(timeout=10) # Check results assert ( received_message_count >= expected_minimum_message_count and received_message_count <= expected_maximum_message_count), \ ('Received {} messages from pub on {},' 'which is not in expected range {}-{}').format( received_message_count, topic, expected_minimum_message_count, expected_maximum_message_count ) finally: # Cleanup self.node.destroy_subscription(subscription)
def generate_launch_description(): # planning_context robot_description_config = xacro.process_file( os.path.join( get_package_share_directory( "moveit_resources_panda_moveit_config"), "config", "panda.urdf.xacro", )) robot_description = {"robot_description": robot_description_config.toxml()} robot_description_semantic_config = load_file( "moveit_resources_panda_moveit_config", "config/panda.srdf") robot_description_semantic = { "robot_description_semantic": robot_description_semantic_config } kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config", "config/kinematics.yaml") robot_description_kinematics = { "robot_description_kinematics": kinematics_yaml } # Planning Functionality ompl_planning_pipeline_config = { "move_group": { "planning_plugin": "ompl_interface/OMPLPlanner", "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } } ompl_planning_yaml = load_yaml("moveit_resources_panda_moveit_config", "config/ompl_planning.yaml") ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) # Trajectory Execution Functionality moveit_simple_controllers_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml") moveit_controllers = { "moveit_simple_controller_manager": moveit_simple_controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } trajectory_execution = { "moveit_manage_controllers": True, "trajectory_execution.allowed_execution_duration_scaling": 1.2, "trajectory_execution.allowed_goal_duration_margin": 0.5, "trajectory_execution.allowed_start_tolerance": 0.01, } planning_scene_monitor_parameters = { "publish_planning_scene": True, "publish_geometry_updates": True, "publish_state_updates": True, "publish_transforms_updates": True, } # Start the actual move_group node/action server run_move_group_node = Node( package="moveit_ros_move_group", executable="move_group", output="screen", parameters=[ robot_description, robot_description_semantic, kinematics_yaml, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, ], ) # RViz rviz_config_file = ( get_package_share_directory("run_ompl_constrained_planning") + "/launch/run_move_group.rviz") rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_config_file], parameters=[ robot_description, robot_description_semantic, ompl_planning_pipeline_config, kinematics_yaml, ], ) # Static TF static_tf = Node( package="tf2_ros", executable="static_transform_publisher", name="static_transform_publisher", output="log", arguments=[ "0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0" ], ) # Publish TF robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher", output="both", parameters=[robot_description], ) # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", "panda_ros_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_controllers_path], output={ "stdout": "screen", "stderr": "screen", }, ) # Load controllers load_controllers = [] for controller in ["panda_arm_controller", "joint_state_controller"]: load_controllers += [ ExecuteProcess( cmd=[ "ros2 run controller_manager spawner.py {}".format( controller) ], shell=True, output="screen", ) ] # Warehouse mongodb server mongodb_server_node = Node( package="warehouse_ros_mongo", executable="mongo_wrapper_ros.py", parameters=[ { "warehouse_port": 33829 }, { "warehouse_host": "localhost" }, { "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection" }, ], output="screen", ) return LaunchDescription([ rviz_node, static_tf, robot_state_publisher, run_move_group_node, ros2_control_node, mongodb_server_node, ] + load_controllers)
def generate_launch_description(): package_name = 'ifm3d_ros2' node_namespace = 'ifm3d' node_name = 'camera' node_exe = 'camera_standalone' # os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \ # "[{%s}] {%s} [{%s}]: {%s}\n({%s}() at {%s}:{%s})" % \ # ("severity", "time", "name", "message", # "function_name", "file_name", "line_number") os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \ "[{%s}] {%s} [{%s}]: {%s}" % ("severity", "time", "name", "message") # XXX: This is a hack, there does not seem to be a nice way (or at least # finding the docs is not obvious) to do this with the ROS2 launch api # # Basically, we are trying to allow for passing through the command line # args to the launch file through to the node executable itself (like ROS # 1). # # My assumption is that either: # 1. This stuff exists somewhere in ROS2 and I don't know about it yet # 2. This stuff will exist in ROS2 soon, so, this will likely get factored # out (hopefully soon) # parameters = [] remaps = [] for arg in sys.argv: if ':=' in arg: split_arg = arg.split(sep=':=', maxsplit=1) assert len(split_arg) == 2 if arg.startswith("ns"): node_namespace = split_arg[1] elif arg.startswith("node"): node_name = split_arg[1] elif arg.startswith("params"): parameters.append(tuple(split_arg)[1]) else: remaps.append(tuple(split_arg)) def add_prefix(tup): assert len(tup) == 2 if node_namespace.startswith("/"): prefix = "%s/%s" % (node_namespace, node_name) else: prefix = "/%s/%s" % (node_namespace, node_name) retval = [None, None] if not tup[0].startswith(prefix): retval[0] = prefix + '/' + tup[0] else: retval[0] = tup[0] if not tup[1].startswith(prefix): retval[1] = prefix + '/' + tup[1] else: retval[1] = tup[1] return tuple(retval) remaps = list(map(add_prefix, remaps)) #------------------------------------------------------------ # Nodes #------------------------------------------------------------ # # Coord frame transform from camera_optical_link to camera_link # tf_node = \ ExecuteProcess( cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', '0', '0', '0', str(-pi/2.), '0', str(-pi/2.), str(node_name + "_link"), str(node_name + "_optical_link")], #output='screen', log_cmd=True ) # # The camera component # camera_node = \ LifecycleNode( package=package_name, node_executable=node_exe, node_namespace=node_namespace, node_name=node_name, output='screen', parameters=parameters, remappings=remaps, log_cmd=True, ) #------------------------------------------------------------ # Events we need to emit to induce state transitions #------------------------------------------------------------ camera_configure_evt = \ EmitEvent( event=ChangeState( lifecycle_node_matcher = \ launch.events.matches_action(camera_node), transition_id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE ) ) camera_activate_evt = \ EmitEvent( event=ChangeState( lifecycle_node_matcher = \ launch.events.matches_action(camera_node), transition_id = lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE ) ) camera_cleanup_evt = \ EmitEvent( event=ChangeState( lifecycle_node_matcher = \ launch.events.matches_action(camera_node), transition_id = lifecycle_msgs.msg.Transition.TRANSITION_CLEANUP ) ) camera_shutdown_evt = EmitEvent(event=launch.events.Shutdown()) #------------------------------------------------------------ # These are the edges of the state machine graph we want to autonomously # manage #------------------------------------------------------------ # # unconfigured -> configuring -> inactive # camera_node_unconfigured_to_inactive_handler = \ RegisterEventHandler( OnStateTransition( target_lifecycle_node = camera_node, start_state = 'configuring', goal_state = 'inactive', entities = [ LogInfo(msg = "Emitting 'TRANSITION_ACTIVATE' event"), camera_activate_evt, ], ) ) # # active -> deactivating -> inactive # camera_node_active_to_inactive_handler = \ RegisterEventHandler( OnStateTransition( target_lifecycle_node = camera_node, start_state = 'deactivating', goal_state = 'inactive', entities = [ LogInfo(msg = "Emitting 'TRANSITION_CLEANUP' event"), camera_cleanup_evt, ], ) ) # # inactive -> cleaningup -> unconfigured # camera_node_inactive_to_unconfigured_handler = \ RegisterEventHandler( OnStateTransition( target_lifecycle_node = camera_node, start_state = 'cleaningup', goal_state = 'unconfigured', entities = [ LogInfo(msg = "Emitting 'TRANSITION_CONFIGURE' event"), camera_configure_evt, ], ) ) # # * -> errorprocessing -> unconfigured # camera_node_errorprocessing_to_unconfigured_handler = \ RegisterEventHandler( OnStateTransition( target_lifecycle_node = camera_node, start_state = 'errorprocessing', goal_state = 'unconfigured', entities = [ LogInfo(msg = "Emitting 'TRANSITION_CONFIGURE' event"), camera_configure_evt, ], ) ) # # * -> shuttingdown -> finalized # camera_node_shuttingdown_to_finalized_handler = \ RegisterEventHandler( OnStateTransition( target_lifecycle_node = camera_node, start_state = 'shuttingdown', goal_state = 'finalized', entities = [ LogInfo(msg = "Emitting 'SHUTDOWN' event"), camera_shutdown_evt, ], ) ) #------------------------------------------------------------ # Now, add all the actions to a launch description and return it #------------------------------------------------------------ ld = LaunchDescription() ld.add_action(camera_node_unconfigured_to_inactive_handler) ld.add_action(camera_node_active_to_inactive_handler) ld.add_action(camera_node_inactive_to_unconfigured_handler) ld.add_action(camera_node_errorprocessing_to_unconfigured_handler) ld.add_action(camera_node_shuttingdown_to_finalized_handler) ld.add_action(tf_node) ld.add_action(camera_node) ld.add_action(camera_configure_evt) return ld
def generate_test_description(): launch_description = LaunchDescription() launch_description.add_action( ExecuteProcess(cmd=[ os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py') ], name='test_dump_params')) launch_description.add_action( ExecuteProcess(cmd=[ os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py'), 'test_dump_params_copy' ], name='test_dump_params_copy')) processes_to_test = [ ExecuteProcess(cmd=[os.getenv('TEST_EXECUTABLE'), '-h'], name='test_dump_params_help', output='screen'), ExecuteProcess(cmd=[os.getenv('TEST_EXECUTABLE')], name='test_dump_params_default', output='screen'), ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'], name='test_dump_params_yaml', output='screen'), ExecuteProcess(cmd=[ os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params' ], name='test_dump_params_markdown', output='screen'), ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'], name='test_dump_params_yaml_verbose', output='screen'), ExecuteProcess(cmd=[ os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params', '-v' ], name='test_dump_params_markdown_verbose', output='screen'), ExecuteProcess(cmd=[ os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n', 'test_dump_params' ], name='test_dump_params_bad_format', output='screen'), ExecuteProcess(cmd=[ os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params,test_dump_params_copy' ], name='test_dump_params_multiple', output='screen'), ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params_error'], name='test_dump_params_error', output='screen') ] for process in processes_to_test: launch_description.add_action(process) launch_description.add_action(launch_testing.actions.ReadyToTest()) return launch_description, {'processes_to_test': processes_to_test}
def generate_launch_description(): description = LaunchDescription() module_toggles = { 'sensors': { 'arducam': False, 'msp_bridge': False, 'vectornav': True }, 'autonomy': {}, 'utilities': { 'image_viewer': False } } description.add_action( LogInfo(msg=["===================================="])) for modules in module_toggles.values(): for module_name, enabled in modules.items(): description.add_action( LogInfo(msg=[ module_name, ': ', "Enabled" if enabled else "Disabled" ])) description.add_action( LogInfo(msg=["===================================="])) description.add_action( DeclareLaunchArgument(name='enable_rosbag_logging', default_value='True', description='Enable rosbag2 logging')) description.add_action( DeclareLaunchArgument(name='enable_ros1_bridge', default_value='True', description='Enable ros1 bridge')) description.add_action( DeclareLaunchArgument(name='enable_rviz', default_value='True', description='Enable rviz2 gui')) # Rosbag2 Logging # Add more topics here topics = ['/acrobat/imu', '/acrobat/camera', '/acrobat/fc_imu'] description.add_action( ExecuteProcess(cmd=['ros2', 'bag', 'record', '--no-discovery'] + topics, name='rosbag2', cwd='/home/martin/rosbag', output='screen', emulate_tty=True, condition=IfCondition(get_toggle('rosbag_logging')))) # Launch ROS1 Bridge description.add_action( Node(package='ros1_bridge', node_executable='dynamic_bridge', node_name='ros1_bridge', output='screen', emulate_tty=True, condition=IfCondition(get_toggle('ros1_bridge')))) # Launch RViz2 description.add_action( Node(package='rviz2', node_executable='rviz2', node_name='rviz', output='screen', emulate_tty=True, condition=IfCondition(get_toggle('rviz')))) sensor_nodes = get_acrobat_sensor_nodes(module_toggles['sensors']) autonomy_nodes = get_acrobat_autonomy_nodes(module_toggles['autonomy']) utility_nodes = get_acrobat_utility_nodes(module_toggles['utilities']) acrobat_node_container = ComposableNodeContainer( node_name='acrobat_container', node_namespace='', package='rclcpp_components', node_executable='component_container', composable_node_descriptions=sensor_nodes + autonomy_nodes + utility_nodes, output='screen', emulate_tty=True, ) description.add_action(acrobat_node_container) return description
def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsType): # planning_context robot_description_config = xacro.process_file( os.path.join( get_package_share_directory( "moveit_resources_panda_moveit_config"), "config", "panda.urdf.xacro", )) robot_description = {"robot_description": robot_description_config.toxml()} robot_description_semantic_config = load_file( "moveit_resources_panda_moveit_config", "config/panda.srdf") robot_description_semantic = { "robot_description_semantic": robot_description_semantic_config } kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config", "config/kinematics.yaml") robot_description_kinematics = { "robot_description_kinematics": kinematics_yaml } # Planning Functionality ompl_planning_pipeline_config = { "move_group": { "planning_plugin": "ompl_interface/OMPLPlanner", "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } } ompl_planning_yaml = load_yaml("moveit_resources_panda_moveit_config", "config/ompl_planning.yaml") ompl_planning_yaml["panda_arm"]["enforce_constrained_state_space"] = True ompl_planning_yaml["panda_arm"][ "projection_evaluator"] = "joints(panda_joint1,panda_joint2)" ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) # Trajectory Execution Functionality moveit_simple_controllers_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml") moveit_controllers = { "moveit_simple_controller_manager": moveit_simple_controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } trajectory_execution = { "moveit_manage_controllers": True, "trajectory_execution.allowed_execution_duration_scaling": 1.5, "trajectory_execution.allowed_goal_duration_margin": 1.0, "trajectory_execution.allowed_start_tolerance": 0.01, } planning_scene_monitor_parameters = { "publish_planning_scene": True, "publish_geometry_updates": True, "publish_state_updates": True, "publish_transforms_updates": True, } # Start the actual move_group node/action server run_move_group_node = Node( package="moveit_ros_move_group", executable="move_group", output="screen", parameters=[ robot_description, robot_description_semantic, kinematics_yaml, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, ], ) # Static TF static_tf = Node( package="tf2_ros", executable="static_transform_publisher", name="static_transform_publisher", output="log", arguments=[ "0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0" ], ) # Publish TF robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher", output="both", parameters=[robot_description], ) # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", "panda_ros_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_controllers_path], output={ "stdout": "screen", "stderr": "screen", }, ) # Load controllers load_controllers = [] for controller in ["panda_arm_controller", "joint_state_controller"]: load_controllers += [ ExecuteProcess( cmd=[ "ros2 run controller_manager spawner.py {}".format( controller) ], shell=True, output="screen", ) ] # test executable ompl_constraint_test = launch_ros.actions.Node( executable=PathJoinSubstitution( [LaunchConfiguration("test_binary_dir"), gtest_name]), parameters=[ robot_description, robot_description_semantic, kinematics_yaml ], output="screen", ) return launch.LaunchDescription([ launch.actions.DeclareLaunchArgument( name="test_binary_dir", description="Binary directory of package " "containing test executables", ), run_move_group_node, static_tf, robot_state_publisher, ros2_control_node, TimerAction(period=2.0, actions=[ompl_constraint_test]), launch_testing.actions.ReadyToTest(), ] + load_controllers), { "run_move_group_node": run_move_group_node, "static_tf": static_tf, "robot_state_publisher": robot_state_publisher, "ros2_control_node": ros2_control_node, "ompl_constraint_test": ompl_constraint_test, }
def generate_launch_description(): # moveit_cpp.yaml is passed by filename for now since it's node specific moveit_cpp_yaml_file_name = ( get_package_share_directory("run_moveit_cpp") + "/config/moveit_cpp.yaml" ) # Component yaml files are grouped in separate namespaces robot_description_config = xacro.process_file( os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", "panda.urdf.xacro", ) ) robot_description = {"robot_description": robot_description_config.toxml()} robot_description_semantic_config = load_file( "moveit_resources_panda_moveit_config", "config/panda.srdf" ) robot_description_semantic = { "robot_description_semantic": robot_description_semantic_config } kinematics_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/kinematics.yaml" ) robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} moveit_simple_controllers_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" ) moveit_controllers = { "moveit_simple_controller_manager": moveit_simple_controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } ompl_planning_pipeline_config = { "planning_pipelines": ["ompl"], "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, }, } ompl_planning_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" ) ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) # MoveItCpp demo executable run_moveit_cpp_node = Node( name="run_moveit_cpp", package="run_moveit_cpp", # TODO(henningkayser): add debug argument # prefix='xterm -e gdb --args', executable="run_moveit_cpp", output="screen", parameters=[ moveit_cpp_yaml_file_name, robot_description, robot_description_semantic, kinematics_yaml, ompl_planning_pipeline_config, moveit_controllers, ], ) # RViz rviz_config_file = ( get_package_share_directory("run_moveit_cpp") + "/launch/run_moveit_cpp.rviz" ) rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_config_file], parameters=[robot_description, robot_description_semantic], ) # Static TF static_tf = Node( package="tf2_ros", executable="static_transform_publisher", name="static_transform_publisher", output="log", arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], ) # Publish TF robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher", output="both", parameters=[robot_description], ) # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", "panda_ros_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_controllers_path], output={ "stdout": "screen", "stderr": "screen", }, ) # Load controllers load_controllers = [] for controller in [ "panda_arm_controller", "panda_hand_controller", "joint_state_controller", ]: load_controllers += [ ExecuteProcess( cmd=["ros2 run controller_manager spawner.py {}".format(controller)], shell=True, output="screen", ) ] return LaunchDescription( [ static_tf, robot_state_publisher, rviz_node, run_moveit_cpp_node, ros2_control_node, ] + load_controllers )
def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') filter_mask_file = os.getenv('TEST_MASK') world = os.getenv('TEST_WORLD') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', os.getenv('BT_NAVIGATOR_XML')) bringup_dir = get_package_share_directory('nav2_bringup') script_dir = os.path.dirname(os.path.realpath(__file__)) params_file = os.path.join(script_dir, 'keepout_params.yaml') # Replace the `use_astar` setting on the params file param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, 'yaml_filename': filter_mask_file } configured_params = RewrittenYaml(source_file=params_file, root_key='', param_rewrites=param_substitutions, convert_types=True) return LaunchDescription([ SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), # Launch gazebo server for simulation ExecuteProcess(cmd=[ 'gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world ], output='screen'), # TODO(orduno) Launch the robot state publisher instead # using a local copy of TB3 urdf file Node(package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=[ '0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link' ]), Node( package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), Node(package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_filters', output='screen', parameters=[{ 'node_names': ['filter_mask_server', 'costmap_filter_info_server'] }, { 'autostart': True }]), # Nodes required for Costmap Filters configuration Node(package='nav2_map_server', executable='map_server', name='filter_mask_server', output='screen', parameters=[configured_params]), Node(package='nav2_map_server', executable='costmap_filter_info_server', name='costmap_filter_info_server', output='screen', parameters=[configured_params]), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), launch_arguments={ 'namespace': '', 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True' }.items()), ])
def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), ) gazebo_ros2_control_demos_path = os.path.join( get_package_share_directory('gazebo_ros2_control_demos')) xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', 'test_cart_effort.xacro.urdf') doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) params = {'robot_description': doc.toxml()} node_robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[params] ) spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', arguments=['-topic', 'robot_description', '-entity', 'cartpole'], output='screen') load_joint_state_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_state_broadcaster'], output='screen' ) load_joint_trajectory_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'effort_controllers'], output='screen' ) return LaunchDescription([ RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( <<<<<<< HEAD target_action=load_joint_state_broadcaster, on_exit=[load_effort_controller], ======= target_action=load_joint_state_controller, on_exit=[load_joint_trajectory_controller], >>>>>>> 895ade63dc26f2cbecef0396d82eeaf4d1493d08 ) ), gazebo, node_robot_state_publisher, spawn_entity, ])
def test_echo_basic(self, launch_service, proc_info, proc_output): params = [ ('/clitest/topic/echo_basic', False, True, False), ('/clitest/topic/echo_compatible_qos', True, True, False), ('/clitest/topic/echo_incompatible_qos', True, False, False), ('/clitest/topic/echo_message_lost', False, True, True), ] for topic, provide_qos, compatible_qos, message_lost in params: with self.subTest(topic=topic, provide_qos=provide_qos, compatible_qos=compatible_qos): # Check for inconsistent arguments assert provide_qos if not compatible_qos else True echo_extra_options = [] publisher_qos_profile = 10 if provide_qos: if compatible_qos: # For compatible test, put publisher at very high quality # and subscription at low echo_extra_options = [ '--qos-reliability', 'best_effort', '--qos-durability', 'volatile' ] publisher_qos_profile = QoSProfile( depth=10, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL) else: # For an incompatible example, reverse the quality extremes # and expect no messages to arrive echo_extra_options = [ '--qos-reliability', 'reliable', '--qos-durability', 'transient_local' ] publisher_qos_profile = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE) if message_lost: echo_extra_options.append('--lost-messages') publisher = self.node.create_publisher(String, topic, publisher_qos_profile) assert publisher def publish_message(): publisher.publish(String(data='hello')) publish_timer = self.node.create_timer(0.5, publish_message) try: command_action = ExecuteProcess( cmd=(['ros2', 'topic', 'echo'] + echo_extra_options + [topic, 'std_msgs/String']), additional_env={'PYTHONUNBUFFERED': '1'}, output='screen') with launch_testing.tools.launch_process( launch_service, command_action, proc_info, proc_output, output_filter=launch_testing_ros.tools. basic_output_filter( filtered_rmw_implementation= get_rmw_implementation_identifier( ))) as command: # The future won't complete - we will hit the timeout self.executor.spin_until_future_complete( rclpy.task.Future(), timeout_sec=5) command.wait_for_shutdown(timeout=10) # Check results if compatible_qos: # TODO(ivanpauno): remove special case when FastRTPS implements the feature # https://github.com/ros2/rmw_fastrtps/issues/395 assert command.output, 'Echo CLI printed no output' if message_lost and 'rmw_fastrtps' in get_rmw_implementation_identifier( ): assert 'does not support reporting lost messages' in command.output assert get_rmw_implementation_identifier( ) in command.output return assert 'data: hello' in command.output.splitlines(), ( 'Echo CLI did not print expected message') else: # TODO(mm318): remove special case for FastRTPS when # https://github.com/ros2/rmw_fastrtps/issues/356 is resolved if 'rmw_fastrtps' in get_rmw_implementation_identifier( ): assert not command.output, ( 'Echo CLI should not have received anything with incompatible QoS' ) else: assert command.output, ( 'Echo CLI did not print incompatible QoS warning' ) assert ( 'New publisher discovered on this topic, offering incompatible' ' QoS.' in command.output ), ('Echo CLI did not print expected incompatible QoS warning' ) finally: # Cleanup self.node.destroy_timer(publish_timer) self.node.destroy_publisher(publisher)
def generate_launch_description(): urdf1 = os.path.join(get_package_share_directory('tello_description'), 'urdf', 'tello_1.urdf') urdf2 = os.path.join(get_package_share_directory('tello_description'), 'urdf', 'tello_2.urdf') dr1_ns = 'drone1' dr2_ns = 'drone2' dr1_params = [{ 'drone_ip': '192.168.86.206', 'command_port': 11001, 'drone_port': 12001, 'data_port': 13001, 'video_port': 14001 }] dr2_params = [{ 'drone_ip': '192.168.86.212', 'command_port': 11002, 'drone_port': 12002, 'data_port': 13002, 'video_port': 14002 }] base_params = [{'drones': [dr1_ns, dr2_ns]}] filter1_params = [{'map_frame': 'map', 'base_frame': 'base1'}] filter2_params = [{'map_frame': 'map', 'base_frame': 'base2'}] return LaunchDescription([ # Rviz ExecuteProcess( cmd=['rviz2', '-d', 'install/flock2/share/flock2/launch/two.rviz'], output='screen'), # Publish N sets of static transforms Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', arguments=[urdf1]), Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', arguments=[urdf2]), # N drivers Node(package='tello_driver', node_executable='tello_driver_main', output='screen', node_name='driver1', node_namespace=dr1_ns, parameters=dr1_params), Node(package='tello_driver', node_executable='tello_driver_main', output='screen', node_name='driver2', node_namespace=dr2_ns, parameters=dr2_params), # Joystick Node(package='joy', node_executable='joy_node', output='screen'), # Flock controller Node(package='flock2', node_executable='flock_base', output='screen', node_name='flock_base', parameters=base_params), # N drone controllers Node(package='flock2', node_executable='drone_base', output='screen', node_name='base1', node_namespace=dr1_ns), Node(package='flock2', node_executable='drone_base', output='screen', node_name='base2', node_namespace=dr2_ns), # Mapper Node(package='fiducial_vlam', node_executable='vmap_main', output='screen'), # N visual localizers Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', node_name='vloc1', node_namespace=dr1_ns), Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', node_name='vloc2', node_namespace=dr2_ns), ])
def generate_launch_description(): # Get parameters for the Servo node servo_yaml = load_yaml("moveit_servo", "config/panda_simulated_config.yaml") servo_params = {"moveit_servo": servo_yaml} # Get URDF and SRDF robot_description_config = xacro.process_file( os.path.join( get_package_share_directory( "moveit_resources_panda_moveit_config"), "config", "panda.urdf.xacro", )) robot_description = {"robot_description": robot_description_config.toxml()} robot_description_semantic_config = load_file( "moveit_resources_panda_moveit_config", "config/panda.srdf") robot_description_semantic = { "robot_description_semantic": robot_description_semantic_config } # RViz rviz_config_file = (get_package_share_directory("moveit2_tutorials") + "/config/demo_rviz_config.rviz") rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_config_file], parameters=[robot_description, robot_description_semantic], ) # ros2_control using FakeSystem as hardware ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[ robot_description, os.path.join( get_package_share_directory( "moveit_resources_panda_moveit_config"), "config", "panda_ros_controllers.yaml", ), ], output={ "stdout": "screen", "stderr": "screen", }, ) # Load controllers load_controllers = [] for controller in ["panda_arm_controller", "joint_state_broadcaster"]: load_controllers += [ ExecuteProcess( cmd=[ "ros2 run controller_manager spawner {}".format(controller) ], shell=True, output="screen", ) ] # Launch as much as possible in components container = ComposableNodeContainer( name="moveit_servo_demo_container", namespace="/", package="rclcpp_components", executable="component_container", composable_node_descriptions=[ ComposableNode( package="robot_state_publisher", plugin="robot_state_publisher::RobotStatePublisher", name="robot_state_publisher", parameters=[robot_description], ), ComposableNode( package="tf2_ros", plugin="tf2_ros::StaticTransformBroadcasterNode", name="static_tf2_broadcaster", parameters=[{ "/child_frame_id": "panda_link0", "/frame_id": "world" }], ), ComposableNode( package="moveit_servo", plugin="moveit_servo::ServoServer", name="servo_server", parameters=[ servo_params, robot_description, robot_description_semantic, ], extra_arguments=[{ "use_intra_process_comms": True }], ), ], output="screen", ) return LaunchDescription([rviz_node, ros2_control_node, container] + load_controllers)
def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') urdf = os.getenv('TEST_URDF') sdf = os.getenv('TEST_SDF') bt_xml_file = os.path.join( get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', os.getenv('BT_NAVIGATOR_XML')) bringup_dir = get_package_share_directory('nav2_bringup') robot1_params_file = os.path.join( bringup_dir, # noqa: F841 'params/nav2_multirobot_params_1.yaml') robot2_params_file = os.path.join( bringup_dir, # noqa: F841 'params/nav2_multirobot_params_2.yaml') # Names and poses of the robots robots = [{ 'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01 }, { 'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01 }] # Launch Gazebo server for simulation start_gazebo_cmd = ExecuteProcess(cmd=[ 'gzserver', '-s', 'libgazebo_ros_factory.so', '--minimal_comms', world ], output='screen') # Define commands for spawing the robots into Gazebo spawn_robots_cmds = [] for robot in robots: spawn_robots_cmds.append( Node(package='nav2_gazebo_spawner', executable='nav2_gazebo_spawner', output='screen', arguments=[ '--robot_name', TextSubstitution(text=robot['name']), '--robot_namespace', TextSubstitution(text=robot['name']), '--sdf', TextSubstitution(text=sdf), '-x', TextSubstitution(text=str(robot['x_pose'])), '-y', TextSubstitution(text=str(robot['y_pose'])), '-z', TextSubstitution(text=str(robot['z_pose'])) ])) # Define commands for launching the robot state publishers robot_state_pubs_cmds = [] for robot in robots: robot_state_pubs_cmds.append( Node(package='robot_state_publisher', executable='robot_state_publisher', namespace=TextSubstitution(text=robot['name']), output='screen', parameters=[{ 'use_sim_time': 'True' }], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], arguments=[urdf])) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: params_file = eval(f"{robot['name']}_params_file") group = GroupAction([ # Instances use the robot's name for namespace PushRosNamespace(robot['name']), IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), launch_arguments={ 'namespace': robot['name'], 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, 'bt_xml_file': bt_xml_file, 'autostart': 'True', 'use_remappings': 'True' }.items()) ]) nav_instances_cmds.append(group) ld = LaunchDescription() ld.add_action( SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), ) ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), ) ld.add_action(start_gazebo_cmd) for spawn_robot in spawn_robots_cmds: ld.add_action(spawn_robot) for state_pub in robot_state_pubs_cmds: ld.add_action(state_pub) for nav_instance in nav_instances_cmds: ld.add_action(nav_instance) return ld
def generate_launch_description(): # Get the launch directory social_bringup_dir = get_package_share_directory('social_nav2_bringup') nav_bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(nav_bringup_dir, 'launch') # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') bt_xml_file = LaunchConfiguration('bt_xml_file') autostart = LaunchConfiguration('autostart') use_remappings = LaunchConfiguration('use_remappings') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') use_simulator = LaunchConfiguration('use_simulator') use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') world = LaunchConfiguration('world') # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative # https://github.com/ros/geometry2/issues/32 # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='', description='Top-level namespace') declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', description='Whether to apply a namespace to the navigation stack') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(social_bringup_dir, 'maps', 'turtlebot3_house.yaml'), description='Full path to map file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true') declare_use_remappings_cmd = DeclareLaunchArgument( 'use_remappings', default_value='false', description='Arguments to pass to all nodes launched by the file') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(social_bringup_dir, 'params', 'nav2_params.yaml'), description= 'Full path to the ROS2 parameters file to use for all launched nodes') declare_bt_xml_cmd = DeclareLaunchArgument( 'bt_xml_file', default_value=os.path.join( get_package_share_directory('social_nav2_bringup'), 'behavior_trees', 'follow_point.xml'), description='Full path to the behavior tree xml file to use') declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(social_bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') declare_use_simulator_cmd = DeclareLaunchArgument( 'use_simulator', default_value='True', description='Whether to start the simulator') declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', description='Whether to start the robot state publisher') declare_use_rviz_cmd = DeclareLaunchArgument( 'use_rviz', default_value='True', description='Whether to start RVIZ') declare_simulator_cmd = DeclareLaunchArgument( 'headless', default_value='False', description='Whether to execute gzclient)') declare_world_cmd = DeclareLaunchArgument( 'world', # TODO(orduno) Switch back once ROS argument passing has been fixed upstream # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), # 'worlds/turtlebot3_worlds/waffle.model'), default_value=os.path.join(social_bringup_dir, 'worlds', 'waffle_house.model'), description='Full path to world model file to load') # Specify the actions start_gazebo_server_cmd = ExecuteProcess( condition=IfCondition(use_simulator), cmd=[ 'gzserver', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world ], cwd=[launch_dir], output='screen') start_gazebo_client_cmd = ExecuteProcess(condition=IfCondition( PythonExpression([use_simulator, ' and not ', headless])), cmd=['gzclient'], cwd=[launch_dir], output='screen') urdf = os.path.join(social_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( condition=IfCondition(use_robot_state_pub), package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', namespace=namespace, output='screen', parameters=[{ 'use_sim_time': use_sim_time }], remappings=remappings, arguments=[urdf]) start_rviz_cmd = Node( condition=IfCondition(use_rviz), package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', rviz_config_file], output='screen', ) exit_event_handler = RegisterEventHandler( event_handler=OnProcessExit(target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown( reason='rviz exited')))) agent_spawner_cmd = Node(package='pedsim_gazebo_plugin', executable='spawn_pedsim_agents.py', name='spawn_pedsim_agents', output='screen') # Create the launch description and populate ld = LaunchDescription() # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_bt_xml_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_remappings_cmd) ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_simulator_cmd) ld.add_action(declare_use_robot_state_pub_cmd) ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) ld.add_action(declare_world_cmd) # Add any conditioned actions ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) ld.add_action(agent_spawner_cmd) ld.add_action(start_rviz_cmd) ld.add_action(exit_event_handler) # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) return ld
def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch') # Create the launch configuration variables namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') bt_xml_file = LaunchConfiguration('bt_xml_file') autostart = LaunchConfiguration('autostart') use_remappings = LaunchConfiguration('use_remappings') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') use_simulator = LaunchConfiguration('use_simulator') use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') use_rviz = LaunchConfiguration('use_rviz') simulator = LaunchConfiguration('simulator') world = LaunchConfiguration('world') # TODO(orduno) Remove once `PushNodeRemapping` is resolved # https://github.com/ros2/launch_ros/issues/56 remappings = [((namespace, '/tf'), '/tf'), ((namespace, '/tf_static'), '/tf_static'), ('/scan', 'scan'), ('/tf', 'tf'), ('/tf_static', 'tf_static'), ('/cmd_vel', 'cmd_vel'), ('/map', 'map')] # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='', description='Top-level namespace') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), description='Full path to map file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description= 'Full path to the ROS2 parameters file to use for all launched nodes') declare_bt_xml_cmd = DeclareLaunchArgument( 'bt_xml_file', default_value=os.path.join( get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), description='Full path to the behavior tree xml file to use') declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='false', description='Automatically startup the nav2 stack') declare_use_remappings_cmd = DeclareLaunchArgument( 'use_remappings', default_value='false', description='Arguments to pass to all nodes launched by the file') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') declare_use_simulator_cmd = DeclareLaunchArgument( 'use_simulator', default_value='True', description='Whether to start the simulator') declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', description='Whether to start the robot state publisher') declare_use_rviz_cmd = DeclareLaunchArgument( 'use_rviz', default_value='True', description='Whether to start RVIZ') declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', description='The simulator to use (gazebo or gzserver)') declare_world_cmd = DeclareLaunchArgument( 'world', # TODO(orduno) Switch back once ROS argument passing has been fixed upstream # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), # 'worlds/turtlebot3_worlds/waffle.model'), default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), description='Full path to world model file to load') # Specify the actions start_gazebo_cmd = ExecuteProcess( condition=IfCondition(use_simulator), cmd=[simulator, '-s', 'libgazebo_ros_init.so', world], cwd=[launch_dir], output='screen') urdf = os.path.join(get_package_share_directory('turtlebot3_description'), 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( condition=IfCondition(use_robot_state_pub), package='robot_state_publisher', node_executable='robot_state_publisher', node_name='robot_state_publisher', output='screen', parameters=[{ 'use_sim_time': use_sim_time }], use_remappings=IfCondition(use_remappings), remappings=remappings, arguments=[urdf]) start_rviz_cmd = Node(condition=IfCondition(use_rviz), package='rviz2', node_executable='rviz2', node_name='rviz2', arguments=['-d', rviz_config_file], output='screen', use_remappings=IfCondition(use_remappings), remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static'), ('goal_pose', 'goal_pose'), ('/clicked_point', 'clicked_point'), ('/initialpose', 'initialpose')]) exit_event_handler = RegisterEventHandler( event_handler=OnProcessExit(target_action=start_rviz_cmd, on_exit=EmitEvent(event=Shutdown( reason='rviz exited')))) bringup_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource( os.path.join(launch_dir, 'nav2_bringup_launch.py')), launch_arguments={ 'namespace': namespace, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'params_file': params_file, 'bt_xml_file': bt_xml_file, 'autostart': autostart, 'use_remappings': use_remappings }.items()) # Create the launch description and populate ld = LaunchDescription() # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_bt_xml_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_remappings_cmd) ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_simulator_cmd) ld.add_action(declare_use_robot_state_pub_cmd) ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) ld.add_action(declare_world_cmd) # Add any conditioned actions ld.add_action(start_gazebo_cmd) ld.add_action(start_rviz_cmd) # Add other nodes and processes we need ld.add_action(exit_event_handler) # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) ld.add_action(bringup_cmd) return ld
def generate_test_description(rmw_implementation): path_to_fixtures = os.path.join(os.path.dirname(__file__), 'fixtures') additional_env = { 'RMW_IMPLEMENTATION': rmw_implementation, 'PYTHONUNBUFFERED': '1' } path_to_talker_node_script = os.path.join(path_to_fixtures, 'talker_node.py') path_to_listener_node_script = os.path.join(path_to_fixtures, 'listener_node.py') hidden_talker_node_action = Node( executable=sys.executable, arguments=[path_to_talker_node_script], remappings=[('chatter', '_hidden_chatter')], additional_env=additional_env ) talker_node_action = Node( executable=sys.executable, arguments=[path_to_talker_node_script], additional_env=additional_env ) listener_node_action = Node( executable=sys.executable, arguments=[path_to_listener_node_script], remappings=[('chatter', 'chit_chatter')], additional_env=additional_env ) path_to_repeater_node_script = os.path.join(path_to_fixtures, 'repeater_node.py') array_repeater_node_action = Node( executable=sys.executable, arguments=[path_to_repeater_node_script, 'test_msgs/msg/Arrays'], name='array_repeater', remappings=[('/array_repeater/output', '/arrays')], output='screen', additional_env=additional_env ) defaults_repeater_node_action = Node( executable=sys.executable, arguments=[path_to_repeater_node_script, 'test_msgs/msg/Defaults'], name='defaults_repeater', remappings=[('/defaults_repeater/output', '/defaults')], additional_env=additional_env, ) bounded_sequences_repeater_node_action = Node( executable=sys.executable, arguments=[ path_to_repeater_node_script, 'test_msgs/msg/BoundedSequences' ], name='bounded_sequences_repeater', remappings=[('/bounded_sequences_repeater/output', '/bounded_sequences')], additional_env=additional_env ) unbounded_sequences_repeater_node_action = Node( executable=sys.executable, arguments=[ path_to_repeater_node_script, 'test_msgs/msg/UnboundedSequences' ], name='unbounded_sequences_repeater', remappings=[('/unbounded_sequences_repeater/output', '/unbounded_sequences')], additional_env=additional_env ) path_to_controller_node_script = os.path.join(path_to_fixtures, 'controller_node.py') cmd_vel_controller_node_action = Node( executable=sys.executable, arguments=[path_to_controller_node_script], additional_env=additional_env ) return LaunchDescription([ # Always restart daemon to isolate tests. ExecuteProcess( cmd=['ros2', 'daemon', 'stop'], name='daemon-stop', on_exit=[ ExecuteProcess( cmd=['ros2', 'daemon', 'start'], name='daemon-start', on_exit=[ # Add talker/listener pair. talker_node_action, listener_node_action, # Add hidden talker. hidden_talker_node_action, # Add topic repeaters. array_repeater_node_action, defaults_repeater_node_action, bounded_sequences_repeater_node_action, unbounded_sequences_repeater_node_action, # Add stamped data publisher. cmd_vel_controller_node_action, launch_testing.actions.ReadyToTest() ], additional_env=additional_env ) ] ), ]), locals()
def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') # Names and poses of the robots robots = [{ 'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01 }, { 'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01 }] # Simulation settings world = LaunchConfiguration('world') simulator = LaunchConfiguration('simulator') # On this example all robots are launched with the same settings map_yaml_file = LaunchConfiguration('map') params_file = LaunchConfiguration('params_file') bt_xml_file = LaunchConfiguration('bt_xml_file') rviz_config_file = LaunchConfiguration('rviz_config') log_settings = LaunchConfiguration('log_settings', default='true') # Declare the launch arguments declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), description='Full path to world file to load') declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', description='The simulator to use (gazebo or gzserver)') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), description='Full path to map file to load') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description= 'Full path to the ROS2 parameters file to use for all launched nodes') declare_bt_xml_cmd = DeclareLaunchArgument( 'bt_xml_file', default_value=os.path.join( get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), description='Full path to the behavior tree xml file to use') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), description='Full path to the RVIZ config file to use') # Start Gazebo with plugin providing the robot spawing service start_gazebo_cmd = ExecuteProcess( cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world], output='screen') # Define commands for spawing the robots into Gazebo spawn_robots_cmds = [] for robot in robots: spawn_robots_cmds.append( IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'spawn_robot_launch.py')), launch_arguments={ 'x_pose': TextSubstitution(text=str(robot['x_pose'])), 'y_pose': TextSubstitution(text=str(robot['y_pose'])), 'z_pose': TextSubstitution(text=str(robot['z_pose'])), 'robot_name': robot['name'], 'turtlebot_type': TextSubstitution(text='waffle') }.items())) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: namespaced_rviz_config_file = ReplaceString( source_file=rviz_config_file, replacements={'<robot_namespace>': ('/' + robot['name'])}) group = GroupAction([ # TODO(orduno) # Each `action.Node` within the `localization` and `navigation` launch # files has two versions, one with the required remaps and another without. # The `use_remappings` flag specifies which runs. # A better mechanism would be to have a PushNodeRemapping() action: # https://github.com/ros2/launch_ros/issues/56 # For more on why we're remapping topics, see the note below # PushNodeRemapping(remappings) # Instances use the robot's name for namespace PushRosNamespace(robot['name']), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch', 'nav2_tb3_simulation_launch.py')), launch_arguments={ # TODO(orduno) might not be necessary to pass the robot name 'namespace': robot['name'], 'map_yaml_file': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, 'bt_xml_file': bt_xml_file, 'autostart': 'False', 'use_remappings': 'True', 'rviz_config_file': namespaced_rviz_config_file, 'use_simulator': 'False' }.items()), LogInfo(condition=IfCondition(log_settings), msg=['Launching ', robot['name']]), LogInfo(condition=IfCondition(log_settings), msg=[robot['name'], ' map yaml: ', map_yaml_file]), LogInfo(condition=IfCondition(log_settings), msg=[robot['name'], ' params yaml: ', params_file]), LogInfo(condition=IfCondition(log_settings), msg=[robot['name'], ' behavior tree xml: ', bt_xml_file]), LogInfo(condition=IfCondition(log_settings), msg=[ robot['name'], ' rviz config file: ', namespaced_rviz_config_file ]) ]) nav_instances_cmds.append(group) # A note on the `remappings` variable defined above and the fact it's passed as a node arg. # A few topics have fully qualified names (have a leading '/'), these need to be remapped # to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative # for multi-robot transforms: # https://github.com/ros/geometry2/issues/32 # https://github.com/ros/robot_state_publisher/pull/30 # Create the launch description and populate ld = LaunchDescription() # Declare the launch options ld.add_action(declare_simulator_cmd) ld.add_action(declare_world_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_bt_xml_cmd) ld.add_action(declare_rviz_config_file_cmd) # Add the actions to start gazebo, robots and simulations ld.add_action(start_gazebo_cmd) for spawn_robot_cmd in spawn_robots_cmds: ld.add_action(spawn_robot_cmd) for simulation_instance_cmd in nav_instances_cmds: ld.add_action(simulation_instance_cmd) return ld
def generate_launch_description(): model, plugin, media = GazeboRosPaths.get_paths() if 'GAZEBO_MODEL_PATH' in environ: model += ':' + environ['GAZEBO_MODEL_PATH'] if 'GAZEBO_PLUGIN_PATH' in environ: plugin += ':' + environ['GAZEBO_PLUGIN_PATH'] if 'GAZEBO_RESOURCE_PATH' in environ: media += ':' + environ['GAZEBO_RESOURCE_PATH'] env = { 'GAZEBO_MODEL_PATH': model, 'GAZEBO_PLUGIN_PATH': plugin, 'GAZEBO_RESOURCE_PATH': media } return LaunchDescription([ DeclareLaunchArgument('world', default_value='', description='Specify world file name'), DeclareLaunchArgument( 'version', default_value='false', description='Set "true" to output version information.'), DeclareLaunchArgument( 'verbose', default_value='false', description='Set "true" to increase messages written to terminal.' ), DeclareLaunchArgument( 'help', default_value='false', description='Set "true" to produce gzserver help message.'), DeclareLaunchArgument( 'pause', default_value='false', description='Set "true" to start the server in a paused state.'), DeclareLaunchArgument( 'physics', default_value='', description='Specify a physics engine (ode|bullet|dart|simbody).'), DeclareLaunchArgument('play', default_value='', description='Play the specified log file.'), DeclareLaunchArgument('record', default_value='false', description='Set "true" to record state data.'), DeclareLaunchArgument( 'record_encoding', default_value='', description='Specify compression encoding format for log data ' '(zlib|bz2|txt).'), DeclareLaunchArgument( 'record_path', default_value='', description='Absolute path in which to store state data.'), DeclareLaunchArgument( 'record_period', default_value='', description='Specify recording period (seconds).'), DeclareLaunchArgument('record_filter', default_value='', description='Specify recording filter ' '(supports wildcard and regular expression).'), DeclareLaunchArgument( 'seed', default_value='', description='Start with a given a random number seed.'), DeclareLaunchArgument( 'iters', default_value='', description='Specify number of iterations to simulate.'), DeclareLaunchArgument( 'minimal_comms', default_value='false', description='Set "true" to reduce TCP/IP traffic output.'), DeclareLaunchArgument( 'profile', default_value='', description='Specify physics preset profile name from the options ' 'in the world file.'), DeclareLaunchArgument( 'extra_gazebo_args', default_value='', description='Extra arguments to be passed to Gazebo'), # Specific to gazebo_ros DeclareLaunchArgument( 'gdb', default_value='false', description='Set "true" to run gzserver with gdb'), DeclareLaunchArgument( 'valgrind', default_value='false', description='Set "true" to run gzserver with valgrind'), DeclareLaunchArgument( 'init', default_value='true', description='Set "false" not to load "libgazebo_ros_init.so"'), DeclareLaunchArgument( 'factory', default_value='true', description='Set "false" not to load "libgazebo_ros_factory.so"'), # Wait for (https://github.com/ros-simulation/gazebo_ros_pkgs/pull/941) # DeclareLaunchArgument('force_system', default_value='true', # description='Set "false" not to load \ # "libgazebo_ros_force_system.so"'), ExecuteProcess( cmd=[ 'gzserver', # Pass through arguments to gzserver LaunchConfiguration('world'), ' ', _boolean_command('version'), ' ', _boolean_command('verbose'), ' ', _boolean_command('help'), ' ', _boolean_command('pause'), ' ', _arg_command('physics'), ' ', LaunchConfiguration('physics'), ' ', _arg_command('play'), ' ', LaunchConfiguration('play'), ' ', _boolean_command('record'), ' ', _arg_command('record_encoding'), ' ', LaunchConfiguration('record_encoding'), ' ', _arg_command('record_path'), ' ', LaunchConfiguration('record_path'), ' ', _arg_command('record_period'), ' ', LaunchConfiguration('record_period'), ' ', _arg_command('record_filter'), ' ', LaunchConfiguration('record_filter'), ' ', _arg_command('seed'), ' ', LaunchConfiguration('seed'), ' ', _arg_command('iters'), ' ', LaunchConfiguration('iters'), ' ', _boolean_command('minimal_comms'), _plugin_command('init'), ' ', _plugin_command('factory'), ' ', # Wait for (https://github.com/ros-simulation/gazebo_ros_pkgs/pull/941) # _plugin_command('force_system'), ' ', _arg_command('profile'), ' ', LaunchConfiguration('profile'), LaunchConfiguration('extra_gazebo_args'), ], output='screen', additional_env=env, shell=True, prefix=PythonExpression([ '"gdb -ex run --args" if "true" == "', LaunchConfiguration('gdb'), '" else "valgrind" if "true" == "', LaunchConfiguration('valgrind'), '" else ""' ]), ) ])
def generate_servo_test_description( *args, gtest_name: SomeSubstitutionsType, start_position_path: SomeSubstitutionsType = ""): # Get parameters using the demo config file servo_yaml = load_yaml("moveit_servo", "config/panda_simulated_config.yaml") servo_params = {"moveit_servo": servo_yaml} # Get URDF and SRDF if start_position_path: initial_positions_file = os.path.join(os.path.dirname(__file__), start_position_path) robot_description_config = xacro.process_file( os.path.join( get_package_share_directory( "moveit_resources_panda_moveit_config"), "config", "panda.urdf.xacro", ), mappings={"initial_positions_file": initial_positions_file}, ) else: robot_description_config = xacro.process_file( os.path.join( get_package_share_directory( "moveit_resources_panda_moveit_config"), "config", "panda.urdf.xacro", )) robot_description = {"robot_description": robot_description_config.toxml()} robot_description_semantic_config = load_file( "moveit_resources_panda_moveit_config", "config/panda.srdf") robot_description_semantic = { "robot_description_semantic": robot_description_semantic_config } # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", "panda_ros_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, ros2_controllers_path], output={ "stdout": "screen", "stderr": "screen", }, ) # Load controllers load_controllers = [] for controller in ["panda_arm_controller", "joint_state_controller"]: load_controllers += [ ExecuteProcess( cmd=[ "ros2 run controller_manager spawner.py {}".format( controller) ], shell=True, output="screen", ) ] # Component nodes for tf and Servo test_container = ComposableNodeContainer( name="test_servo_integration_container", namespace="/", package="rclcpp_components", executable="component_container", composable_node_descriptions=[ ComposableNode( package="robot_state_publisher", plugin="robot_state_publisher::RobotStatePublisher", name="robot_state_publisher", parameters=[robot_description], ), ComposableNode( package="tf2_ros", plugin="tf2_ros::StaticTransformBroadcasterNode", name="static_tf2_broadcaster", parameters=[{ "/child_frame_id": "panda_link0", "/frame_id": "world" }], ), ComposableNode( package="moveit_servo", plugin="moveit_servo::ServoServer", name="servo_server", parameters=[ servo_params, robot_description, robot_description_semantic, ], extra_arguments=[{ "use_intra_process_comm": True }], ), ], output="screen", ) # Unknown how to set timeout # https://github.com/ros2/launch/issues/466 servo_gtest = launch_ros.actions.Node( executable=PathJoinSubstitution( [LaunchConfiguration("test_binary_dir"), gtest_name]), parameters=[servo_params], output="screen", ) return launch.LaunchDescription([ launch.actions.DeclareLaunchArgument( name="test_binary_dir", description="Binary directory of package " "containing test executables", ), ros2_control_node, test_container, TimerAction(period=2.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] + load_controllers), { "test_container": test_container, "servo_gtest": servo_gtest, "ros2_control_node": ros2_control_node, }
def generate_launch_description(): model, plugin, media = GazeboRosPaths.get_paths() if 'GAZEBO_MODEL_PATH' in environ: model += ':' + environ['GAZEBO_MODEL_PATH'] if 'GAZEBO_PLUGIN_PATH' in environ: plugin += ':' + environ['GAZEBO_PLUGIN_PATH'] if 'GAZEBO_RESOURCE_PATH' in environ: media += ':' + environ['GAZEBO_RESOURCE_PATH'] env = { 'GAZEBO_MODEL_PATH': model, 'GAZEBO_PLUGIN_PATH': plugin, 'GAZEBO_RESOURCE_PATH': media } return LaunchDescription([ DeclareLaunchArgument( 'version', default_value='false', description='Set "true" to output version information'), DeclareLaunchArgument( 'verbose', default_value='false', description='Set "true" to increase messages written to terminal'), DeclareLaunchArgument( 'help', default_value='false', description='Set "true" to produce gzclient help message'), DeclareLaunchArgument( 'extra_gazebo_args', default_value='', description='Extra arguments to be passed to Gazebo'), # Specific to gazebo_ros DeclareLaunchArgument( 'gdb', default_value='false', description='Set "true" to run gzserver with gdb'), DeclareLaunchArgument( 'valgrind', default_value='false', description='Set "true" to run gzserver with valgrind'), # Execute ExecuteProcess( cmd=[[ 'gzclient', _boolean_command('version'), ' ', _boolean_command('verbose'), ' ', _boolean_command('help'), ' ', LaunchConfiguration('extra_gazebo_args'), ]], output='screen', additional_env=env, shell=True, prefix=PythonExpression([ '"gdb -ex run --args" if "true" == "', LaunchConfiguration('gdb'), '"else "valgrind" if "true" == "', LaunchConfiguration('valgrind'), '"else ""' ]), ) ])
def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch') # Create the launch configuration variables slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') use_simulator = LaunchConfiguration('use_simulator') use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') world = LaunchConfiguration('world') # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative # https://github.com/ros/geometry2/issues/32 # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='', description='Top-level namespace') declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', description='Whether to apply a namespace to the navigation stack') declare_slam_cmd = DeclareLaunchArgument( 'slam', default_value='False', description='Whether run a SLAM') declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join( bringup_dir, 'maps', 'turtlebot3_world.yaml'), description='Full path to map file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true') declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description='Full path to the ROS2 parameters file to use for all launched nodes') declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join( bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') declare_use_simulator_cmd = DeclareLaunchArgument( 'use_simulator', default_value='True', description='Whether to start the simulator') declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', description='Whether to start the robot state publisher') declare_use_rviz_cmd = DeclareLaunchArgument( 'use_rviz', default_value='True', description='Whether to start RVIZ') declare_simulator_cmd = DeclareLaunchArgument( 'headless', default_value='False', description='Whether to execute gzclient)') declare_world_cmd = DeclareLaunchArgument( 'world', # TODO(orduno) Switch back once ROS argument passing has been fixed upstream # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), # worlds/turtlebot3_worlds/waffle.model') default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), description='Full path to world model file to load') # Specify the actions start_gazebo_server_cmd = ExecuteProcess( condition=IfCondition(use_simulator), cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world], cwd=[launch_dir], output='screen') start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression( [use_simulator, ' and not ', headless])), cmd=['gzclient'], cwd=[launch_dir], output='screen') urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( condition=IfCondition(use_robot_state_pub), package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', namespace=namespace, output='screen', parameters=[{'use_sim_time': use_sim_time}], remappings=remappings, arguments=[urdf]) rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_dir, 'rviz_launch.py')), condition=IfCondition(use_rviz), launch_arguments={'namespace': '', 'use_namespace': 'False', 'rviz_config': rviz_config_file}.items()) bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={'namespace': namespace, 'use_namespace': use_namespace, 'slam': slam, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'params_file': params_file, 'autostart': autostart}.items()) # Create the launch description and populate ld = LaunchDescription() # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_simulator_cmd) ld.add_action(declare_use_robot_state_pub_cmd) ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) ld.add_action(declare_world_cmd) # Add any conditioned actions ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) return ld