def generate_launch_description():
node_list = [ # required nodes
Node(
package="plane",
namespace="plane",
executable="transmitter",
name="msg_transmitter",
parameters=[
{"pth_top": "pth_msg"},
{"gps_top": "gps_fix"},
{"gcu_addr": "13A20041D17945"},
],
)
]
sensor_descriptions = {
# key: device path to match
# value: node description to form from matched device path
# value[0](**value[1](matched_path)) constructs the node
'/dev/xbee*': (Node, lambda f: {
"package":"xbee_uav",
"namespace":"plane",
"executable":"radio",
"name":"xbee_radio",
"parameters":[
{"xbee_port": f},
{"xbee_baud": 9600},
]}), # dont have more than one radio
'/dev/gps*': (Node, lambda f: {
"package":"gps",
"namespace":"plane",
"executable":"neo_gps",
"name":"gps",
"parameters":[
{"gps_baud": 9600},
{"gps_top": "gps_fix"},
{"gps_port": f},
]}),
'/dev/pth*': (Node, lambda f: {
"package":"pth",
"namespace":"plane",
"executable":"pth_timeref",
"name":"pth",
"parameters":[
{"pth_top": "pth_msg"},
{"pth_port": f},
]}),
'/dev/daq*': (ComposableNodeContainer, lambda f: {
"name":"uldaq_container",
"namespace":"plane",
"package":"rclcpp_components",
"executable":"component_container",
"composable_node_descriptions":[
ComposableNode(
package="uldaq",
plugin="uldaq_ros::UldaqPublisher",
name="uldaq_publisher",
parameters=[
{
"v_range": 5,
"chan_num": 8,
"rate": 1000,
}
],
)
],
"output":"screen",
"emulate_tty":True,
})
}
for dev_path, node_construct in sensor_descriptions.items():
for dev_path_match in glob.glob(dev_path):
# Inject this found path into the parameters
# and then pass the parameters to the correct
# ROS node type.
node_list.append(
node_construct[0](**node_construct[1](dev_path_match))
)
return LaunchDescription(node_list)
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
package_name = "gcamp_gazebo"
robot_file = "skidbot.urdf"
rviz_file = "test.rviz"
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
os.path.join(get_package_share_directory("gazebo_ros"), "launch"),
"/gazebo.launch.py",
]
),
)
urdf = os.path.join(get_package_share_directory(package_name), "urdf", robot_file)
rviz = os.path.join(get_package_share_directory(package_name), "rviz", rviz_file)
xml = open(urdf, "r").read()
xml = xml.replace('"', '\\"')
xacro_file = os.path.join(
get_package_share_directory(package_name), "urdf", robot_file
)
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],
# )
with open(urdf, 'r') as infp:
robot_desc = infp.read()
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
arguments=[urdf],
# parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
)
joint_state_publisher_node = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
)
joint_state_publisher_gui_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui',
condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
spawn_entity = Node(
package="gazebo_ros",
executable="spawn_entity.py",
arguments=["-topic", "robot_description", "-entity"],
output="screen",
)
return LaunchDescription(
[
DeclareLaunchArgument(name='gui', default_value='True',
description='Flag to enable joint_state_publisher_gui'),
DeclareLaunchArgument(name='rvizconfig', default_value=rviz,
description='Absolute path to rviz config file'),
DeclareLaunchArgument(name='model', default_value=urdf,
description='Absolute path to robot urdf file'),
robot_state_publisher_node,
joint_state_publisher_node,
joint_state_publisher_gui_node,
rviz_node,
# gazebo,
# spawn_entity,
]
)
def generate_launch_description():
# Define LaunchDescription variable
ld = LaunchDescription()
# use:
# - 'zed' for "ZED" camera
# - 'zedm' for "ZED mini" camera
# - 'zed2' for "ZED2" camera
camera_model = 'zed2'
# Camera name
camera_name = 'zed2'
# URDF file to be loaded by Robot State Publisher
urdf = os.path.join(get_package_share_directory('zed_wrapper'), 'urdf',
camera_model + '.urdf')
# ZED Configurations to be loaded by ZED Node
config_common = os.path.join(get_package_share_directory('zed_wrapper'),
'config', 'common.yaml')
config_camera = os.path.join(get_package_share_directory('zed_wrapper'),
'config', camera_model + '.yaml')
# Set LOG format
os.environ[
'RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time} [{name}] [{severity}] {message}'
# Robot State Publisher node
rsp_node = Node(
package='robot_state_publisher',
node_namespace="/" + camera_name,
node_executable='robot_state_publisher',
node_name=camera_name + '_state_publisher',
output='screen',
arguments=[urdf],
)
zed_node_comp = ComposableNode(
package='zed_components',
node_plugin='stereolabs::ZedCamera',
node_namespace='/' + camera_name,
node_name='zed_node',
parameters=[
config_common, # Common parameters
config_camera, # Camera related parameters
])
zed_cvt_node_comp = ComposableNode(
package='zed_rgb_convert_component',
node_plugin='stereolabs::ZedRgbCvtComponent',
node_namespace='/' + camera_name,
node_name='zed_cvt_node',
remappings=[('/' + camera_name + "/zed_image_4ch",
'/' + camera_name + "/zed_node/rgb/image_rect_color"),
('/' + camera_name + "/camera_info",
'/' + camera_name + "/zed_node/rgb/camera_info")])
"""Generate launch description with multiple components."""
container = ComposableNodeContainer(
package='rclcpp_components',
node_namespace="/" + camera_name,
node_name='zed_container',
output='screen',
node_executable='component_container',
composable_node_descriptions=[
zed_node_comp,
zed_cvt_node_comp,
],
parameters=[
config_common, # Common parameters
config_camera, # Camera related parameters
])
# Add nodes to LaunchDescription
ld.add_action(rsp_node)
ld.add_action(container)
return ld
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
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')
autostart = LaunchConfiguration('autostart')
use_composition = LaunchConfiguration('use_composition')
# 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,
'yaml_filename': map_yaml_file
}
configured_params = RewrittenYaml(source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True)
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_autostart_cmd = DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack')
declare_use_composition_cmd = DeclareLaunchArgument(
'use_composition',
default_value='True',
description='Whether to use composed bringup')
# 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
}.items()),
Node(condition=IfCondition(use_composition),
package='nav2_bringup',
executable='composed_bringup',
output='screen',
parameters=[configured_params, {
'autostart': autostart
}],
remappings=remappings),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'navigation_launch.py')),
condition=IfCondition(PythonExpression(['not ', use_composition])),
launch_arguments={
'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file
}.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_use_composition_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd_group)
return ld
def generate_launch_description():
return LaunchDescription([
# Arguments
DeclareLaunchArgument(name='namespace',
default_value='diffbot2',
description='Node namespace'),
DeclareLaunchArgument(name='robot_name',
default_value=LaunchConfiguration('namespace'),
description='Robot Name'),
DeclareLaunchArgument(name='server_only',
default_value='false',
description='No gui, only server'),
# ign gazebo
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
_path([
FindPackageShare('ros_ign_gazebo'), 'launch',
'ign_gazebo.launch.py'
])),
launch_arguments={
'ign_args': [
'-r ',
PythonExpression([
'"-s " if "true" == "',
LaunchConfiguration('server_only'), '" else ""'
]),
_path([
FindPackageShare('diffbot2_simulation'), 'worlds',
'diffbot2_default.sdf'
])
]
}.items()),
# diffbot2_description
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
_path([
FindPackageShare('diffbot2_description'), 'launch',
'spawn_robot.launch.py'
])),
launch_arguments={'namespace':
LaunchConfiguration('namespace')}.items()),
# ros ign bridge
Node(package='ros_ign_bridge',
namespace=LaunchConfiguration('namespace'),
executable='parameter_bridge',
arguments=[
[
'/model/',
LaunchConfiguration('namespace'),
'/cmd_vel@geometry_msgs/msg/[email protected]'
],
[
'/model/',
LaunchConfiguration('namespace'),
'/odometry@nav_msgs/msg/[email protected]'
]
],
output='screen',
remappings=[
(['/model/',
LaunchConfiguration('namespace'), '/cmd_vel'],
['/', LaunchConfiguration('namespace'), '/cmd_vel']),
(['/model/',
LaunchConfiguration('namespace'), '/odometry'],
['/', LaunchConfiguration('namespace'), '/odometry'])
]),
# robot spawn
Node(package='ros_ign_gazebo',
namespace=LaunchConfiguration('namespace'),
executable='create',
arguments=[
'-world', 'default', '-string',
_xacro_load([
FindPackageShare('diffbot2_description'), 'urdf',
'diffbot2.xacro'
]), '-name',
LaunchConfiguration('robot_name'), '-allow_renaming', 'true',
'-z', '1'
],
output='screen'),
])
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
urdf_system = xacro_to_urdf("ros2_assembly", "urdf", "system.urdf.xacro")
urdf_sweepee = xacro_to_urdf("ros2_assembly", "urdf/sweepee", "sweepee.urdf.xacro")
urdf_ur5 = xacro_to_urdf("ros2_assembly", "urdf", "ur5.urdf.xacro")
urdf_ur10 = xacro_to_urdf("ros2_assembly", "urdf", "ur10.urdf.xacro")
# urdf_env = xacro_to_urdf("ros2_assembly", "urdf", "env.urdf.xacro")
sweepee_description_config = ""
with open(urdf_sweepee, 'r') as file:
sweepee_description_config = file.read()
sweepee_description = {'robot_description' : sweepee_description_config}
ur5_description_config = ""
with open(urdf_ur5, 'r') as file:
ur5_description_config = file.read()
ur5_description = {'robot_description' : ur5_description_config}
ur10_description_config = ""
with open(urdf_ur10, 'r') as file:
ur10_description_config = file.read()
ur10_description = {'robot_description' : ur10_description_config}
robot_description_config = ""
with open(urdf_system, 'r') as file:
robot_description_config = file.read()
system_description = {'robot_description' : robot_description_config,
'robot_description_ur10' : ur10_description_config,
'robot_description_ur5' : ur5_description_config,
'robot_description_sweepee' : sweepee_description_config,}
# env_description_config = ""
# with open(urdf_env, 'r') as file:
# env_description_config = file.read()
# env_description = {'env_description' : env_description_config}
rviz_config_file = "/home/kolmogorov/ros2/ros2_extra/src/ros2_assembly/rviz/model.rviz"
param_substitutions = {
'use_sim_time': 'true',
'yaml_filename': '/home/kolmogorov/ros2/ros2_extra/src/ros2_assembly/map/second.yaml'}
configured_params = RewrittenYaml(
source_file=os.path.join(get_package_share_directory("ros2_assembly"), "config", "map.yaml"),
root_key='/',
param_rewrites=param_substitutions,
convert_types=True)
rviz2_node = Node(
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
output='screen',
arguments=['-d', rviz_config_file],
parameters=[system_description])
param_node = Node(
package='ros2_assembly',
node_executable='param_node',
node_name='param_node',
output='screen',
parameters=[system_description])
map_serv_node = Node(
package='nav2_map_server',
node_executable='map_server',
node_name='map_server',
output='screen',
parameters=[configured_params])
rs_sweepee = Node(
package='robot_state_publisher',
node_executable='robot_state_publisher',
node_name='robot_state_publisher_sweepee',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
node_namespace='sweepee',
arguments=[urdf_sweepee])
rs_ur10 = Node(
package='robot_state_publisher',
node_executable='robot_state_publisher',
node_name='robot_state_publisher_ur10',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
node_namespace='ur10',
arguments=[urdf_ur10])
rs_ur5 = Node(
package='robot_state_publisher',
node_executable='robot_state_publisher',
node_name='robot_state_publisher_ur5',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
node_namespace='ur5',
arguments=[urdf_ur5])
fk_ur10 = Node(
package='fake_joint_driver',
node_executable='fake_joint_driver_node',
node_namespace ='ur10',
#node_name='fake_joint_driver_node_try',
parameters=[os.path.join(get_package_share_directory("ros2_assembly"), "config", "ur10_controllers.yaml"),
os.path.join(get_package_share_directory("ros2_assembly"), "config", "ur10_start_positions.yaml"),
ur10_description]
)
fk_sweepee = Node(
package='fake_joint_driver',
node_executable='fake_joint_driver_node',
node_namespace ='sweepee',
#node_name='fake_joint_driver_node_try',
parameters=[os.path.join(get_package_share_directory("ros2_assembly"), "config", "sweepee_controllers.yaml"),
os.path.join(get_package_share_directory("ros2_assembly"), "config", "sweepee_start_positions.yaml"),
sweepee_description]
)
fk_ur5 = Node(
package='fake_joint_driver',
node_executable='fake_joint_driver_node',
node_namespace ='ur5',
#node_name='fake_joint_driver_node_try',
parameters=[os.path.join(get_package_share_directory("ros2_assembly"), "config", "ur5_controllers.yaml"),
os.path.join(get_package_share_directory("ros2_assembly"), "config", "ur5_start_positions.yaml"),
ur5_description]
)
tf1 = Node(
package='tf2_ros',
node_executable='static_transform_publisher',
node_name='static_transform_publisher_1',
output='screen',
arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'map'])
tf2 = Node(
package='tf2_ros',
node_executable='static_transform_publisher',
node_name='static_transform_publisher_2',
output='screen',
arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'map', 'base_footprint'])
tf_sweepee_ur10 = Node(
package='tf2_ros',
node_executable='static_transform_publisher',
node_name='static_transform_publisher_sweepee_ur10',
output='screen',
arguments=['0.0', '0.0', '0.3', '0.0', '0.0', '0.0', 'base_footprint', 'ur10_base_link'])
moveit_params_file = get_package_share_directory('ros2_assembly') + "/config/moveit_params.yaml"
robot_description_semantic_config = load_file('ros2_assembly', 'config/srdf/ur10.srdf')
ur10_description_semantic = {'robot_description_semantic' : robot_description_semantic_config}
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('ros2_assembly', 'config/ompl_planning.yaml')
# ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml)
kinematics_yaml = load_yaml('ros2_assembly', 'config/kinematics.yaml')
robot_description_kinematics = { 'robot_description_kinematics' : kinematics_yaml }
controllers_yaml = load_yaml('ros2_assembly', 'config/controllers.yaml')
moveit_controllers = { 'moveit_simple_controller_manager' : controllers_yaml }
# MoveItCpp demo executable
run_moveit_cpp_node = Node(node_name='run_moveit_cpp',
package='ros2_assembly',
prefix='xterm -e gdb --args',
node_namespace='ur10',
node_executable='run_moveit_cpp',
output='screen',
parameters=[moveit_params_file,
ur10_description,
ur10_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
moveit_controllers])
ld = LaunchDescription()
# ld.add_action(map_serv_node)
ld.add_action(rviz2_node)
ld.add_action(param_node)
ld.add_action(rs_sweepee)
ld.add_action(rs_ur5)
ld.add_action(rs_ur10)
# ld.add_action(fk_sweepee)
ld.add_action(fk_ur10)
ld.add_action(fk_ur5)
ld.add_action(tf1)
ld.add_action(tf2)
ld.add_action(tf_sweepee_ur10)
ld.add_action(run_moveit_cpp_node)
return ld
def generate_launch_description():
# planning_context
robot_description_config = load_file('moveit_resources_panda_description', 'urdf/panda.urdf')
robot_description = {'robot_description' : robot_description_config}
robot_description_semantic_config = load_file('moveit_resources_panda_moveit_config', 'config/panda.srdf')
robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config}
kinematics_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/kinematics.yaml')
robot_description_kinematics = { 'robot_description_kinematics' : kinematics_yaml }
# Planning Functionality
ompl_planning_pipeline_config = { 'move_group' : {
'planning_plugin' : 'ompl_interface/OMPLPlanner',
'request_adapters' : """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""" ,
'start_state_max_bounds_error' : 0.1 } }
ompl_planning_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/ompl_planning.yaml')
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
# Trajectory Execution Functionality
controllers_yaml = load_yaml('run_move_group', '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('run_move_group') + "/launch/run_move_group.rviz"
rviz_node = Node(package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config_file],
parameters=[robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml])
# Static TF
static_tf = Node(package='tf2_ros',
executable='static_transform_publisher',
name='static_transform_publisher',
output='log',
arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'panda_link0'])
# Publish TF
robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='both',
parameters=[robot_description])
# Fake joint driver
fake_joint_driver_node = Node(package='fake_joint_driver',
executable='fake_joint_driver_node',
parameters=[{'controller_name': 'panda_arm_controller'},
os.path.join(get_package_share_directory("run_move_group"), "config", "panda_controllers.yaml"),
os.path.join(get_package_share_directory("run_move_group"), "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')
return LaunchDescription([ rviz_node, static_tf, robot_state_publisher, run_move_group_node, fake_joint_driver_node, mongodb_server_node ])
def generate_launch_description():
# Get the launch directory
jemalloc_env = SetEnvironmentVariable(
'LD_PRELOAD', '/usr/lib/x86_64-linux-gnu/libjemalloc.so.2')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
gb_nav_dir = get_package_share_directory('gb_navigation')
gb_nav_launch_dir = os.path.join(gb_nav_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')
default_bt_xml_file = LaunchConfiguration('default_bt_xml_file')
autostart = LaunchConfiguration('autostart')
cmd_vel_topic = LaunchConfiguration('cmd_vel_topic')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_rviz = LaunchConfiguration('use_rviz')
# TODO(orduno) Remove once `PushNodeRemapping` is resolved
# https://github.com/ros2/launch_ros/issues/56
remappings = [((namespace, '/tf'), '/tf'),
((namespace, '/tf_static'), '/tf_static'), ('/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(gb_nav_dir, 'maps/robocup2021', 'map.yaml'),
description='Full path to map 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(gb_nav_dir, 'params',
'nav2_tiago_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
#declare_params_file_cmd = DeclareLaunchArgument(
# 'params_file',
# default_value=os.path.join(nav2_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_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='true',
description='Automatically startup the nav2 stack')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(gb_nav_dir, 'rviz', 'nav2_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
declare_cmd_vel_topic_cmd = DeclareLaunchArgument(
'cmd_vel_topic',
default_value='nav_vel',
description='Command velocity topic')
start_rviz_cmd = Node(condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
arguments=['-d', rviz_config_file],
output='log',
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(gb_nav_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':
default_bt_xml_file,
'autostart': autostart,
'cmd_vel_topic': cmd_vel_topic
}.items())
odom_2_world_cmd = Node(package="tf2_ros",
executable="static_transform_publisher",
arguments=[
"2.9", "2.38", "0.0", "1.57", "0.0", "0.0",
"odom", "world"
])
# PDDL Actions
move_1_cmd = Node(package='plansys2_bt_actions',
executable='bt_action_node',
name='move_1',
namespace=namespace,
output='screen',
parameters=[
gb_nav_dir + '/config/params.yaml', {
'action_name':
'move',
'bt_xml_file':
gb_nav_dir + '/behavior_trees_xml/move.xml'
}
])
search_1_cmd = Node(package='plansys2_bt_actions',
executable='bt_action_node',
name='search_1',
namespace=namespace,
output='screen',
parameters=[
gb_nav_dir + '/config/params.yaml', {
'action_name':
'search',
'bt_xml_file':
gb_nav_dir + '/behavior_trees_xml/search.xml'
}
])
# Create the launch description and populate
ld = LaunchDescription()
ld.add_action(jemalloc_env)
# 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_rviz_cmd)
ld.add_action(declare_cmd_vel_topic_cmd)
# Add any conditioned actions
ld.add_action(start_rviz_cmd)
ld.add_action(odom_2_world_cmd)
# Add other nodes and processes we need
#ld.add_action(exit_event_handler)
# Add PDDL Actions
ld.add_action(move_1_cmd)
ld.add_action(search_1_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd)
return ld
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='spawn_robot',
node_executable='spawn_robot',
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',
node_executable='robot_state_publisher',
node_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(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', 'nav2_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_CONSOLE_STDOUT_LINE_BUFFERED', '1'), )
ld.add_action(start_gazebo_cmd)
for spawn_robot in spawn_robots_cmds:
ld.add_action(spawn_robot)
for state_pub in robot_state_pubs_cmds:
ld.add_action(state_pub)
for nav_instance in nav_instances_cmds:
ld.add_action(nav_instance)
return ld
def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType):
# Get URDF and SRDF
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
joint_limits_yaml = {
"robot_description_planning":
load_yaml("moveit_resources_panda_moveit_config",
"config/joint_limits.yaml")
}
# Get parameters for the Pose Tracking node
pose_tracking_yaml = load_yaml("moveit_servo",
"config/pose_tracking_settings.yaml")
pose_tracking_params = {"moveit_servo": pose_tracking_yaml}
# Get parameters for the Servo node
servo_yaml = load_yaml("moveit_servo",
"config/panda_simulated_config_pose_tracking.yaml")
servo_params = {"moveit_servo": servo_yaml}
kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/kinematics.yaml")
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_broadcaster"]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner {}".format(controller)
],
shell=True,
output="screen",
)
]
# Component nodes for tf and Servo
test_container = ComposableNodeContainer(
name="test_pose_tracking_container",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=[
ComposableNode(
package="robot_state_publisher",
plugin="robot_state_publisher::RobotStatePublisher",
name="robot_state_publisher",
parameters=[robot_description],
),
ComposableNode(
package="tf2_ros",
plugin="tf2_ros::StaticTransformBroadcasterNode",
name="static_tf2_broadcaster",
parameters=[{
"/child_frame_id": "panda_link0",
"/frame_id": "world"
}],
),
],
output="screen",
)
pose_tracking_gtest = launch_ros.actions.Node(
executable=PathJoinSubstitution(
[LaunchConfiguration("test_binary_dir"), gtest_name]),
parameters=[
robot_description,
robot_description_semantic,
pose_tracking_params,
servo_params,
kinematics_yaml,
joint_limits_yaml,
],
output="screen",
)
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(
name="test_binary_dir",
description="Binary directory of package "
"containing test executables",
),
ros2_control_node,
test_container,
TimerAction(period=2.0, actions=[pose_tracking_gtest]),
launch_testing.actions.ReadyToTest(),
] + load_controllers), {
"test_container": test_container,
"servo_gtest": pose_tracking_gtest,
"ros2_control_node": ros2_control_node,
}
def generate_launch_description():
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/kinematics.yaml")
robot_description_kinematics = {
"robot_description_kinematics": kinematics_yaml
}
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters":
"""default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
"moveit_resources_panda_moveit_config",
"config/panda_controllers.yaml")
moveit_controllers = {
"moveit_simple_controller_manager":
moveit_simple_controllers_yaml,
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
## BEGIN_SUB_TUTORIAL add_config
## * Add a dictionary with the warehouse_ros options
warehouse_ros_config = {
# For warehouse_ros_sqlite
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
"warehouse_host": "/path/to/my/warehouse_db.sqlite",
# For warehouse_ros_mongodb use the following instead
# "warehouse_port": 33829,
# "warehouse_host": "localhost",
# "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection",
}
## END_SUB_TUTORIAL
# Start the actual move_group node/action server
## BEGIN_SUB_TUTORIAL set_config_move_group
## * Add it to the Move Group config
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,
# here
warehouse_ros_config,
],
)
## END_SUB_TUTORIAL
# RViz
rviz_config_file = (get_package_share_directory("moveit2_tutorials") +
"/launch/move_group.rviz")
## BEGIN_SUB_TUTORIAL set_config_rviz
## * and to the RViz config
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,
# here
warehouse_ros_config,
],
)
## END_SUB_TUTORIAL
# 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.py {}".format(
controller)
],
shell=True,
output="screen",
)
]
# Warehouse mongodb server
## BEGIN_SUB_TUTORIAL start_mongodb_server
## * Optionally, start the MongoDB server (uncomment if necessary)
# mongodb_server_node = Node(
# package="warehouse_ros_mongo",
# executable="mongo_wrapper_ros.py",
# parameters=[
# warehouse_ros_config,
# ],
# output="screen",
# )
## END_SUB_TUTORIAL
return LaunchDescription([
rviz_node,
static_tf,
robot_state_publisher,
run_move_group_node,
ros2_control_node,
mongodb_server_node,
] + load_controllers)
def generate_launch_description():
"""
Launch all nodes defined in the architecture for Ouster Driver integration.
"""
ouster_demo_pkg_prefix = get_package_share_directory(
'ouster_full_pipe_demo')
os1_top_param_file = os.path.join(ouster_demo_pkg_prefix,
'param/os1_top.param.yaml')
os1_middle_left_param_file = os.path.join(
ouster_demo_pkg_prefix, 'param/os1_middle_left.param.yaml')
os1_middle_right_param_file = os.path.join(
ouster_demo_pkg_prefix, 'param/os1_middle_right.param.yaml')
voxel_grid_param_file = os.path.join(ouster_demo_pkg_prefix,
'param/os1_voxel_grid.param.yaml')
ray_ground_classifier_param_file = os.path.join(
ouster_demo_pkg_prefix, 'param/os1_ray_ground.param.yaml')
euclidean_cluster_param_file = os.path.join(
ouster_demo_pkg_prefix, 'param/os1_euclidean_cluster.param.yaml')
pc_filter_transform_param_file = os.path.join(
ouster_demo_pkg_prefix, 'param/os1_point_cloud_transform.param.yaml')
rviz_cfg_path = os.path.join(ouster_demo_pkg_prefix,
'config/ouster_demo.rviz')
# Argumentsavp_
pc_filter_transform_param = DeclareLaunchArgument(
'pc_filter_transform_param_file',
default_value=pc_filter_transform_param_file,
description='Path to config file for Point Cloud Filter/Transform Nodes'
)
os1_top_param = DeclareLaunchArgument(
'os1_top_param_file',
default_value=os1_top_param_file,
description='Path to config file for Ouster OS1 Top')
os1_middle_left_param = DeclareLaunchArgument(
'os1_middle_left_param_file',
default_value=os1_middle_left_param_file,
description='Path to config file for Ouster OS1 Middle Left')
os1_middle_right_param = DeclareLaunchArgument(
'os1_middle_right_param_file',
default_value=os1_middle_right_param_file,
description='Path to config file for Ouster OS1 Middle Right')
voxel_grid_param = DeclareLaunchArgument(
'voxel_grid_param_file',
default_value=voxel_grid_param_file,
description='Path to config file for Voxel Grid Downsample')
ray_ground_classifier_param = DeclareLaunchArgument(
'ray_ground_classifier_param_file',
default_value=ray_ground_classifier_param_file,
description='Path to config file for Ray Ground Classifier')
euclidean_cluster_param = DeclareLaunchArgument(
'euclidean_cluster_param_file',
default_value=euclidean_cluster_param_file,
description='Path to config file for Euclidean Clustering')
with_rviz_param = DeclareLaunchArgument(
'with_rviz',
default_value='True',
description='Launch RVIZ2 in addition to other nodes')
# Nodes
static_transform = Node(
package='tf2_ros',
node_executable='static_transform_publisher',
arguments=['1', '0', '1', '0', '0', '0', 'base_link', 'lidar_top'])
filter_transform_lidar_top = Node(
package='point_cloud_filter_transform_nodes',
node_executable='point_cloud_filter_transform_node_exe',
node_name='filter_transform_lidar_top',
node_namespace='lidar_top',
parameters=[LaunchConfiguration('pc_filter_transform_param_file')],
remappings=[('points_in', '/lidar_top/points')],
)
os1_top = Node(package='ouster_node',
node_executable='ouster_cloud_node_exe',
node_namespace='lidar_top',
parameters=[LaunchConfiguration('os1_top_param_file')])
os1_middle_left = Node(
package='ouster_node',
node_executable='ouster_cloud_node_exe',
node_namespace='middle_left',
parameters=[LaunchConfiguration('os1_middle_left_param_file')])
os1_middle_right = Node(
package='ouster_node',
node_executable='ouster_cloud_node_exe',
node_namespace='lidar_middle_right',
parameters=[LaunchConfiguration('os1_middle_right_param_file')])
voxel_grid = Node(
package='voxel_grid_nodes',
node_executable='voxel_grid_cloud_node_exe',
parameters=[LaunchConfiguration('voxel_grid_param_file')],
remappings=[('points_in', 'lidar_top/points')])
ray_ground_classifier = Node(
package='ray_ground_classifier_nodes',
node_executable='ray_ground_classifier_cloud_node_exe',
parameters=[LaunchConfiguration('ray_ground_classifier_param_file')],
remappings=[('points_in', '/points_downsampled')],
)
euclidean_clustering = Node(
package='euclidean_cluster_nodes',
node_executable='euclidean_cluster_exe',
parameters=[LaunchConfiguration('euclidean_cluster_param_file')],
remappings=[('points_in', '/nonground_points')],
)
rviz2 = Node(package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=['-d', str(rviz_cfg_path)],
condition=IfCondition(LaunchConfiguration('with_rviz')))
return LaunchDescription([
static_transform,
pc_filter_transform_param,
os1_top_param,
# os1_middle_left_param,
# os1_middle_right_param,
voxel_grid_param,
ray_ground_classifier_param,
euclidean_cluster_param,
with_rviz_param,
filter_transform_lidar_top,
os1_top,
# os1_middle_left,
# os1_middle_right,
voxel_grid,
ray_ground_classifier,
euclidean_clustering,
rviz2
])
def generate_launch_description():
pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')
pkg_dolly_ignition = get_package_share_directory('dolly_ignition')
# Gazebo launch
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py'),
),
)
# Spawn dolly
spawn = Node(package='ros_ign_gazebo', node_executable='create',
arguments=[
'-name', 'dolly',
'-x', '5.0',
'-z', '0.46',
'-Y', '1.57',
'-file', os.path.join(pkg_dolly_ignition, 'models', 'dolly_ignition',
'model.sdf')],
output='screen')
# Follow node
follow = Node(
package='dolly_follow',
node_executable='dolly_follow',
output='screen',
remappings=[
('cmd_vel', '/dolly/cmd_vel'),
('laser_scan', '/dolly/laser_scan')
]
)
# Bridge
bridge = Node(
package='ros_ign_bridge',
node_executable='parameter_bridge',
arguments=['/dolly/cmd_vel@geometry_msgs/msg/[email protected]',
'/dolly/laser_scan@sensor_msgs/msg/[email protected]',
'/dolly/odometry@nav_msgs/msg/[email protected]'],
output='screen'
)
# RViz
rviz = Node(
package='rviz2',
node_executable='rviz2',
arguments=['-d', os.path.join(pkg_dolly_ignition, 'rviz', 'dolly_ignition.rviz')],
condition=IfCondition(LaunchConfiguration('rviz'))
)
return LaunchDescription([
DeclareLaunchArgument(
'ign_args',
default_value=[os.path.join(pkg_dolly_ignition, 'worlds', 'station.sdf') +
' -v 2 --gui-config ' +
os.path.join(pkg_dolly_ignition, 'ign', 'gui.config'), ''],
description='Ignition Gazebo arguments'),
DeclareLaunchArgument('rviz', default_value='true',
description='Open RViz.'),
gazebo,
spawn,
follow,
bridge,
rviz
])
def generate_launch_description():
# Boilerplate to fetch the necessary parameter files:
scan_downsampler_param_file = os.path.join(
get_package_share_directory('autoware_demos'),
'param/autoware_academy_demo/scan_downsampler.param.yaml')
ndt_localizer_param_file = os.path.join(
get_package_share_directory('autoware_demos'),
'param/autoware_academy_demo/ndt_localizer.param.yaml')
pc_filter_transform_param_file = os.path.join(
get_package_share_directory('point_cloud_filter_transform_nodes'),
'param/vlp16_sim_lexus_filter_transform.param.yaml')
map_publisher_param_file = os.path.join(
get_package_share_directory('autoware_demos'),
'param/autoware_academy_demo/map_publisher.param.yaml')
rviz_cfg_path = os.path.join(get_package_share_directory('autoware_demos'),
'rviz2/autoware_academy_demo.rviz')
pc_filter_transform_param = DeclareLaunchArgument(
'pc_filter_transform_param_file',
default_value=pc_filter_transform_param_file,
description='Path to config file for Point Cloud Filter/Transform Nodes'
)
scan_downsampler_param = DeclareLaunchArgument(
'scan_downsampler_param_file',
default_value=scan_downsampler_param_file,
description='Path to config file for lidar scan downsampler')
ndt_localizer_param = DeclareLaunchArgument(
'ndt_localizer_param_file',
default_value=ndt_localizer_param_file,
description='Path to config file for ndt localizer')
map_publisher_param = DeclareLaunchArgument(
'map_publisher_param_file',
default_value=map_publisher_param_file,
description='Path to config file for Map Publisher')
# Node definitions.
filter_transform_vlp16_front = Node(
package='point_cloud_filter_transform_nodes',
node_executable='point_cloud_filter_transform_node_exe',
node_name='filter_transform_vlp16_front',
node_namespace='lidar_front',
parameters=[LaunchConfiguration('pc_filter_transform_param_file')],
remappings=[("points_in", "points_raw")])
scan_downsampler = Node(
package='voxel_grid_nodes',
node_executable='voxel_grid_node_exe',
node_namespace='lidar_front',
node_name='voxel_grid_cloud_node',
parameters=[LaunchConfiguration('scan_downsampler_param_file')],
remappings=[("points_in", "points_filtered"),
("points_downsampled", "points_filtered_downsampled")])
ndt_localizer = Node(
package='ndt_nodes',
node_executable='p2d_ndt_localizer_exe',
node_namespace='localization',
node_name='p2d_ndt_localizer_node',
parameters=[LaunchConfiguration('ndt_localizer_param_file')],
remappings=[("points_in", "/lidar_front/points_filtered_downsampled")])
map_publisher = Node(
package='ndt_nodes',
node_executable='ndt_map_publisher_exe',
node_namespace='localization',
parameters=[LaunchConfiguration('map_publisher_param_file')])
rviz2 = Node(
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=['-d', str(rviz_cfg_path)],
)
# Since we don't use an odometry source, odometry frame is statically defined to be
# overlapping with the base_link.
# TODO(yunus.caliskan): To be removed after #476
odom_bl_publisher = Node(
package='tf2_ros',
node_executable='static_transform_publisher',
arguments=["0", "0", "0", "0", "0", "0", "odom", "base_link"])
return LaunchDescription([
map_publisher_param, pc_filter_transform_param, scan_downsampler_param,
ndt_localizer_param, filter_transform_vlp16_front, map_publisher,
scan_downsampler, odom_bl_publisher, ndt_localizer, rviz2
])
def generate_launch_description():
# TODO(jacobperron): Include image_proc launch file when it exists
stereo_cam_name = LaunchConfiguration('stereo_cam_name')
use_stereo_view = LaunchConfiguration('use_stereo_view')
return LaunchDescription([
DeclareLaunchArgument(
name='approximate_sync',
default_value='True',
description=
'Whether to use approximate synchronization of topics. Set to true if '
'the left and right cameras do not produce exactly synced timestamps.'
),
DeclareLaunchArgument(
name='use_system_default_qos',
default_value='False',
description=
'Use the RMW QoS settings for the image and camera info subscriptions.'
),
DeclareLaunchArgument(name='stereo_cam_name',
default_value='/camera',
description='Namespace'),
DeclareLaunchArgument(
name='use_stereo_view',
default_value='True',
description='Choice if stereo_view should be started.'),
ComposableNodeContainer(
package='rclcpp_components',
executable='component_container',
name='stereo_image_proc_container',
namespace='',
composable_node_descriptions=[
ComposableNode(
package='stereo_image_proc',
node_plugin='stereo_image_proc::DisparityNode',
name='disparity_node',
namespace=stereo_cam_name,
remappings=[
('left/image_rect', 'left/image_raw'),
('right/image_rect', 'right/image_raw'),
],
parameters=[{
'approximate_sync':
LaunchConfiguration('approximate_sync'),
'use_system_default_qos':
LaunchConfiguration('use_system_default_qos'),
}]),
ComposableNode(
package='stereo_image_proc',
node_plugin='stereo_image_proc::PointCloudNode',
name='point_cloud_node',
namespace=stereo_cam_name,
remappings=[('left/image_rect_color', 'left/image_raw')],
parameters=[{
'approximate_sync':
LaunchConfiguration('approximate_sync'),
'use_system_default_qos':
LaunchConfiguration('use_system_default_qos'),
}]),
],
),
Node(condition=IfCondition(use_stereo_view),
package='image_view',
executable='stereo_view',
name='stereo_view_node',
arguments=['raw'],
remappings=[
('/stereo/disparity', [stereo_cam_name, '/disparity']),
('/stereo/left/image', [stereo_cam_name, '/left/image_raw']),
('/stereo/right/image', [stereo_cam_name,
'/right/image_raw']),
],
output='screen',
emulate_tty=True),
])
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',
'sub_camera_info_best_effort_not_reliable': 1, # Gazebo camera uses 'best effort'
}]),
# 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():
# Launch Arguments
use_sim_time = LaunchConfiguration('use_sim_time', default=True)
config_rviz2 = LaunchConfiguration(
'config_rviz2',
default=os.path.join(get_package_share_directory('ign_moveit2'),
'launch', 'rviz.rviz'))
log_level = LaunchConfiguration('log_level', default='error')
return LaunchDescription([
# Launch Arguments
DeclareLaunchArgument('use_sim_time',
default_value=use_sim_time,
description="If true, use simulated clock"),
DeclareLaunchArgument('config_rviz2',
default_value=config_rviz2,
description="Path to config for RViz2"),
DeclareLaunchArgument(
'log_level',
default_value=log_level,
description="Log level of all nodes launched by this script"),
# MoveIt2 move_group action server
IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(
get_package_share_directory('panda_moveit2_config'),
'launch', 'move_group_action_server.launch.py')
]),
# Simulation time does not function properly (as of Nov 2020), see https://github.com/AndrejOrsula/ign_moveit2/issues/4
launch_arguments=[('use_sim_time', 'False'),
('config_rviz2', config_rviz2),
('log_level', log_level)]),
# Clock bridge (IGN -> ROS2)
Node(package='ros_ign_bridge',
executable='parameter_bridge',
name='parameter_bridge_block',
output='screen',
arguments=[
'/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
'--ros-args', '--log-level', log_level
],
parameters=[{
'use_sim_time': use_sim_time
}]),
# JointState bridge (IGN -> ROS2)
Node(
package='ros_ign_bridge',
executable='parameter_bridge',
name='parameter_bridge_joint_states',
output='screen',
arguments=[
'/world/default/model/panda/joint_state@sensor_msgs/msg/JointState[ignition.msgs.Model',
'--ros-args', '--log-level', log_level
],
parameters=[{
'use_sim_time': use_sim_time
}],
remappings=[('/world/default/model/panda/joint_state',
'/joint_states')]),
# JointTrajectory bridge (ROS2 -> IGN)
Node(
package='ros_ign_bridge',
executable='parameter_bridge',
name='parameter_bridge_joint_trajectory',
output='screen',
arguments=[
'/joint_trajectory@trajectory_msgs/msg/JointTrajectory]ignition.msgs.JointTrajectory',
'--ros-args', '--log-level', log_level
],
parameters=[{
'use_sim_time': use_sim_time
}]),
# JointTrajectoryProgress bridge (IGN -> ROS2)
Node(
package='ros_ign_bridge',
executable='parameter_bridge',
name='parameter_bridge_joint_trajectory_progreess',
output='screen',
arguments=[
'/joint_trajectory_progress@std_msgs/msg/Float32[ignition.msgs.Float',
'--ros-args', '--log-level', log_level
],
parameters=[{
'use_sim_time': use_sim_time
}])
])
def generate_launch_description():
# Each ballast weight weighs 0.19kg
model_params = {
'mdl_mass': 11.1,
'mdl_volume': 0.0111335,
'mdl_fluid_density': 997.0,
# After 1st round drag tests
'mdl_thrust_scale': 0.17,
'mdl_drag_coef_f': 0.80,
}
# Must match camera name in URDF file
# Should also match the camera name in the camera info 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', 'orca.urdf')
orca_driver_path = get_package_share_directory('orca_driver')
map_path = os.path.join(orca_driver_path, 'maps', 'simple_map.yaml')
# Run pose_filter_node or not
filter_poses = False
# Optionally build and use a map
build_map = False
use_built_map = True
if build_map:
vmap_node_params = {
# Publish marker /tf
'psm_publish_tfs': 1,
# 6 aluminum markers: 0.165
'map_marker_length': 0.165,
# Don't load a map
'map_load_filename': '',
# Save the map
'map_save_filename': 'sam_map',
# Map initialization
'map_init_style': 1,
'map_init_id': 0,
'map_init_pose_x': 2.0,
'map_init_pose_y': 0.0,
'map_init_pose_z': -0.16,
'map_init_pose_roll': 0.0,
'map_init_pose_pitch': 1.570796,
'map_init_pose_yaw': 1.570796 * 2,
}
else:
vmap_node_params = {
# Publish marker /tf
'psm_publish_tfs': 1,
# Load map
'map_load_filename': 'sam_map' if use_built_map else map_path,
# Don't save the map
'map_save_filename': '',
}
rov_node_params = {
# ros2 run orca_base set_pid.py /rov_node rov_pressure_pid_ 0.0001 6 no_overshoot
'rov_pressure_pid_kp': 0.00002,
'rov_pressure_pid_ki': 0.0000066667,
'rov_pressure_pid_kd': 0.00004002,
'planner_target_z': -0.2,
}
rov_node_params.update(model_params)
fp_node_params = {
# Publish map=>base tf if we're not running a filter
'publish_tf': not filter_poses,
}
pose_filter_node_params = {
'predict_accel': True,
'predict_accel_control': True,
'predict_accel_drag': True,
'predict_accel_buoyancy': True,
'filter_baro': True, # Fuse depth
'filter_fcam': True,
'publish_tf': True, # Publish map=>base tf
# How far in front of a marker is a good pose?
'good_pose_dist': 2.0,
# Process noise, similar to /depth noise
'ukf_process_noise': 0.0004,
# Turn outlier detection off
'ukf_outlier_distance': -1.0,
}
pose_filter_node_params.update(model_params)
auv_node_params = {
# Timer is stable w/ or w/o filter:
'loop_driver': 0,
# If we're not running a filter, then override depth in auv_node
'depth_override': not filter_poses,
# 'depth_override': False,
# FT3: turn pids off while tuning dead reckoning
'auv_pid_enabled': True,
# A little tuning in the field
'auv_yaw_pid_kd': 0.5,
# FT3: use the estimate yaw while PID tuning
'control_use_est_yaw': False,
# Slow down while tuning
# 'auv_xy_accel': 0.05,
# 'auv_xy_velo': 0.2,
# How far in front of a marker is a good pose?
# FT3: set to 3m, this assumes we're always looking at 2+ markers TODO
'good_pose_dist': 3.0,
# Global planner
'global_plan_target_z': -0.1,
'global_plan_allow_mtm': True,
# Do not allow waypoints
'pose_plan_waypoints': False,
# If xy distance is > this, then build a long plan (rotate, run, rotate)
# Otherwise build a short plan (move in all DoF at once)
# FT3: never replan
# 'pose_plan_max_short_plan_xy': 0.5,
'pose_plan_max_short_plan_xy': 99.0,
# How close to the markers should we get?
'pose_plan_target_dist': 0.8,
'mtm_plan_target_dist': 1.5,
}
auv_node_params.update(model_params)
all_entities = [
# Publish static joints
Node(package='robot_state_publisher',
node_executable='robot_state_publisher',
output='log',
arguments=[urdf_path]),
# Decode h264 stream
Node(
package='image_transport',
node_executable='republish',
output='screen',
node_name='republish_node',
node_namespace=camera_name,
arguments=[
'h264', # Input
'raw' # Output
],
remappings=[
('in', 'image_raw'),
('in/compressed', 'image_raw/compressed'),
('in/theora', 'image_raw/theora'),
('in/h264', 'image_raw/h264'),
('out', 'repub_raw'),
('out/compressed', 'repub_raw/compressed'),
('out/theora', 'repub_raw/theora'),
('out/theora', 'repub_raw/h264'),
]),
# Joystick driver, generates joy messages
Node(
package='joy',
node_executable='joy_node',
output='screen',
node_name='joy_node',
parameters=[{
'dev': '/dev/input/js0' # Update as required
}]),
# Barometer filter
Node(package='orca_filter',
node_executable='baro_filter_node',
output='screen',
parameters=[{
'ukf_Q': True,
}]),
# ROV controller, uses joystick to control the sub
Node(
package='orca_base',
node_executable='rov_node',
output='screen',
node_name='rov_node',
parameters=[rov_node_params],
remappings=[
('barometer', 'filtered_barometer'),
('control',
'rov_control'), # Send control messages to auv_node
]),
# Depth node, turns /barometer messages into /depth messages
Node(package='orca_filter',
node_executable='depth_node',
output='screen',
node_name='depth_node',
parameters=[model_params],
remappings=[
('barometer', 'filtered_barometer'),
('fp', '/' + camera_name + '/fp'),
]),
# Publish, and possibly build, a map
Node(package='fiducial_vlam',
node_executable='vmap_main',
output='screen',
node_name='vmap_node',
parameters=[vmap_node_params]),
# Localize against the map
Node(
package='fiducial_vlam',
node_executable='vloc_main',
output='screen',
node_name='vloc_node',
node_namespace=camera_name,
parameters=[{
'psl_camera_frame_id': camera_frame,
# Localize, don't calibrate
'loc_calibrate_not_loocalize': 0,
# 0: OpenCV, 1: GTSAM
# Crashing bug in GTSAM, use OpenCV for now
'loc_camera_sam_not_cv': 0,
# 0: DICT_4x4_50 (default)
# 8: DICT_6X6_250
'loc_aruco_dictionary_id': 0,
# Do not publish tfs
'psl_publish_tfs': 0,
# Camera info is published reliable, not best-effort
'psl_sub_camera_info_best_effort_not_reliable': 0,
# Publish the camera pose, but nothing else
'psl_publish_camera_pose': 1,
'psl_publish_base_pose': 0,
'psl_publish_camera_odom': 0,
'psl_publish_base_odom': 0,
# Keep the existing timestamps
'psl_stamp_msgs_with_current_time': 0,
}],
remappings=[
('image_raw', 'repub_raw'),
]),
# FP node, generate fiducial poses from observations and poses
Node(package='orca_filter',
node_executable='fp_node',
output='screen',
node_name='fp_node',
node_namespace=camera_name,
parameters=[fp_node_params]),
# Annotate image for diagnostics
Node(package='orca_base',
node_executable='annotate_image_node',
output='screen',
node_name='annotate_image_node',
node_namespace=camera_name,
remappings=[
('image_raw', 'repub_raw'),
]),
]
if filter_poses:
all_entities.append(
Node(package='orca_filter',
node_executable='pose_filter_node',
output='screen',
node_name='pose_filter_node',
parameters=[pose_filter_node_params],
remappings=[
('fcam_fp', '/' + camera_name + '/fp'),
]))
all_entities.append(
Node(package='orca_base',
node_executable='auv_node',
output='screen',
node_name='auv_node',
parameters=[auv_node_params],
remappings=[
('filtered_fp', 'filtered_fp'),
('barometer', 'filtered_barometer'),
]))
else:
all_entities.append(
Node(package='orca_base',
node_executable='auv_node',
output='screen',
node_name='auv_node',
parameters=[auv_node_params],
remappings=[
('filtered_fp', '/' + camera_name + '/fp'),
('barometer', 'filtered_barometer'),
]))
return LaunchDescription(all_entities)
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
mecanum_bot_cartographer_prefix = get_package_share_directory('mecanum_bot_cartographer')
cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', default=os.path.join(
mecanum_bot_cartographer_prefix, 'config'))
configuration_basename = LaunchConfiguration('configuration_basename',
default='mecanum_bot_lds_2d.lua')
resolution = LaunchConfiguration('resolution', default='0.05')
publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
rviz_config_dir = os.path.join(get_package_share_directory('mecanum_bot_cartographer'),
'rviz', 'mecanum_bot_cartographer.rviz')
return LaunchDescription([
DeclareLaunchArgument(
'cartographer_config_dir',
default_value=cartographer_config_dir,
description='Full path to config file to load'),
DeclareLaunchArgument(
'configuration_basename',
default_value=configuration_basename,
description='Name of lua file for cartographer'),
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
Node(
package='cartographer_ros',
node_executable='cartographer_node',
node_name='cartographer_node',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
arguments=['-configuration_directory', cartographer_config_dir,
'-configuration_basename', configuration_basename]),
DeclareLaunchArgument(
'resolution',
default_value=resolution,
description='Resolution of a grid cell in the published occupancy grid'),
DeclareLaunchArgument(
'publish_period_sec',
default_value=publish_period_sec,
description='OccupancyGrid publishing period'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/occupancy_grid.launch.py']),
launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution,
'publish_period_sec': publish_period_sec}.items(),
),
Node(
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}],
output='screen'),
])
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')
map_subscribe_transient_local = LaunchConfiguration(
'map_subscribe_transient_local')
lifecycle_nodes = [
'controller_server', 'planner_server', 'recoveries_server',
'bt_navigator', 'waypoint_follower'
]
# 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(
'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(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': lifecycle_nodes
}]),
])
def generate_launch_description():
moveit_config = (
MoveItConfigsBuilder("moveit_resources_panda")
.robot_description(file_path="config/panda.urdf.xacro")
.trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
.to_moveit_configs()
)
## BEGIN_SUB_TUTORIAL add_config
## * Add a dictionary with the warehouse_ros options
warehouse_ros_config = {
# For warehouse_ros_sqlite
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
"warehouse_host": "/path/to/my/warehouse_db.sqlite",
# For warehouse_ros_mongodb use the following instead
# "warehouse_port": 33829,
# "warehouse_host": "localhost",
# "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection",
}
## END_SUB_TUTORIAL
# Start the actual move_group node/action server
## BEGIN_SUB_TUTORIAL set_config_move_group
## * Add it to the Move Group config
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
moveit_config.to_dict(),
# here
warehouse_ros_config,
],
)
## END_SUB_TUTORIAL
# RViz
rviz_config_file = (
get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz"
)
## BEGIN_SUB_TUTORIAL set_config_rviz
## * and to the RViz config
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
moveit_config.planning_pipelines,
moveit_config.joint_limits,
# here
warehouse_ros_config,
],
)
## END_SUB_TUTORIAL
# 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=[moveit_config.robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"ros2_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[moveit_config.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.py {}".format(controller)],
shell=True,
output="screen",
)
]
# Warehouse mongodb server
## BEGIN_SUB_TUTORIAL start_mongodb_server
## * Optionally, start the MongoDB server (uncomment if necessary)
# mongodb_server_node = Node(
# package="warehouse_ros_mongo",
# executable="mongo_wrapper_ros.py",
# parameters=[
# warehouse_ros_config,
# ],
# output="screen",
# )
## END_SUB_TUTORIAL
return LaunchDescription(
[
rviz_node,
static_tf,
robot_state_publisher,
run_move_group_node,
ros2_control_node,
mongodb_server_node,
]
+ load_controllers
)
def generate_launch_description():
map_yaml_file = os.getenv('TEST_MAP')
world = os.getenv('TEST_WORLD')
bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
'behavior_trees',
os.getenv('BT_NAVIGATOR_XML'))
bringup_dir = get_package_share_directory('nav2_bringup')
params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml')
# Replace the default parameter values for testing special features
# without having multiple params_files inside the nav2 stack
context = LaunchContext()
param_substitutions = {}
if (os.getenv('ASTAR') == 'True'):
param_substitutions.update({'use_astar': 'True'})
if (os.getenv('GROOT_MONITORING') == 'True'):
param_substitutions.update({'enable_groot_monitoring': 'True'})
configured_params = RewrittenYaml(
source_file=params_file,
root_key='',
param_rewrites=param_substitutions,
convert_types=True)
new_yaml = configured_params.perform(context)
return LaunchDescription([
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
# Launch gazebo server for simulation
ExecuteProcess(
cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
'--minimal_comms', world],
output='screen'),
# TODO(orduno) Launch the robot state publisher instead
# using a local copy of TB3 urdf file
Node(
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
Node(
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
launch_arguments={'namespace': '',
'use_namespace': 'False',
'map': map_yaml_file,
'use_sim_time': 'True',
'params_file': new_yaml,
'bt_xml_file': bt_navigator_xml,
'autostart': 'True'}.items()),
])
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
ur_type = LaunchConfiguration("ur_type")
robot_ip = LaunchConfiguration("robot_ip")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
prefix = LaunchConfiguration("prefix")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
launch_rviz = LaunchConfiguration("launch_rviz")
launch_servo = LaunchConfiguration("launch_servo")
headless_mode = LaunchConfiguration("headless_mode")
joint_limit_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"]
)
kinematics_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"]
)
physical_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"]
)
visual_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"]
)
script_filename = PathJoinSubstitution(
[FindPackageShare("ur_robot_driver"), "resources", "ros_control.urscript"]
)
input_recipe_filename = PathJoinSubstitution(
[FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"]
)
output_recipe_filename = PathJoinSubstitution(
[FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"]
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
" ",
"robot_ip:=",
robot_ip,
" ",
"joint_limit_params:=",
joint_limit_params,
" ",
"kinematics_params:=",
kinematics_params,
" ",
"physical_params:=",
physical_params,
" ",
"visual_params:=",
visual_params,
" ",
"safety_limits:=",
safety_limits,
" ",
"safety_pos_margin:=",
safety_pos_margin,
" ",
"safety_k_position:=",
safety_k_position,
" ",
"name:=",
# Also ur_type parameter could be used but then the planning group names in yaml
# configs has to be updated!
"ur",
" ",
"script_filename:=",
script_filename,
" ",
"input_recipe_filename:=",
input_recipe_filename,
" ",
"output_recipe_filename:=",
output_recipe_filename,
" ",
"prefix:=",
prefix,
" ",
"use_fake_hardware:=",
use_fake_hardware,
" ",
"fake_sensor_commands:=",
fake_sensor_commands,
" ",
"headless_mode:=",
headless_mode,
" ",
]
)
robot_description = {"robot_description": robot_description_content}
# MoveIt Configuration
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "srdf", moveit_config_file]
),
" ",
"name:=",
# Also ur_type parameter could be used but then the planning group names in yaml
# configs has to be updated!
"ur",
" ",
"prefix:=",
prefix,
" ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
kinematics_yaml = load_yaml("ur_moveit_config", "config/kinematics.yaml")
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
robot_description_planning = {
"robot_description_planning": load_yaml_abs(str(joint_limit_params.perform(context)))
}
# Planning Configuration
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("ur_moveit_config", "config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Configuration
controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
moveit_controllers = {
"moveit_simple_controller_manager": controllers_yaml,
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": False,
"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
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
robot_description_planning,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
# 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",
)
# rviz with moveit configuration
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"]
)
rviz_node = Node(
package="rviz2",
condition=IfCondition(launch_rviz),
executable="rviz2",
name="rviz2_moveit",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
robot_description_kinematics,
robot_description_planning,
],
)
# 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", "base_link"],
)
# Servo node for realtime control
servo_yaml = load_yaml("ur_moveit_config", "config/ur_servo.yaml")
servo_params = {"moveit_servo": servo_yaml}
servo_node = Node(
package="moveit_servo",
condition=IfCondition(launch_servo),
executable="servo_node_main",
parameters=[
servo_params,
robot_description,
robot_description_semantic,
],
output={
"stdout": "screen",
"stderr": "screen",
},
)
nodes_to_start = [move_group_node, mongodb_server_node, rviz_node, static_tf, servo_node]
return nodes_to_start
def generate_launch_description():
arg_show_rviz = DeclareLaunchArgument(
"start_rviz",
default_value="false",
description="start RViz automatically with the launch file",
)
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare("diffbot_description"), "urdf", "diffbot_system.urdf.xacro"]
),
]
)
robot_description = {"robot_description": robot_description_content}
diffbot_diff_drive_controller = PathJoinSubstitution(
[
FindPackageShare("ros2_control_demo_bringup"),
"config",
"diffbot_diff_drive_controller.yaml",
]
)
node_robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[robot_description],
)
controller_manager_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, diffbot_diff_drive_controller],
output={
"stdout": "screen",
"stderr": "screen",
},
)
spawn_dd_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["diffbot_base_controller"],
output="screen",
)
spawn_jsb_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster"],
output="screen",
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("diffbot_description"), "config", "diffbot.rviz"]
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
condition=IfCondition(LaunchConfiguration("start_rviz")),
)
return LaunchDescription(
[
arg_show_rviz,
node_robot_state_publisher,
controller_manager_node,
spawn_dd_controller,
spawn_jsb_controller,
rviz_node,
]
)
def generate_launch_description():
# Get the launch directory
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
nav2_launch_dir = os.path.join(nav2_bringup_dir, 'launch')
pilot_dir = get_package_share_directory('pilot_urjc_bringup')
pilot_launch_dir = os.path.join(pilot_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')
cmd_vel_topic = LaunchConfiguration('cmd_vel_topic')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_rviz = LaunchConfiguration('use_rviz')
# TODO(orduno) Remove once `PushNodeRemapping` is resolved
# https://github.com/ros2/launch_ros/issues/56
remappings = [((namespace, '/tf'), '/tf'),
((namespace, '/tf_static'), '/tf_static'), ('/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(nav2_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(nav2_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='true',
description='Automatically startup the nav2 stack')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(nav2_bringup_dir, 'rviz',
'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
declare_cmd_vel_topic_cmd = DeclareLaunchArgument(
'cmd_vel_topic',
default_value='/cmd_vel',
description='Command velocity topic')
rviz_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(nav2_launch_dir, 'rviz_launch.py')),
condition=IfCondition(use_rviz),
launch_arguments={
'namespace': '',
'use_namespace': 'False',
'rviz_config': rviz_config_file
}.items())
bringup_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(pilot_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,
'cmd_vel_topic': cmd_vel_topic
}.items())
shm_model_path = (get_package_share_directory('pilot_urjc_bringup') +
'/params/pilot_modes.yaml')
# Start as a normal node is currently not possible.
# Path to SHM file should be passed as a ROS parameter.
mode_manager_node = Node(package='system_modes',
executable='mode_manager',
parameters=[{
'modelfile': shm_model_path
}],
output='screen')
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_cmd_vel_topic_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_rviz_cmd)
# Add any conditioned actions
ld.add_action(rviz_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd)
# Add system modes manager
ld.add_action(mode_manager_node)
return ld
def generate_launch_description():
return LaunchDescription([
Node(package='wall_follower',
executable='wall_follower',
name='wall_follower')
])
def generate_launch_description():
# Nav needs TF. Specifically:
# ros2 run tf2_ros static_transform_publisher
# 1 2 3 0.5 0.1 -1.0 odom base_link
# ros2 run tf2_ros static_transform_publisher
# 1 2 3 0.5 0.1 -1.0 map odom
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
# Create the launch configuration variables
slam = LaunchConfiguration('slam')
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
autostart = LaunchConfiguration('autostart')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_rviz = LaunchConfiguration('use_rviz')
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace')
declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_slam_cmd = DeclareLaunchArgument('slam',
default_value='False',
description='Whether run a SLAM')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
# default_value=os.path.join(
# bringup_dir, 'maps', 'hadabot_world.yaml'),
default_value='./hadabot_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(
'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')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join('.', 'rviz', 'hadabot_nav2_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
# Specify the actions
rviz_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'rviz_launch.py')),
condition=IfCondition(use_rviz),
launch_arguments={
'namespace': '',
'use_namespace': 'False',
'rviz_config': rviz_config_file
}.items())
nav2_bringup_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'bringup_launch.py')),
launch_arguments={
'namespace': namespace,
'use_namespace':
use_namespace,
'slam': slam,
'map': map_yaml_file,
'use_sim_time':
use_sim_time,
'params_file': params_file,
'default_bt_xml_filename':
default_bt_xml_filename,
'autostart': autostart
}.items())
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_slam_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_rviz_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(rviz_cmd)
ld.add_action(nav2_bringup_cmd)
# Add turtlesim
ld.add_action(
Node(package='turtlesim',
namespace='turtlesim1',
executable='turtlesim_node',
name='sim',
remappings=[("/turtlesim1/turtle1/cmd_vel", "/cmd_vel")]))
# Add tf2 broadcaster
ld.add_action(
Node(package='hadabot_tf2',
executable='hadabot_tf2_broadcaster',
name='hadabot_tf2_broadbaster_node',
remappings=[("/pose", "/turtlesim1/turtle1/pose")]))
return ld
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
ur_type = LaunchConfiguration("ur_type")
robot_ip = LaunchConfiguration("robot_ip")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
runtime_config_package = LaunchConfiguration("runtime_config_package")
controllers_file = LaunchConfiguration("controllers_file")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
activate_joint_controller = LaunchConfiguration(
"activate_joint_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
headless_mode = LaunchConfiguration("headless_mode")
launch_dashboard_client = LaunchConfiguration("launch_dashboard_client")
use_tool_communication = LaunchConfiguration("use_tool_communication")
tool_parity = LaunchConfiguration("tool_parity")
tool_baud_rate = LaunchConfiguration("tool_baud_rate")
tool_stop_bits = LaunchConfiguration("tool_stop_bits")
tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars")
tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars")
tool_device_name = LaunchConfiguration("tool_device_name")
tool_tcp_port = LaunchConfiguration("tool_tcp_port")
tool_voltage = LaunchConfiguration("tool_voltage")
joint_limit_params = PathJoinSubstitution([
FindPackageShare(description_package), "config", ur_type,
"joint_limits.yaml"
])
kinematics_params = PathJoinSubstitution([
FindPackageShare(description_package), "config", ur_type,
"default_kinematics.yaml"
])
physical_params = PathJoinSubstitution([
FindPackageShare(description_package), "config", ur_type,
"physical_parameters.yaml"
])
visual_params = PathJoinSubstitution([
FindPackageShare(description_package), "config", ur_type,
"visual_parameters.yaml"
])
script_filename = PathJoinSubstitution([
FindPackageShare("ur_robot_driver"), "resources",
"ros_control.urscript"
])
input_recipe_filename = PathJoinSubstitution([
FindPackageShare("ur_robot_driver"), "resources",
"rtde_input_recipe.txt"
])
output_recipe_filename = PathJoinSubstitution([
FindPackageShare("ur_robot_driver"), "resources",
"rtde_output_recipe.txt"
])
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]),
" ",
"robot_ip:=",
robot_ip,
" ",
"joint_limit_params:=",
joint_limit_params,
" ",
"kinematics_params:=",
kinematics_params,
" ",
"physical_params:=",
physical_params,
" ",
"visual_params:=",
visual_params,
" ",
"safety_limits:=",
safety_limits,
" ",
"safety_pos_margin:=",
safety_pos_margin,
" ",
"safety_k_position:=",
safety_k_position,
" ",
"name:=",
ur_type,
" ",
"script_filename:=",
script_filename,
" ",
"input_recipe_filename:=",
input_recipe_filename,
" ",
"output_recipe_filename:=",
output_recipe_filename,
" ",
"prefix:=",
prefix,
" ",
"use_fake_hardware:=",
use_fake_hardware,
" ",
"fake_sensor_commands:=",
fake_sensor_commands,
" ",
"headless_mode:=",
headless_mode,
" ",
"use_tool_communication:=",
use_tool_communication,
" ",
"tool_parity:=",
tool_parity,
" ",
"tool_baud_rate:=",
tool_baud_rate,
" ",
"tool_stop_bits:=",
tool_stop_bits,
" ",
"tool_rx_idle_chars:=",
tool_rx_idle_chars,
" ",
"tool_tx_idle_chars:=",
tool_tx_idle_chars,
" ",
"tool_device_name:=",
tool_device_name,
" ",
"tool_tcp_port:=",
tool_tcp_port,
" ",
"tool_voltage:=",
tool_voltage,
" ",
])
robot_description = {"robot_description": robot_description_content}
initial_joint_controllers = PathJoinSubstitution(
[FindPackageShare(runtime_config_package), "config", controllers_file])
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "rviz", "view_robot.rviz"])
# define update rate
update_rate_config_file = PathJoinSubstitution([
FindPackageShare(runtime_config_package),
"config",
ur_type.perform(context) + "_update_rate.yaml",
])
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
robot_description, update_rate_config_file,
initial_joint_controllers
],
output="screen",
condition=IfCondition(use_fake_hardware),
)
ur_control_node = Node(
package="ur_robot_driver",
executable="ur_ros2_control_node",
parameters=[
robot_description, update_rate_config_file,
initial_joint_controllers
],
output="screen",
condition=UnlessCondition(use_fake_hardware),
)
dashboard_client_node = Node(
package="ur_robot_driver",
condition=IfCondition(launch_dashboard_client)
and UnlessCondition(use_fake_hardware),
executable="dashboard_client",
name="dashboard_client",
output="screen",
emulate_tty=True,
parameters=[{
"robot_ip": robot_ip
}],
)
tool_communication_node = Node(
package="ur_robot_driver",
condition=IfCondition(use_tool_communication),
executable="tool_communication.py",
name="ur_tool_comm",
output="screen",
parameters=[{
"robot_ip": robot_ip,
"tcp_port": tool_tcp_port,
"device_name": tool_device_name,
}],
)
controller_stopper_node = Node(
package="ur_robot_driver",
executable="controller_stopper_node",
name="controller_stopper",
output="screen",
emulate_tty=True,
condition=UnlessCondition(use_fake_hardware),
parameters=[
{
"headless_mode": headless_mode
},
{
"joint_controller_active": activate_joint_controller
},
{
"consistent_controllers": [
"io_and_status_controller",
"force_torque_sensor_broadcaster",
"joint_state_broadcaster",
"speed_scaling_state_broadcaster",
]
},
],
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
condition=IfCondition(launch_rviz),
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster", "--controller-manager",
"/controller_manager"
],
)
io_and_status_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["io_and_status_controller", "-c", "/controller_manager"],
)
speed_scaling_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"speed_scaling_state_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
force_torque_sensor_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"force_torque_sensor_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
forward_position_controller_spawner_stopped = Node(
package="controller_manager",
executable="spawner",
arguments=[
"forward_position_controller", "-c", "/controller_manager",
"--inactive"
],
)
# There may be other controllers of the joints, but this is the initially-started one
initial_joint_controller_spawner_started = Node(
package="controller_manager",
executable="spawner",
arguments=[initial_joint_controller, "-c", "/controller_manager"],
condition=IfCondition(activate_joint_controller),
)
initial_joint_controller_spawner_stopped = Node(
package="controller_manager",
executable="spawner",
arguments=[
initial_joint_controller, "-c", "/controller_manager", "--inactive"
],
condition=UnlessCondition(activate_joint_controller),
)
nodes_to_start = [
control_node,
ur_control_node,
dashboard_client_node,
tool_communication_node,
controller_stopper_node,
robot_state_publisher_node,
rviz_node,
joint_state_broadcaster_spawner,
io_and_status_controller_spawner,
speed_scaling_state_broadcaster_spawner,
force_torque_sensor_broadcaster_spawner,
forward_position_controller_spawner_stopped,
initial_joint_controller_spawner_stopped,
initial_joint_controller_spawner_started,
]
return nodes_to_start
def generate_launch_description():
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare("rrbot_description"),
"urdf",
"rrbot.urdf.xacro",
]
),
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[
FindPackageShare("ros2_control_demo_bringup"),
"config",
"rrbot_controllers.yaml",
]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rrbot_description"), "config", "rrbot.rviz"]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["forward_position_controller", "-c", "/controller_manager"],
)
nodes = [
control_node,
robot_state_pub_node,
rviz_node,
joint_state_broadcaster_spawner,
robot_controller_spawner,
]
return LaunchDescription(nodes)
def generate_launch_description():
ld = LaunchDescription([
# World path argument
DeclareLaunchArgument(
'world_path',
default_value=world_file_path,
description='Provide full world file path and name'),
LogInfo(msg=LaunchConfiguration('world_path')),
])
# Get path to gazebo package
gazebo_package_prefix = get_package_share_directory('gazebo_ros')
# Launch gazebo servo with world file from world_path
gazebo_server = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[gazebo_package_prefix, '/launch/gzserver.launch.py']),
launch_arguments={'world': LaunchConfiguration('world_path')}.items(),
)
ld.add_action(gazebo_server)
instance = 0
for model_params in models:
generate_model_params = models[model_params]["generate_params"]
if generate_model_params["model_name"] == "NotSet":
generate_model_params["model_name"] = 'sitl_{:s}_{:d}'.format(
generate_model_params["base_model"], instance)
# Path for PX4 binary storage
sitl_output_path = '/tmp/{:s}'.format(
generate_model_params["model_name"])
generate_model_args = ""
for params in generate_model_params:
generate_model_args += ' --{:s} "{:s}"'.format(
params, str(generate_model_params[params]))
generate_model = [
'python3 {:s}/{:s}/scripts/jinja_model_gen.py{:s}'.format(
ros2_ws_parent, models[model_params]["gazebo_name"],
generate_model_args).replace("\n", "").replace(" ", "")
]
# Command to make storage folder
sitl_folder_cmd = ['mkdir -p \"{:s}\"'.format(sitl_output_path)]
# Calculate spawn locations
spawn_pose = models[model_params]["spawn_pose"]
latitude_vehicle = float(latitude) + (
(float(spawn_pose[1]) / 6378137.0) * (180.0 / np.pi))
longitude_vehicle = float(longitude) + (
(float(spawn_pose[0]) / (6378137.0 * np.cos(
(np.pi * float(latitude)) / 180.0))) * (180.0 / np.pi))
altitude_vehicle = float(altitude) + float(spawn_pose[2])
# Set each xterm with PX4 environment variables
px4_env = '''export PX4_SIM_MODEL=\"{:s}\"; export PX4_HOME_LAT={:s};
export PX4_HOME_LON={:s}; export PX4_HOME_ALT={:s};'''.format(
generate_model_params["base_model"], str(latitude_vehicle),
str(longitude_vehicle),
str(altitude_vehicle)).replace("\n", "").replace(" ", "")
# Set path for PX4 build
px4_path = '{:s}/{:s}/build/{:s}'.format(
ros2_ws_parent, models[model_params]["autopilot_name"],
models[model_params]["autopilot_build_type"])
# Command to export model and run PX4 binary
px4_cmd = '''{:s} eval \"\"{:s}/bin/px4\"
-w {:s} \"{:s}/etc\" -s etc/init.d-posix/rcS -i {:d}\"; bash'''.format(
px4_env, px4_path, sitl_output_path, px4_path, instance)
# Xterm command to name xterm window and run px4_cmd
xterm_px4_cmd = [
'''xterm -hold -T \"PX4 NSH {:s}\"
-n \"PX4 NSH {:s}\" -e \'{:s}\''''.format(
sitl_output_path, sitl_output_path,
px4_cmd).replace("\n", "").replace(" ", "")
]
# Execute jinja generator
jinja_model_generate = ExecuteProcess(
cmd=generate_model,
name='jinja_gen_{:s}'.format(generate_model_params["model_name"]),
shell=True,
output='screen')
ld.add_action(jinja_model_generate)
# Make storage command
make_sitl_folder = ExecuteProcess(
cmd=sitl_folder_cmd,
name='make_sitl_folder_{:s}'.format(
generate_model_params["model_name"]),
shell=True)
ld.add_action(make_sitl_folder)
# Run PX4 binary
px4_posix = ExecuteProcess(cmd=xterm_px4_cmd,
name='xterm_px4_nsh_{:s}'.format(
generate_model_params["model_name"]),
shell=True)
ld.add_action(px4_posix)
# Spawn entity
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-entity', '{:s}'.format(generate_model_params["model_name"]),
'-x',
str(spawn_pose[0]), '-y',
str(spawn_pose[1]), '-z',
str(spawn_pose[2]), '-R',
str(spawn_pose[3]), '-P',
str(spawn_pose[4]), '-Y',
str(spawn_pose[5]), '-file',
'/tmp/{:s}.sdf'.format(generate_model_params["model_name"])
],
name='spawn_{:s}'.format(generate_model_params["model_name"]),
output='screen')
ld.add_action(spawn_entity)
# Increment instance
instance += 1
# Launch gazebo client
gazebo_client = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[gazebo_package_prefix, '/launch/gzclient.launch.py']))
LogInfo(msg="\nWaiting to launch Gazebo Client...\n")
sleep(2)
ld.add_action(gazebo_client)
return ld
