def generate_launch_description():
xacro_path = os.path.join(get_package_share_directory('dsr_description2'),
'xacro')
# planning_context
robot_description = {
'robot_description':
Command([
'xacro', ' ', xacro_path, '/',
LaunchConfiguration('model'), '.urdf.xacro color:=',
LaunchConfiguration('color')
])
}
robot_description_semantic_config = load_file('moveit_config_m1013',
'config/m1013.srdf')
robot_description_semantic = {
'robot_description_semantic': robot_description_semantic_config
}
kinematics_yaml = load_yaml('moveit_config_m1013',
'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_config_m1013',
'config/ompl_planning.yaml')
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
# Trajectory Execution Functionality
controllers_yaml = load_yaml('moveit_config_m1013',
'config/controllers.yaml')
moveit_controllers = {
'moveit_simple_controller_manager':
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(
'dsr_description2') + "/rviz/moveit.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', 'base', 'base_0'])
# Publish TF
robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='both',
parameters=[robot_description])
# Fake joint driver
fake_joint_driver_node = Node(
package='fake_joint_driver',
executable='fake_joint_driver_node',
parameters=[
{
'controller_name': 'dsr_joint_trajectory_controller'
},
os.path.join(get_package_share_directory("moveit_config_m1013"),
"config", "fake_controllers.yaml"),
os.path.join(get_package_share_directory("moveit_config_m1013"),
"config", "start_positions.yaml"), robot_description
])
# 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')
# dsr_control2
dsr_control2 = Node(
package='dsr_control2',
executable='dsr_control_node2',
name='dsr_control_node2',
output='screen',
parameters=[
{
"name": LaunchConfiguration('name')
},
{
"rate": 100
},
{
"standby": 5000
},
{
"command": True
},
{
"host": LaunchConfiguration('host')
},
{
"port": LaunchConfiguration('port')
},
{
"mode": LaunchConfiguration('mode')
},
{
"model": LaunchConfiguration('model')
},
{
"gripper": "none"
},
{
"mobile": "none"
},
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
])
return LaunchDescription(args + [
rviz_node, static_tf, robot_state_publisher, run_move_group_node,
fake_joint_driver_node, mongodb_server_node, dsr_control2
])
def generate_launch_description():
runtime_share = get_package_share_directory('robot_runtime')
teleop_params_file = os.path.join(runtime_share, 'config',
'teleop_xbox.config.yaml')
use_base_driver = IfCondition(LaunchConfiguration('base_driver'))
standard_params = {'use_sim_time': LaunchConfiguration('use_sim_time')}
base_device = LaunchConfiguration('base_device')
return LaunchDescription([
DeclareLaunchArgument('base_driver', default_value='true'),
DeclareLaunchArgument('use_sim_time', default_value='false'),
DeclareLaunchArgument('base_device', default_value='/dev/ttyUSB0'),
# Base
Node(
package='kobuki_node',
executable='kobuki_ros_node',
name='kobuki_node',
parameters=[standard_params, {
'device_port': base_device,
}],
condition=use_base_driver,
output='screen',
),
# Teleop
Node(
package='cmd_vel_mux',
executable='cmd_vel_mux',
name='cmd_vel_mux',
parameters=[standard_params],
remappings=[('/cmd_vel', '/commands/velocity')],
output='screen',
),
GroupAction([
PushRosNamespace('joy'),
Node(
package='robot_indicators',
executable='robot_indicators',
name='xpad_led',
parameters=[standard_params],
output='screen',
),
Node(
package='robot_indicators',
executable='joy_commands',
name='commands',
parameters=[standard_params],
output='screen',
),
Node(
package='joy',
executable='joy_node',
name='driver',
parameters=[standard_params],
output='screen',
),
Node(
package='teleop_twist_joy',
executable='teleop_node',
name='interpreter',
parameters=[
teleop_params_file,
standard_params,
],
output='screen',
),
]),
])
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')
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')
# 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(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_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',
node_executable='robot_state_publisher',
node_name='robot_state_publisher',
node_namespace=namespace,
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}],
remappings=remappings,
arguments=[urdf])
rviz_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'nav2_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, 'nav2_bringup_launch.py')),
launch_arguments={
'namespace': namespace,
'use_namespace': use_namespace,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'params_file': params_file,
'bt_xml_file': bt_xml_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_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_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
def generate_launch_description():
ld = launch.LaunchDescription()
pkg_name = "navigation"
pkg_share_path = get_package_share_directory(pkg_name)
ld.add_action(
launch.actions.DeclareLaunchArgument("use_sim_time",
default_value="false"))
ld.add_action(
launch.actions.DeclareLaunchArgument("debug", default_value="false"))
dbg_sub = None
if sys.stdout.isatty():
dbg_sub = [
launch.substitutions.PythonExpression([
'"" if "false" == "',
launch.substitutions.LaunchConfiguration("debug"),
'" else "debug_ros2launch ' + os.ttyname(sys.stdout.fileno()) +
'"'
]), 'stdbuf -o L'
]
DRONE_DEVICE_ID = os.getenv('DRONE_DEVICE_ID')
namespace = DRONE_DEVICE_ID
ld.add_action(
ComposableNodeContainer(
namespace='',
name=namespace + '_navigation',
package='rclcpp_components',
executable='component_container_mt',
composable_node_descriptions=[
ComposableNode(
package=pkg_name,
plugin='navigation::Navigation',
namespace=namespace,
name='navigation',
parameters=[
pkg_share_path + '/config/navigation.yaml',
{
"use_sim_time":
launch.substitutions.LaunchConfiguration(
"use_sim_time")
},
],
remappings=[
("~/octomap_in", "/" + DRONE_DEVICE_ID +
"/octomap_server/octomap_full"),
("~/odometry_in", "/" + DRONE_DEVICE_ID +
"/control_interface/local_odom"),
("~/desired_pose_in", "/" + DRONE_DEVICE_ID +
"/control_interface/desired_pose"),
("~/hover_in", "~/hover"),
("~/goto_in", "~/goto_waypoints"),
("~/goto_trigger_in", "~/goto_start"),
("~/control_diagnostics_in", "/" + DRONE_DEVICE_ID +
"/control_interface/diagnostics"),
("~/bumper_in",
"/" + DRONE_DEVICE_ID + "/bumper/obstacle_sectors"),
("~/local_waypoint_in", "~/local_waypoint"),
("~/local_path_in", "~/local_path"),
("~/gps_waypoint_in", "~/gps_waypoint"),
("~/gps_path_in", "~/gps_path"),
("~/diagnostics_out", "~/diagnostics"),
("~/local_path_out", "/" + DRONE_DEVICE_ID +
"/control_interface/local_path"),
("~/gps_path_out", "/" + DRONE_DEVICE_ID +
"/control_interface/gps_path"),
("~/waypoint_to_local_out", "/" + DRONE_DEVICE_ID +
"/control_interface/waypoint_to_local"),
("~/path_to_local_out", "/" + DRONE_DEVICE_ID +
"/control_interface/path_to_local"),
],
),
],
output='screen',
prefix=dbg_sub,
parameters=[
{
"use_sim_time":
launch.substitutions.LaunchConfiguration("use_sim_time")
},
],
))
return ld
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
rviz_config_file = LaunchConfiguration('rviz_config')
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
default_value='navigation',
description=(
'Top-level namespace. The value will be used to replace the '
'<robot_namespace> keyword on the rviz config file.'))
declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config',
default_value=os.path.join(bringup_dir, 'rviz',
'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
# Launch rviz
start_rviz_cmd = Node(condition=UnlessCondition(use_namespace),
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=['-d', rviz_config_file],
output='screen')
namespaced_rviz_config_file = ReplaceString(
source_file=rviz_config_file,
replacements={'<robot_namespace>': ('/', namespace)})
start_namespaced_rviz_cmd = Node(
condition=IfCondition(use_namespace),
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
node_namespace=namespace,
arguments=['-d', namespaced_rviz_config_file],
output='screen',
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static'),
('/goal_pose', 'goal_pose'),
('/clicked_point', 'clicked_point'),
('/initialpose', 'initialpose')])
exit_event_handler = RegisterEventHandler(
condition=UnlessCondition(use_namespace),
event_handler=OnProcessExit(
target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
exit_event_handler_namespaced = RegisterEventHandler(
condition=IfCondition(use_namespace),
event_handler=OnProcessExit(
target_action=start_namespaced_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
# 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_rviz_config_file_cmd)
# Add any conditioned actions
ld.add_action(start_rviz_cmd)
ld.add_action(start_namespaced_rviz_cmd)
# Add other nodes and processes we need
ld.add_action(exit_event_handler)
ld.add_action(exit_event_handler_namespaced)
return ld
def generate_launch_description():
# Get the launch directory
launch_dir = os.path.join(get_package_share_directory('nav2_bringup'),
'launch')
# Create the launch configuration variables
map_yaml_file = launch.substitutions.LaunchConfiguration('map')
use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time')
params_file = launch.substitutions.LaunchConfiguration('params')
bt_xml_file = launch.substitutions.LaunchConfiguration('bt_xml_file')
autostart = launch.substitutions.LaunchConfiguration('autostart')
stdout_linebuf_envvar = launch.actions.SetEnvironmentVariable(
'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'yaml_filename': map_yaml_file,
'bt_xml_filename': bt_xml_file,
'autostart': autostart
}
configured_params = RewrittenYaml(source_file=params_file,
rewrites=param_substitutions,
convert_types=True)
# Declare the launch arguments
declare_map_yaml_cmd = launch.actions.DeclareLaunchArgument(
'map',
default_value='test_map.yaml',
description='Full path to map file to load')
declare_use_sim_time_cmd = launch.actions.DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = launch.actions.DeclareLaunchArgument(
'params',
default_value=os.path.join(launch_dir, 'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
declare_autostart_cmd = launch.actions.DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack')
declare_bt_xml_cmd = launch.actions.DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(get_package_prefix('nav2_bt_navigator'),
'behavior_trees',
'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
start_map_server_cmd = launch_ros.actions.Node(
package='nav2_map_server',
node_executable='map_server',
node_name='map_server',
output='screen',
parameters=[configured_params])
start_localizer_cmd = launch_ros.actions.Node(
package='nav2_amcl',
node_executable='amcl',
node_name='amcl',
output='screen',
parameters=[configured_params])
start_world_model_cmd = launch_ros.actions.Node(
package='nav2_world_model',
node_executable='world_model',
output='screen',
parameters=[configured_params])
start_dwb_cmd = launch_ros.actions.Node(package='dwb_controller',
node_executable='dwb_controller',
output='screen',
parameters=[configured_params])
start_planner_cmd = launch_ros.actions.Node(
package='nav2_navfn_planner',
node_executable='navfn_planner',
node_name='navfn_planner',
output='screen',
parameters=[configured_params])
start_primitive_cmd = launch_ros.actions.Node(
package='nav2_motion_primitives',
node_executable='motion_primitives_node',
node_name='motion_primitives',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}])
start_navigator_cmd = launch_ros.actions.Node(
package='nav2_bt_navigator',
node_executable='bt_navigator',
node_name='bt_navigator',
output='screen',
parameters=[configured_params])
start_lifecycle_manager_cmd = launch_ros.actions.Node(
package='nav2_lifecycle_manager',
node_executable='lifecycle_manager',
node_name='lifecycle_manager',
output='screen',
parameters=[configured_params])
# Create the launch description and populate
ld = launch.LaunchDescription()
# Set environment variables
ld.add_action(stdout_linebuf_envvar)
# Declare the launch options
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_bt_xml_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_lifecycle_manager_cmd)
ld.add_action(start_map_server_cmd)
ld.add_action(start_localizer_cmd)
ld.add_action(start_world_model_cmd)
ld.add_action(start_dwb_cmd)
ld.add_action(start_planner_cmd)
ld.add_action(start_primitive_cmd)
ld.add_action(start_navigator_cmd)
return ld
def generate_launch_description():
# Camera model
# use:
# - 'zed' for "ZED" camera
# - 'zedm' for "ZED mini" camera
# - 'zed2' for "ZED2" camera
# - 'zed2i' for "ZED2i" camera
camera_model = 'zed'
# Launch configuration variables (can be changed by CLI command)
svo_path = LaunchConfiguration('svo_path')
# Configuration variables
# Camera name. Can be different from camera model, used to distinguish camera in multi-camera systems
camera_name = 'zed'
node_name = 'zed_node' # Zed Node name
publish_urdf = 'true' # Publish static frames from camera URDF
# Robot base frame. Note: overrides the parameter `pos_tracking.base_frame` in `common.yaml`.
base_frame = 'base_link'
# Position X of the camera with respect to the base frame [m].
cam_pos_x = '0.0'
# Position Y of the camera with respect to the base frame [m].
cam_pos_y = '0.0'
# Position Z of the camera with respect to the base frame [m].
cam_pos_z = '0.0'
# Roll orientation of the camera with respect to the base frame [rad].
cam_roll = '0.0'
# Pitch orientation of the camera with respect to the base frame [rad].
cam_pitch = '0.0'
# Yaw orientation of the camera with respect to the base frame [rad].
cam_yaw = '0.0'
# ZED Configurations to be loaded by ZED Node
config_common_path = os.path.join(
get_package_share_directory('zed_wrapper'), 'config', 'common.yaml')
if (camera_model != 'zed'):
config_camera_path = os.path.join(
get_package_share_directory('zed_wrapper'), 'config',
camera_model + '.yaml')
else:
config_camera_path = ''
# URDF/xacro file to be loaded by the Robot State Publisher node
xacro_path = os.path.join(get_package_share_directory('zed_wrapper'),
'urdf', 'zed_descr.urdf.xacro')
# ZED Wrapper node
zed_wrapper_launch = IncludeLaunchDescription(
launch_description_source=PythonLaunchDescriptionSource([
get_package_share_directory('zed_wrapper'),
'/launch/include/zed_camera.launch.py'
]),
launch_arguments={
'camera_model': camera_model,
'camera_name': camera_name,
'node_name': node_name,
'config_common_path': config_common_path,
'config_camera_path': config_camera_path,
'publish_urdf': publish_urdf,
'xacro_path': xacro_path,
'svo_path': svo_path,
'base_frame': base_frame,
'cam_pos_x': cam_pos_x,
'cam_pos_y': cam_pos_y,
'cam_pos_z': cam_pos_z,
'cam_roll': cam_roll,
'cam_pitch': cam_pitch,
'cam_yaw': cam_yaw
}.items())
declare_svo_path_cmd = DeclareLaunchArgument(
'svo_path',
default_value='',
description=
'Path to an input SVO file. Note: overrides the parameter `general.svo_file` in `common.yaml`.'
)
# Define LaunchDescription variable
ld = LaunchDescription()
# Launch parameters
ld.add_action(declare_svo_path_cmd)
# Add nodes to LaunchDescription
ld.add_action(zed_wrapper_launch)
return ld
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("moveit2_tutorials") +
"/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")
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 = {
"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
moveit_cpp_node = Node(
name="moveit_cpp_tutorial",
package="moveit2_tutorials",
executable="moveit_cpp_tutorial",
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("moveit2_tutorials") +
"/launch/moveit_cpp_tutorial.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_broadcaster",
]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner {}".format(controller)
],
shell=True,
output="screen",
)
]
return LaunchDescription([
static_tf,
robot_state_publisher,
rviz_node,
moveit_cpp_node,
ros2_control_node,
] + load_controllers)
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')
use_namespace = LaunchConfiguration('use_namespace')
slam = LaunchConfiguration('slam')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
autostart = LaunchConfiguration('autostart')
stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
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', description='Full path to map yaml file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
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(
'default_bt_xml_filename',
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='true',
description='Automatically startup the nav2 stack')
# Specify the actions
bringup_cmd_group = GroupAction([
PushRosNamespace(condition=IfCondition(use_namespace),
namespace=namespace),
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'slam_launch.py')),
condition=IfCondition(slam),
launch_arguments={
'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file
}.items()),
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'localization_launch.py')),
condition=IfCondition(
PythonExpression(['not ', slam])),
launch_arguments={
'namespace': namespace,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'use_lifecycle_mgr': 'false'
}.items()),
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'navigation_launch.py')),
launch_arguments={
'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'default_bt_xml_filename':
default_bt_xml_filename,
'use_lifecycle_mgr': 'false',
'map_subscribe_transient_local': 'true'
}.items()),
])
# Create the launch description and populate
ld = LaunchDescription()
# Set environment variables
ld.add_action(stdout_linebuf_envvar)
# 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_bt_xml_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd_group)
return ld
import os
import launch
import launch_ros.actions
from ament_index_python.packages import get_package_share_directory
# string with message to publish on topic /carla/available/scenarios
ros_topic_msg_string = "{{ 'scenarios': [{{ 'name': 'FollowLeadingVehicle', 'scenario_file': '{}'}}] }}".format(
os.path.join(get_package_share_directory('carla_ad_demo'),
'config/FollowLeadingVehicle.xosc'))
def generate_launch_description():
ld = launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(name='host',
default_value='localhost'),
launch.actions.DeclareLaunchArgument(name='port',
default_value='2000'),
launch.actions.DeclareLaunchArgument(name='town',
default_value='Town01'),
launch.actions.DeclareLaunchArgument(name='timeout',
default_value='2'),
launch.actions.DeclareLaunchArgument(
name='synchronous_mode_wait_for_vehicle_control_command',
default_value='False'),
launch.actions.DeclareLaunchArgument(name='fixed_delta_seconds',
default_value='0.05'),
launch.actions.DeclareLaunchArgument(
name='scenario_runner_path',
default_value=os.environ.get('SCENARIO_RUNNER_PATH')),
launch.actions.DeclareLaunchArgument(name='role_name',
def generate_launch_description():
ld = launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(name='host',
default_value='localhost'),
launch.actions.DeclareLaunchArgument(name='port',
default_value='2000'),
launch.actions.DeclareLaunchArgument(name='town',
default_value='Town01'),
launch.actions.DeclareLaunchArgument(name='timeout',
default_value='2'),
launch.actions.DeclareLaunchArgument(
name='synchronous_mode_wait_for_vehicle_control_command',
default_value='False'),
launch.actions.DeclareLaunchArgument(name='fixed_delta_seconds',
default_value='0.05'),
launch.actions.DeclareLaunchArgument(
name='scenario_runner_path',
default_value=os.environ.get('SCENARIO_RUNNER_PATH')),
launch.actions.DeclareLaunchArgument(name='role_name',
default_value='ego_vehicle'),
launch_ros.actions.Node(
package='carla_twist_to_control',
executable='carla_twist_to_control',
name='carla_twist_to_control',
remappings=[([
"/carla/",
launch.substitutions.LaunchConfiguration('role_name'),
"/vehicle_control_cmd"
], [
"/carla/",
launch.substitutions.LaunchConfiguration('role_name'),
"/vehicle_control_cmd_manual"
])],
parameters=[{
'role_name':
launch.substitutions.LaunchConfiguration('role_name')
}]),
launch.actions.ExecuteProcess(
cmd=[
"ros2", "topic", "pub", "/carla/available_scenarios",
"carla_ros_scenario_runner_types/CarlaScenarioList",
ros_topic_msg_string
],
name='topic_pub_vailable_scenarios',
),
launch.actions.IncludeLaunchDescription(
launch.launch_description_sources.PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('carla_ros_bridge'),
'carla_ros_bridge.launch.py')),
launch_arguments={
'host':
launch.substitutions.LaunchConfiguration('host'),
'port':
launch.substitutions.LaunchConfiguration('port'),
'town':
launch.substitutions.LaunchConfiguration('town'),
'timeout':
launch.substitutions.LaunchConfiguration('timeout'),
'synchronous_mode_wait_for_vehicle_control_command':
launch.substitutions.LaunchConfiguration(
'synchronous_mode_wait_for_vehicle_control_command'),
'fixed_delta_seconds':
launch.substitutions.LaunchConfiguration('fixed_delta_seconds')
}.items()),
launch.actions.IncludeLaunchDescription(
launch.launch_description_sources.PythonLaunchDescriptionSource(
os.path.join(
get_package_share_directory('carla_spawn_objects'),
'carla_example_ego_vehicle.launch.py')),
launch_arguments={
'object_definition_file':
get_package_share_directory('carla_spawn_objects') +
'/config/objects.json',
'role_name':
launch.substitutions.LaunchConfiguration('role_name')
}.items()),
launch.actions.IncludeLaunchDescription(
launch.launch_description_sources.PythonLaunchDescriptionSource(
os.path.join(
get_package_share_directory('carla_waypoint_publisher'),
'carla_waypoint_publisher.launch.py')),
launch_arguments={
'host': launch.substitutions.LaunchConfiguration('host'),
'port': launch.substitutions.LaunchConfiguration('port'),
'timeout': launch.substitutions.LaunchConfiguration('timeout'),
'role_name':
launch.substitutions.LaunchConfiguration('role_name')
}.items()),
launch.actions.IncludeLaunchDescription(
launch.launch_description_sources.PythonLaunchDescriptionSource(
os.path.join(
get_package_share_directory('carla_ros_scenario_runner'),
'carla_ros_scenario_runner.launch.py')),
launch_arguments={
'host':
launch.substitutions.LaunchConfiguration('host'),
'port':
launch.substitutions.LaunchConfiguration('port'),
'role_name':
launch.substitutions.LaunchConfiguration('role_name'),
'scenario_runner_path':
launch.substitutions.LaunchConfiguration(
'scenario_runner_path'),
'wait_for_ego':
'True'
}.items()),
launch_ros.actions.Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
remappings=[("carla/ego_vehicle/spectator_pose",
"/carla/ego_vehicle/rgb_view/control/set_transform")],
arguments=[
'-d',
os.path.join(get_package_share_directory('carla_ad_demo'),
'config/carla_ad_demo_ros2.rviz')
],
on_exit=launch.actions.Shutdown())
])
return ld
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
namespace = LaunchConfiguration('namespace')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
map_subscribe_transient_local = LaunchConfiguration(
'map_subscribe_transient_local')
# 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')]
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'bt_xml_filename': bt_xml_file,
'autostart': autostart,
'map_subscribe_transient_local': map_subscribe_transient_local
}
configured_params = RewrittenYaml(source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True)
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
DeclareLaunchArgument('namespace',
default_value='',
description='Top-level namespace'),
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack'),
DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params',
'nav2_params.yaml'),
description='Full path to the ROS2 parameters file to use'),
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'),
DeclareLaunchArgument(
'use_lifecycle_mgr',
default_value='true',
description='Whether to launch the lifecycle manager'),
DeclareLaunchArgument(
'map_subscribe_transient_local',
default_value='false',
description=
'Whether to set the map subscriber QoS to transient local'),
Node(package='nav2_controller',
node_executable='controller_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_planner',
node_executable='planner_server',
node_name='planner_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_recoveries',
node_executable='recoveries_server',
node_name='recoveries_server',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}],
remappings=remappings),
Node(package='nav2_bt_navigator',
node_executable='bt_navigator',
node_name='bt_navigator',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(package='nav2_waypoint_follower',
node_executable='waypoint_follower',
node_name='waypoint_follower',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(condition=IfCondition(use_lifecycle_mgr),
package='nav2_lifecycle_manager',
node_executable='lifecycle_manager',
node_name='lifecycle_manager_navigation',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'autostart': autostart
}, {
'node_names': [
'controller_server', 'planner_server',
'recoveries_server', 'bt_navigator', 'waypoint_follower'
]
}]),
])
import os
import sys
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
package_name = 'fiducial_vlam'
package_share_directory = get_package_share_directory(package_name)
package_launch_directory = os.path.join(package_share_directory, 'launch')
package_cfg_directory = os.path.join(package_share_directory, 'cfg')
pi30_tello_driver_args = [{
'drone_ip': '192.168.0.30',
'command_port': 11002,
'drone_port': 8889,
'data_port': 13002,
'video_port': 14002
}]
pi35_tello_driver_args = [{
'drone_ip': '192.168.0.35',
'command_port': 38065,
'drone_port': 8889,
'data_port': 8890,
'video_port': 11111
}]
vloc_args = [{
'loc_calibrate_not_localize': 1,
#!/usr/bin/python3
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
import launch
import os
frame_name = LaunchConfiguration("frame_name", default="laser_frame")
leg_detector_path = get_package_share_directory('leg_detector')
rviz2_config_path = leg_detector_path + "/rviz/default.rviz"
forest_file_path = leg_detector_path + "/config/trained_leg_detector_res=0.33.yaml"
def generate_launch_description():
ld = LaunchDescription([
# Launching RVIZ2
launch.actions.ExecuteProcess(
cmd=['ros2', 'run', 'rviz2', 'rviz2', '-d', rviz2_config_path],
output='screen')
])
declare_frame_name_cmd = DeclareLaunchArgument(
'frame_name',
default_value='laser_frame',
description='lidar frame name')
def launch(launch_descriptor, argv):
parser = argparse.ArgumentParser(description='launch amcl turtlebot demo')
parser.add_argument(
'--map',
help='path to map (will be passed to map_server)')
args = parser.parse_args(argv)
ld = launch_descriptor
package = 'turtlebot2_drivers'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='kobuki_node')],
name='kobuki_node',
exit_handler=restart_exit_handler,
)
package = 'astra_camera'
ld.add_process(
cmd=[
get_executable_path(package_name=package, executable_name='astra_camera_node'),
'-dw', '320', '-dh', '240', '-C', '-I'],
name='astra_camera_node',
exit_handler=restart_exit_handler,
)
package = 'depthimage_to_laserscan'
ld.add_process(
cmd=[
get_executable_path(
package_name=package, executable_name='depthimage_to_laserscan_node')],
name='depthimage_to_laserscan_node',
exit_handler=restart_exit_handler,
)
package = 'tf2_ros'
ld.add_process(
# The XYZ/Quat numbers for base_link -> camera_rgb_frame are taken from the
# turtlebot URDF in
# https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
cmd=[
get_executable_path(
package_name=package, executable_name='static_transform_publisher'),
'-0.087', '-0.0125', '0.287',
'0', '0', '0', '1',
'base_link',
'camera_rgb_frame'
],
name='static_tf_pub_base_rgb',
exit_handler=restart_exit_handler,
)
package = 'tf2_ros'
ld.add_process(
# The XYZ/Quat numbers for camera_rgb_frame -> camera_depth_frame are taken from the
# turtlebot URDF in
# https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
cmd=[
get_executable_path(
package_name=package, executable_name='static_transform_publisher'),
'0', '0.0250', '0',
'0', '0', '0', '1',
'camera_rgb_frame',
'camera_depth_frame'
],
name='static_tf_pub_rgb_depth',
exit_handler=restart_exit_handler,
)
package = 'joy'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='joy_node')],
name='joy_node',
exit_handler=restart_exit_handler,
)
package = 'teleop_twist_joy'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='teleop_node')],
name='teleop_node',
exit_handler=restart_exit_handler,
)
turtlebot2_amcl_share = get_package_share_directory('turtlebot2_amcl')
map_path = os.path.join(turtlebot2_amcl_share, 'examples', 'osrf_map.yaml')
if args.map:
map_path = args.map
package = 'map_server'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='map_server'), map_path],
name='map_server',
)
package = 'amcl'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='amcl'), '--use-map-topic'],
name='amcl',
exit_handler=restart_exit_handler,
output_handlers=[ConsoleOutput()],
)
return ld
def generate_launch_description():
# Get URDF via xacro
robot_description_path = os.path.join(
get_package_share_directory('robot_description'), 'urdf', 'panda.xacro'
)
robot_description_config = xacro.process_file(
robot_description_path,
mappings={'end_effector': 'false', 'gazebo': 'false'}
)
robot_description = {'robot_description': robot_description_config.toxml()}
robot_controller = os.path.join(
get_package_share_directory('robot_hw_interface'),
'config',
'robot_controllers.yaml'
)
rviz_config_file = os.path.join(
get_package_share_directory('robot_hw_interface'),
'rviz',
'panda.rviz'
)
control_node = Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[robot_description, robot_controller],
output={
'stdout': 'screen',
'stderr': 'screen',
},
)
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
parameters=[robot_description]
)
start_controller_node = Node(
package='robot_hw_interface',
executable='start_controller',
output='screen',
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config_file],
)
return LaunchDescription([
control_node,
start_controller_node,
robot_state_publisher_node,
rviz_node,
])
def generate_launch_description():
if not "tesseract_collision" in os.environ["AMENT_PREFIX_PATH"]:
# workaround for pluginlib ClassLoader bug: manually add tesseract_collision to the AMENT_PREFIX_PATH env variable
head, tail = os.path.split(get_package_prefix('crs_support'))
path = os.path.join(head, 'tesseract_collision')
os.environ["AMENT_PREFIX_PATH"] += os.pathsep + path
urdf = os.path.join(get_package_share_directory('crs_support'), 'urdf',
'swri_demo.urdf')
srdf = os.path.join(get_package_share_directory('crs_support'), 'urdf',
'ur10e_robot.srdf')
gzworld = os.path.join(get_package_share_directory('crs_support'),
'worlds', 'crs.world')
try:
crs_models_dir = str(Path.home().joinpath(
'.gazebo', 'models', 'crs_support').resolve(strict=True))
except FileNotFoundError: #os.path.exists(crs_models_dir):
gazebo_path = str(Path.home().joinpath('.gazebo', 'models').resolve())
os.mkdir(gazebo_path + "/crs_support")
shutil.copytree(
os.path.join(get_package_share_directory('crs_support'), 'meshes'),
Path.home().joinpath(Path.home().joinpath('.gazebo', 'models',
'crs_support',
'meshes').resolve()))
tesseract_env = launch_ros.actions.Node(
node_name='env_node',
package='tesseract_monitoring',
node_executable='tesseract_monitoring_environment_node',
output='screen',
parameters=[{
'use_sim_time': 'true',
'desc_param': 'robot_description',
'robot_description': urdf,
'robot_description_semantic': srdf
}])
gzserver = launch.actions.ExecuteProcess(cmd=[
'gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so', '--world',
gzworld
],
output='screen')
spawner1 = launch_ros.actions.Node(node_name='spawn_node',
package='gazebo_ros',
node_executable='spawn_entity.py',
arguments=[
'-entity', 'test', '-x', '0', '-y',
'0', '-z', '0.05', '-file', urdf
])
return launch.LaunchDescription([
# environment
tesseract_env,
# gazebo
gzserver,
spawner1
])
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 generate_launch_description():
# 1 or more drones:
drones = ['drone1', 'drone2', 'drone3', 'drone4']
# Starting locations:
starting_locations = [
# Face the wall of markers in fiducial.world
['-2.5', '1.5', '1', '0'],
['-1.5', '0.5', '1', '0.785'],
['-0.5', '1.5', '1', '0'],
['-1.5', '2.5', '1', '-0.785']
# 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']
]
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=[{
'use_sim_time': True, # 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': True, # 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': True, # 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': True, # 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': True, # 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': True, # 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
# }]),
# Drone controller
Node(
package='flock2',
node_executable='drone_base',
output='screen',
node_name='drone_base',
node_namespace=namespace,
parameters=[{
'use_sim_time': True, # Use /clock if available
}]),
])
return LaunchDescription(entities)
def generate_launch_description():
# Get the launch directory
pkg_dir = get_package_share_directory('clean_up')
nav_dir = get_package_share_directory('gb_navigation')
manipulation_dir = get_package_share_directory('gb_manipulation')
gb_world_model_dir = get_package_share_directory('gb_world_model')
world_config_dir = os.path.join(gb_world_model_dir, 'config')
namespace = LaunchConfiguration('namespace')
dope_params_file = LaunchConfiguration('dope_params_file')
declare_dope_params_cmd = DeclareLaunchArgument(
'dope_params_file',
default_value=os.path.join(get_package_share_directory('dope_launch'),
'config', 'config_pose.yaml'),
description='Full path to the TF Pose Estimation parameters file to use'
)
declare_namespace_cmd = DeclareLaunchArgument('namespace',
default_value='',
description='Namespace')
stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
gb_manipulation_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('gb_manipulation'),
'launch', 'gb_manipulation_launch.py')))
gb_navigation_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('gb_navigation'),
'launch', 'nav2_tiago_launch.py')))
plansys2_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('plansys2_bringup'),
'launch', 'plansys2_bringup_launch_monolithic.py')),
launch_arguments={
'model_file':
pkg_dir + '/pddl/domain.pddl:' + nav_dir + '/pddl/domain.pddl'
}.items())
dope_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('dope_launch'), 'launch',
'dope_launch.py')),
launch_arguments={'dope_params_file': dope_params_file}.items())
# Specify the actions
clean_up_executor_cmd = LifecycleNode(package='clean_up',
executable='cleanup_executor_node',
name='cleanup_executor_node')
emit_event_to_request_that_clean_up_executor_configure_transition = EmitEvent(
event=ChangeState(
lifecycle_node_matcher=launch.events.matches_action(
clean_up_executor_cmd),
transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
))
emit_event_to_request_that_clean_up_executor_activate_transition = EmitEvent(
event=ChangeState(
lifecycle_node_matcher=launch.events.matches_action(
clean_up_executor_cmd),
transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
))
on_configure_clean_up_executor_handler = RegisterEventHandler(
OnStateTransition(
target_lifecycle_node=clean_up_executor_cmd,
goal_state='inactive',
entities=[
emit_event_to_request_that_clean_up_executor_activate_transition
]))
# Specify the dependencies
vision_cmd = LifecycleNode(package='clean_up',
executable='vision_sim_node',
name='vision')
emit_event_to_request_that_vision_configure_transition = EmitEvent(
event=ChangeState(
lifecycle_node_matcher=launch.events.matches_action(vision_cmd),
transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
))
attention_manager_cmd = Node(package='gb_attention',
executable='attention_server',
output='screen')
wm_cmd = Node(package='gb_world_model',
executable='world_model_main',
output='screen',
parameters=[world_config_dir + '/world.yml'])
ld = LaunchDescription()
# Set environment variables
ld.add_action(stdout_linebuf_envvar)
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_dope_params_cmd)
# Declare the launch options
# Event handlers
ld.add_action(on_configure_clean_up_executor_handler)
ld.add_action(gb_manipulation_cmd)
ld.add_action(gb_navigation_cmd)
ld.add_action(plansys2_cmd)
ld.add_action(clean_up_executor_cmd)
ld.add_action(vision_cmd)
ld.add_action(emit_event_to_request_that_vision_configure_transition)
ld.add_action(
emit_event_to_request_that_clean_up_executor_configure_transition)
ld.add_action(attention_manager_cmd)
ld.add_action(wm_cmd)
ld.add_action(dope_cmd)
return ld
def convertUrdfContent(input,
output=None,
robotName=None,
normal=False,
boxCollision=False,
disableMeshOptimization=False,
enableMultiFile=False,
toolSlot=None,
initTranslation='0 0 0',
initRotation='0 0 1 0',
initPos=None,
linkToDef=False,
jointToDef=False,
relativePathPrefix=None):
"""
Convert a URDF content string into a Webots PROTO file or Robot node string.
The current working directory will be used for relative paths in your URDF file.
To use the location of your URDF file for relative paths, please use the convertUrdfFile() function.
"""
# Retrieve urdfPath if this function has been called from convertUrdfFile()
# And set urdfDirectory accordingly
urdfPath = None
if convertUrdfFile.urdfPath is not None:
urdfPath = convertUrdfFile.urdfPath
urdfDirectory = os.path.dirname(urdfPath)
convertUrdfFile.urdfPath = None
elif relativePathPrefix is not None:
urdfDirectory = relativePathPrefix
else:
urdfDirectory = os.getcwd()
if not type(initTranslation) == str or len(initTranslation.split()) != 3:
sys.exit(
'--translation argument is not valid. It has to be of Type = str and contain 3 values.'
)
if not type(initRotation) == str or len(initRotation.split()) != 4:
sys.exit(
'--rotation argument is not valid. It has to be of Type = str and contain 4 values.'
)
if initPos is not None:
try:
initPos = initPos.replace(",", ' ').replace("[", '').replace(
"]", '').replace("(", '').replace(")", '')
initPos = list(map(float, initPos.split()))
except Exception as e:
sys.exit(
e,
'\n--init-pos argument is not valid. Your list has to be inside of quotation marks. '
'Example: --init-pos="1.0, 2, -0.4"')
if robotName:
if robotName == '':
sys.exit(
'--robot-name argument is not valid. It cannot be an empty string.'
)
isProto = False
else:
isProto = True
urdf2webots.writeRobot.isProto = isProto
urdf2webots.parserURDF.disableMeshOptimization = disableMeshOptimization
urdf2webots.writeRobot.enableMultiFile = enableMultiFile
urdf2webots.writeRobot.initPos = initPos
if isProto:
urdf2webots.writeRobot.toolSlot = toolSlot
urdf2webots.writeRobot.linkToDef = linkToDef
urdf2webots.writeRobot.jointToDef = jointToDef
else:
urdf2webots.writeRobot.toolSlot = None
urdf2webots.writeRobot.linkToDef = False
urdf2webots.writeRobot.jointToDef = False
# Required resets in case of multiple conversions
urdf2webots.writeRobot.indexSolid = 0
urdf2webots.writeRobot.staticBase = False
urdf2webots.parserURDF.Material.namedMaterial.clear()
urdf2webots.parserURDF.Geometry.reference.clear()
# Replace "package://(.*)" occurences
for match in re.finditer('"package://(.*?)"', input):
packageName = match.group(1).split('/')[0]
directory = urdfDirectory
while packageName != os.path.split(directory)[1] and os.path.split(
directory)[1]:
directory = os.path.dirname(directory)
if not os.path.split(directory)[1]:
if 'ROS_VERSION' in os.environ:
if os.environ['ROS_VERSION'] == '1':
try:
rospack = rospkg.RosPack()
directory = rospack.get_path(packageName)
except rospkg.common.ResourceNotFound:
sys.stderr.write('Package "%s" not found.\n' %
packageName)
except NameError:
sys.stderr.write(
'Impossible to find location of "%s" package, installing "rospkg" might help.\n'
% packageName)
else:
try:
directory = get_package_share_directory(packageName)
except PackageNotFoundError:
sys.stderr.write('Package "%s" not found.\n' %
packageName)
else:
sys.stderr.write(
'ROS not sourced, package "%s" will not be found.\n' %
packageName)
if os.path.split(directory)[1]:
packagePath = os.path.split(directory)[0]
input = input.replace('package://' + packageName,
packagePath + '/' + packageName)
else:
sys.stderr.write('Can\'t determine package root path.\n')
# Convert the content into Webots robot
domFile = minidom.parseString(input)
for child in domFile.childNodes:
if child.localName == 'robot':
if isProto:
if output:
if os.path.splitext(
os.path.basename(output))[1] == '.proto':
robotName = os.path.splitext(
os.path.basename(output))[0]
outputFile = output
else:
# treat output as directory and construct filename
robotName = convertLUtoUN(
urdf2webots.parserURDF.getRobotName(
child)) # capitalize
outputFile = os.path.join(output, robotName + '.proto')
else:
robotName = convertLUtoUN(
urdf2webots.parserURDF.getRobotName(
child)) # capitalize
outputFile = output if output else robotName + '.proto'
mkdirSafe(outputFile.replace('.proto', '') +
'_textures') # make a dir called 'x_textures'
if enableMultiFile:
mkdirSafe(outputFile.replace('.proto', '') +
'_meshes') # make a dir called 'x_meshes'
urdf2webots.writeRobot.meshFilesPath = outputFile.replace(
'.proto', '') + '_meshes'
protoFile = open(outputFile, 'w')
urdf2webots.writeRobot.header(protoFile, urdfPath, robotName)
else:
tmp_robot_file = tempfile.NamedTemporaryFile(
mode="w+", prefix='tempRobotURDFStringWebots')
urdf2webots.writeRobot.robotName = robotName
urdf2webots.parserURDF.robotName = robotName # pass robotName
robot = child
linkElementList = []
jointElementList = []
for child in robot.childNodes:
if child.localName == 'link':
linkElementList.append(child)
elif child.localName == 'joint':
jointElementList.append(child)
elif child.localName == 'material':
if not child.hasAttribute('name') \
or child.getAttribute('name') not in urdf2webots.parserURDF.Material.namedMaterial:
material = urdf2webots.parserURDF.Material()
material.parseFromMaterialNode(child)
linkList = []
jointList = []
parentList = []
childList = []
rootLink = urdf2webots.parserURDF.Link()
for link in linkElementList:
linkList.append(
urdf2webots.parserURDF.getLink(link, urdfDirectory))
for joint in jointElementList:
jointList.append(urdf2webots.parserURDF.getJoint(joint))
urdf2webots.writeRobot.staticBase = urdf2webots.parserURDF.removeDummyLinksAndStaticBaseFlag(
linkList, jointList, toolSlot)
for joint in jointList:
parentList.append(joint.parent)
childList.append(joint.child)
parentList.sort()
childList.sort()
for link in linkList:
if urdf2webots.parserURDF.isRootLink(link.name, childList):
# We want to skip links between the robot and the static environment.
rootLink = link
previousRootLink = link
while rootLink in ['base_link', 'base_footprint']:
directJoints = []
for joint in jointList:
if joint.parent == rootLink.name:
directJoints.append(joint)
if len(directJoints) == 1:
for childLink in linkList:
if childLink.name == directJoints[0].child:
previousRootLink = rootLink
rootLink = childLink
else:
rootLink = previousRootLink
break
print('Root link: ' + rootLink.name)
break
for child in robot.childNodes:
if child.localName == 'gazebo':
urdf2webots.parserURDF.parseGazeboElement(
child, rootLink.name, linkList)
sensorList = (urdf2webots.parserURDF.IMU.list +
urdf2webots.parserURDF.P3D.list +
urdf2webots.parserURDF.Camera.list +
urdf2webots.parserURDF.Lidar.list)
print('There are %d links, %d joints and %d sensors' %
(len(linkList), len(jointList), len(sensorList)))
if isProto:
urdf2webots.writeRobot.declaration(protoFile, robotName,
initTranslation,
initRotation)
urdf2webots.writeRobot.URDFLink(protoFile,
rootLink,
1,
parentList,
childList,
linkList,
jointList,
sensorList,
boxCollision=boxCollision,
normal=normal,
robot=True)
protoFile.write('}\n')
protoFile.close()
return
else:
urdf2webots.writeRobot.URDFLink(
tmp_robot_file,
rootLink,
0,
parentList,
childList,
linkList,
jointList,
sensorList,
boxCollision=boxCollision,
normal=normal,
robot=True,
initTranslation=initTranslation,
initRotation=initRotation)
tmp_robot_file.seek(0)
return (tmp_robot_file.read())
sys.exit('Could not parse the URDF file.\n')
def __init__(self):
super().__init__('hal_ati_ft_mini45')
self.declare_parameter("hz", "10.0")
self.rate = float(self.get_parameter("hz").value)
self.declare_parameter("services.get_information",
"/ati_ft_mini45/get_information")
self.get_information_service_name = self.get_parameter(
"services.get_information").value
self.declare_parameter("services.get_biases",
"/ati_ft_mini45/get_biases")
self.get_biases_service_name = self.get_parameter(
"services.get_biases").value
self.declare_parameter("services.get_calibration_matrix",
"/ati_ft_mini45/get_calibration_matrix")
self.get_calibration_matrix_service_name = self.get_parameter(
"services.get_calibration_matrix").value
self.declare_parameter("services.set_biases",
"/ati_ft_mini45/set_biases")
self.set_biases_service_name = self.get_parameter(
"services.set_biases").value
self.declare_parameter("services.compute_calibration",
"/ati_ft_mini45/compute_calibration")
self.compute_calibration_service_name = self.get_parameter(
"services.compute_calibration").value
self.declare_parameter("services.compute_bias",
"/ati_ft_mini45/compute_bias")
self.compute_bias_service_name = self.get_parameter(
"services.compute_bias").value
self.declare_parameter("frames.ft_sensor", "ati_ft_link")
self.ft_link = self.get_parameter("frames.ft_sensor").value
self.declare_parameter("publishers.ft_measures",
"/ati_ft_mini45/ft_measures")
self.pub_ft_measures_name = self.get_parameter(
"publishers.ft_measures").value
self.publisher_ = self.create_publisher(WrenchStamped,
self.pub_ft_measures_name, 1)
self.srv_get_info = self.create_service(
ATIinformation, self.get_information_service_name,
self.get_information)
self.ser_biases_computed = self.create_service(
BiasesComputed, self.get_biases_service_name, self.get_biases)
self.ser_get_calib_matrix = self.create_service(
CalibrationMatrix, self.get_calibration_matrix_service_name,
self.get_calibration_matrix)
self.ser_set_biases = self.create_service(SetBiases,
self.set_biases_service_name,
self.set_biases)
self.ser_compute_calibration = self.create_service(
Empty, self.compute_calibration_service_name,
self.compute_calibration)
self.ser_compute_bias = self.create_service(
Empty, self.compute_bias_service_name, self.compute_bias)
self.bus_type = "socketcan"
self.declare_parameter("channel", "can0")
self.channel = self.get_parameter("channel").value
self.declare_parameter("calibrate_sensors_on_start", "False")
self.do_calibration = self.get_parameter(
"calibrate_sensors_on_start").value
self.hal_ft_sensor = HAL_ATI_FT_Mini45(bustype=self.bus_type,
channel=self.channel)
self.hal_ft_sensor.init(do_calibration=self.do_calibration)
package_share_directory = get_package_share_directory(
'hal_ati_ft_mini45')
self.calib_filename = package_share_directory + '/resources/FT10484_Net.xml'
self.hal_ft_sensor.parse_calibration_XML(self.calib_filename)
self.timer = self.create_timer(1.0 / self.rate, self.timer_callback)
def generate_launch_description():
# use: 'zed' for "ZED" camera - 'zedm' for "ZED mini" camera
camera_model = 'zed'
# URDF file to be loaded by Robot State Publisher
urdf = os.path.join(get_package_share_directory('stereolabs_zed'), 'urdf',
camera_model + '.urdf')
# ZED Configurations to be loaded by ZED Node
config_common = os.path.join(get_package_share_directory('stereolabs_zed'),
'config', 'common.yaml')
config_camera = os.path.join(get_package_share_directory('stereolabs_zed'),
'config', camera_model + '.yaml')
# Set LOG format
os.environ[
'RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message}'
# Launch Description
ld = launch.LaunchDescription()
# Prepare the ZED node
zed_node = LifecycleNode(
node_namespace='zed', # must match the namespace in config -> YAML
node_name='zed_node', # must match the node name in config -> YAML
package='stereolabs_zed',
node_executable='zed_wrapper_node',
output='screen',
parameters=[
config_common, # Common parameters
config_camera, # Camera related parameters
])
# Prepare the Robot State Publisher node
rsp_node = Node(node_name='zed_state_publisher',
package='robot_state_publisher',
node_executable='robot_state_publisher',
output='screen',
arguments=[urdf, 'robot_description:=zed_description'])
# Make the ZED node take the 'configure' transition
zed_configure_trans_event = EmitEvent(event=ChangeState(
lifecycle_node_matcher=launch.events.process.matches_action(zed_node),
transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
))
# Make the ZED node take the 'activate' transition
zed_activate_trans_event = EmitEvent(event=ChangeState(
lifecycle_node_matcher=launch.events.process.matches_action(zed_node),
transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
))
# Shutdown event
shutdown_event = EmitEvent(event=launch.events.Shutdown())
# When the ZED node reaches the 'inactive' state from 'unconfigured', make it take the 'activate' transition and start the Robot State Publisher
zed_inactive_from_unconfigured_state_handler = RegisterEventHandler(
OnStateTransition(
target_lifecycle_node=zed_node,
start_state='configuring',
goal_state='inactive',
entities=[
# Log
LogInfo(
msg=
"'ZED' reached the 'INACTIVE' state, start the 'Robot State Publisher' node and 'activating'."
),
# Robot State Publisher
rsp_node,
# Change State event ( inactive -> active )
zed_activate_trans_event,
],
))
# When the ZED node reaches the 'inactive' state from 'active', it has been deactivated and it will wait for a manual activation
zed_inactive_from_active_state_handler = RegisterEventHandler(
OnStateTransition(
target_lifecycle_node=zed_node,
start_state='deactivating',
goal_state='inactive',
entities=[
# Log
LogInfo(
msg=
"'ZED' reached the 'INACTIVE' state from 'ACTIVE' state. Waiting for manual activation..."
)
],
))
# When the ZED node reaches the 'active' state, log a message.
zed_active_state_handler = RegisterEventHandler(
OnStateTransition(
target_lifecycle_node=zed_node,
goal_state='active',
entities=[
# Log
LogInfo(msg="'ZED' reached the 'ACTIVE' state"),
],
))
# When the ZED node reaches the 'finalized' state, log a message and exit.
zed_finalized_state_handler = RegisterEventHandler(
OnStateTransition(
target_lifecycle_node=zed_node,
goal_state='finalized',
entities=[
# Log
LogInfo(
msg=
"'ZED' reached the 'FINALIZED' state. Killing the node..."
),
shutdown_event,
],
))
# Add the actions to the launch description.
# The order they are added reflects the order in which they will be executed.
ld.add_action(zed_inactive_from_unconfigured_state_handler)
ld.add_action(zed_inactive_from_active_state_handler)
ld.add_action(zed_active_state_handler)
ld.add_action(zed_finalized_state_handler)
ld.add_action(zed_node)
ld.add_action(zed_configure_trans_event)
return ld
def generate_launch_description():
# Must match camera name in URDF file
camera_name = 'forward_camera'
camera_frame = 'forward_camera_frame'
orca_description_path = get_package_share_directory('orca_description')
urdf_path = os.path.join(orca_description_path, 'urdf', 'pt2.urdf')
orca_driver_path = get_package_share_directory('orca_driver')
camera_info_path = os.path.join(orca_driver_path, 'cfg',
'brusb_wet_640x480.ini')
map_path = os.path.join(orca_driver_path, 'maps', 'simple_map.yaml')
return LaunchDescription([
# Publish static joints
Node(package='robot_state_publisher',
node_executable='robot_state_publisher',
output='log',
arguments=[urdf_path]),
# Forward camera
Node(package='opencv_cam',
node_executable='opencv_cam_node',
output='log',
node_name='opencv_cam_node',
remappings=[
('image_raw', '/' + camera_name + '/image_raw'),
('camera_info', '/' + camera_name + '/camera_info'),
],
parameters=[{
'camera_info_path': camera_info_path,
}]),
# Driver
Node(
package='orca_driver',
node_executable='driver_node',
output='log',
node_name='driver_node',
parameters=[{
'voltage_multiplier': 5.05,
'thruster_4_reverse':
True, # Thruster 4 ESC is programmed incorrectly
'tilt_channel': 6,
'voltage_min': 12.0
}]),
# AUV controller
Node(
package='orca_base',
node_executable='base_node',
output='log',
node_name='base_node',
parameters=[{
'auto_start': 5, # Auto-start mission >= 5
'auv_z_target': -0.5,
'auv_xy_distance': 2.0,
'auv_x_pid_kp': 0.0, # TODO
'auv_x_pid_ki': 0.0,
'auv_x_pid_kd': 0.0,
'auv_y_pid_kp': 0.0,
'auv_y_pid_ki': 0.0,
'auv_y_pid_kd': 0.0,
'auv_z_pid_kp': 0.0,
'auv_z_pid_ki': 0.0,
'auv_z_pid_kd': 0.0,
'auv_yaw_pid_kp': 0.0,
'auv_yaw_pid_ki': 0.0,
'auv_yaw_pid_kd': 0.0,
}],
remappings=[('filtered_odom', '/' + camera_name + '/base_odom')]),
# Mapper
Node(
package='fiducial_vlam',
node_executable='vmap_node',
output='log',
node_name='vmap_node',
parameters=[{
'publish_tfs': 1,
'marker_length': 0.1778,
'marker_map_load_full_filename': map_path,
'make_not_use_map': 0,
# 'map_init_style': 1, # Init style 1: marker id and location is specified below:
# 'map_init_id': 0,
# 'map_init_pose_x': 0.0,
# 'map_init_pose_y': 0.0,
# 'map_init_pose_z': -0.5,
# 'map_init_pose_roll': math.pi / 2,
# 'map_init_pose_pitch': 0.0,
# 'map_init_pose_yaw': -math.pi / 2,
}]),
# Localizer
Node(
package='fiducial_vlam',
node_executable='vloc_node',
output='log',
node_name='vloc_node',
node_namespace=camera_name,
parameters=[{
'publish_tfs': 1,
'publish_camera_pose': 0,
'publish_base_pose': 0,
'publish_camera_odom': 0,
'publish_base_odom': 1,
'publish_image_marked': 0,
'stamp_msgs_with_current_time':
0, # Use incoming message time, not now()
'camera_frame_id': camera_frame,
't_camera_base_x': 0.,
't_camera_base_y': 0.063,
't_camera_base_z': -0.16,
't_camera_base_roll': 0.,
't_camera_base_pitch': -math.pi / 2,
't_camera_base_yaw': math.pi / 2
}]),
])
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 description():
return get_package_share_directory(
vehicle_model.perform(context) + "_description")
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 launch_setup(context, *args, **kwargs):
# fmt: off
architecture_type = LaunchConfiguration("architecture_type",
default="awf/universe")
autoware_launch_file = LaunchConfiguration(
"autoware_launch_file",
default=default_autoware_launch_file_of(
architecture_type.perform(context)))
autoware_launch_package = LaunchConfiguration(
"autoware_launch_package",
default=default_autoware_launch_package_of(
architecture_type.perform(context)))
global_frame_rate = LaunchConfiguration("global_frame_rate", default=30.0)
global_real_time_factor = LaunchConfiguration("global_real_time_factor",
default=1.0)
global_timeout = LaunchConfiguration("global_timeout", default=180)
initialize_duration = LaunchConfiguration("initialize_duration",
default=30)
launch_autoware = LaunchConfiguration("launch_autoware", default=True)
launch_rviz = LaunchConfiguration("launch_rviz", default=False)
output_directory = LaunchConfiguration("output_directory",
default=Path("/tmp"))
port = LaunchConfiguration("port", default=8080)
record = LaunchConfiguration("record", default=True)
scenario = LaunchConfiguration("scenario", default=Path("/dev/null"))
sensor_model = LaunchConfiguration("sensor_model", default="")
sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8)
vehicle_model = LaunchConfiguration("vehicle_model", default="")
workflow = LaunchConfiguration("workflow", default=Path("/dev/null"))
# fmt: on
print(f"architecture_type := {architecture_type.perform(context)}")
print(
f"autoware_launch_file := {autoware_launch_file.perform(context)}")
print(
f"autoware_launch_package := {autoware_launch_package.perform(context)}"
)
print(f"global_frame_rate := {global_frame_rate.perform(context)}")
print(
f"global_real_time_factor := {global_real_time_factor.perform(context)}"
)
print(f"global_timeout := {global_timeout.perform(context)}")
print(f"initialize_duration := {initialize_duration.perform(context)}")
print(f"launch_autoware := {launch_autoware.perform(context)}")
print(f"launch_rviz := {launch_rviz.perform(context)}")
print(f"output_directory := {output_directory.perform(context)}")
print(f"port := {port.perform(context)}")
print(f"record := {record.perform(context)}")
print(f"scenario := {scenario.perform(context)}")
print(f"sensor_model := {sensor_model.perform(context)}")
print(f"sigterm_timeout := {sigterm_timeout.perform(context)}")
print(f"vehicle_model := {vehicle_model.perform(context)}")
print(f"workflow := {workflow.perform(context)}")
def make_parameters():
parameters = [
{
"architecture_type": architecture_type
},
{
"autoware_launch_file": autoware_launch_file
},
{
"autoware_launch_package": autoware_launch_package
},
{
"initialize_duration": initialize_duration
},
{
"launch_autoware": launch_autoware
},
{
"port": port
},
{
"record": record
},
{
"sensor_model": sensor_model
},
{
"vehicle_model": vehicle_model
},
]
def description():
return get_package_share_directory(
vehicle_model.perform(context) + "_description")
if vehicle_model.perform(context):
parameters.append(description() +
"/config/vehicle_info.param.yaml")
parameters.append(description() +
"/config/simulator_model.param.yaml")
return parameters
return [
# fmt: off
DeclareLaunchArgument("architecture_type",
default_value=architecture_type),
DeclareLaunchArgument("autoware_launch_file",
default_value=autoware_launch_file),
DeclareLaunchArgument("autoware_launch_package",
default_value=autoware_launch_package),
DeclareLaunchArgument("global_frame_rate",
default_value=global_frame_rate),
DeclareLaunchArgument("global_real_time_factor",
default_value=global_real_time_factor),
DeclareLaunchArgument("global_timeout", default_value=global_timeout),
DeclareLaunchArgument("launch_autoware",
default_value=launch_autoware),
DeclareLaunchArgument("launch_rviz", default_value=launch_rviz),
DeclareLaunchArgument("output_directory",
default_value=output_directory),
DeclareLaunchArgument("scenario", default_value=scenario),
DeclareLaunchArgument("sensor_model", default_value=sensor_model),
DeclareLaunchArgument("sigterm_timeout",
default_value=sigterm_timeout),
DeclareLaunchArgument("vehicle_model", default_value=vehicle_model),
DeclareLaunchArgument("workflow", default_value=workflow),
# fmt: on
Node(
package="scenario_test_runner",
executable="scenario_test_runner",
namespace="simulation",
name="scenario_test_runner",
output="screen",
on_exit=Shutdown(),
arguments=[
# fmt: off
"--global-frame-rate",
global_frame_rate,
"--global-real-time-factor",
global_real_time_factor,
"--global-timeout",
global_timeout,
"--output-directory",
output_directory,
"--scenario",
scenario,
"--workflow",
workflow,
# fmt: on
],
),
Node(
package="simple_sensor_simulator",
executable="simple_sensor_simulator_node",
namespace="simulation",
name="simple_sensor_simulator",
output="screen",
parameters=[{
"port": port
}],
),
LifecycleNode(
package="openscenario_interpreter",
executable="openscenario_interpreter_node",
namespace="simulation",
name="openscenario_interpreter",
output="screen",
parameters=make_parameters(),
# on_exit=Shutdown(),
),
Node(
package="openscenario_visualization",
executable="openscenario_visualization_node",
namespace="simulation",
name="openscenario_visualizer",
output="screen",
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output={
"stderr": "log",
"stdout": "log"
},
condition=IfCondition(launch_rviz),
arguments=[
"-d",
str(
# Path(get_package_share_directory("scenario_test_runner"))
Path(get_package_share_directory("traffic_simulator")) /
"config/scenario_simulator_v2.rviz"),
],
),
]
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():
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():
use_sim_time = True
map_yaml_file = os.getenv('TEST_MAP')
world = os.getenv('TEST_WORLD')
bringup_package = get_package_share_directory('nav2_bringup')
params_file = os.path.join(bringup_package, 'params/nav2_params.yaml')
astar = (os.getenv('ASTAR').lower() == 'true')
bt_navigator_install_path = get_package_prefix('nav2_bt_navigator')
bt_navigator_xml = os.path.join(bt_navigator_install_path,
'behavior_trees',
os.getenv('BT_NAVIGATOR_XML'))
return LaunchDescription([
launch.actions.SetEnvironmentVariable(
'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
# Launch gazebo server for simulation
launch.actions.ExecuteProcess(cmd=[
'gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world
],
output='screen'),
# Launch navigation2 nodes
launch_ros.actions.Node(package='tf2_ros',
node_executable='static_transform_publisher',
output='screen',
arguments=[
'0', '0', '0', '0', '0', '0',
'base_footprint', 'base_link'
]),
launch_ros.actions.Node(
package='tf2_ros',
node_executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'base_link',
'base_scan']),
launch_ros.actions.Node(package='nav2_map_server',
node_executable='map_server',
node_name='map_server',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'yaml_filename': map_yaml_file
}]),
launch_ros.actions.Node(package='nav2_amcl',
node_executable='amcl',
node_name='amcl',
output='screen',
parameters=[params_file]),
launch_ros.actions.Node(package='nav2_controller',
node_executable='controller_server',
output='screen',
parameters=[params_file]),
launch_ros.actions.Node(package='nav2_planner',
node_executable='planner_server',
node_name='planner_server',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'use_astar': astar
}]),
launch_ros.actions.Node(package='nav2_recoveries',
node_executable='recoveries_server',
node_name='recoveries_server',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}]),
launch_ros.actions.Node(package='nav2_bt_navigator',
node_executable='bt_navigator',
node_name='bt_navigator',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'bt_xml_filename': bt_navigator_xml
}]),
launch_ros.actions.Node(package='nav2_lifecycle_manager',
node_executable='lifecycle_manager',
node_name='lifecycle_manager',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'node_names': [
'map_server', 'amcl',
'controller_server', 'planner_server',
'recoveries_server', 'bt_navigator'
]
}, {
'autostart': True
}]),
])
def launch(launch_descriptor, argv):
ld = launch_descriptor
package = 'turtlebot2_drivers'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='kobuki_node')],
name='kobuki_node',
exit_handler=restart_exit_handler,
)
package = 'astra_camera'
ld.add_process(
cmd=[
get_executable_path(package_name=package, executable_name='astra_camera_node'),
'-dw', '320', '-dh', '240', '-C', '-I'],
name='astra_camera_node',
exit_handler=restart_exit_handler,
)
package = 'depthimage_to_pointcloud2'
ld.add_process(
cmd=[
get_executable_path(
package_name=package, executable_name='depthimage_to_pointcloud2_node')],
name='depthimage_to_pointcloud2_node',
exit_handler=restart_exit_handler,
)
package = 'tf2_ros'
ld.add_process(
# The XYZ/Quat numbers for base_link -> camera_rgb_frame are taken from the
# turtlebot URDF in
# https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
cmd=[
get_executable_path(
package_name=package, executable_name='static_transform_publisher'),
'-0.087', '-0.0125', '0.287',
'0', '0', '0', '1',
'base_link',
'camera_rgb_frame'
],
name='static_tf_pub_base_rgb',
exit_handler=restart_exit_handler,
)
package = 'tf2_ros'
ld.add_process(
# The XYZ/Quat numbers for camera_rgb_frame -> camera_depth_frame are taken from the
# turtlebot URDF in
# https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro
cmd=[
get_executable_path(
package_name=package, executable_name='static_transform_publisher'),
'0', '0.0250', '0',
'0', '0', '0', '1',
'camera_rgb_frame',
'openni_depth_optical_frame'
],
name='static_tf_pub_rgb_depth',
exit_handler=restart_exit_handler,
)
package = 'joy'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='joy_node')],
name='joy_node',
exit_handler=restart_exit_handler,
)
package = 'teleop_twist_joy'
ld.add_process(
cmd=[get_executable_path(package_name=package, executable_name='teleop_node')],
name='teleop_node',
exit_handler=restart_exit_handler,
)
package = 'cartographer_ros'
turtlebot2_cartographer_prefix = get_package_share_directory('turtlebot2_cartographer')
cartographer_config_dir = os.path.join(turtlebot2_cartographer_prefix, 'configuration_files')
ld.add_process(
cmd=[
get_executable_path(package_name=package, executable_name='cartographer_node'),
'-configuration_directory', cartographer_config_dir,
'-configuration_basename', 'turtlebot_2d.lua'
],
name='cartographer_node',
exit_handler=restart_exit_handler,
)
return ld
