From 16c83d7a8cee725c5afda12895f5a9a57717811a Mon Sep 17 00:00:00 2001 From: techlabs Date: Thu, 1 Sep 2022 18:37:54 +0200 Subject: [PATCH 01/16] adding empty world for testing --- turtlebot4_ignition_bringup/worlds/empty.sdf | 78 ++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 turtlebot4_ignition_bringup/worlds/empty.sdf diff --git a/turtlebot4_ignition_bringup/worlds/empty.sdf b/turtlebot4_ignition_bringup/worlds/empty.sdf new file mode 100644 index 0000000..a84bb2c --- /dev/null +++ b/turtlebot4_ignition_bringup/worlds/empty.sdf @@ -0,0 +1,78 @@ + + + + + false + + + 0.002 + 1.0 + + + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.90000000000000002 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0 0 -0 0 + + + + \ No newline at end of file From dfc924f05eacab81afbb5d70d8ba1c9e7165f774 Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Tue, 11 Oct 2022 18:58:48 +0200 Subject: [PATCH 02/16] namespacing launch files --- .../launch/ignition.launch.py | 47 ++-- .../launch/ros_ign_bridge.launch.py | 210 +++++++++++++++++- turtlebot4_ignition_toolbox/src/hmi_node.cpp | 10 +- 3 files changed, 237 insertions(+), 30 deletions(-) diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index f8b12e5..a278a6c 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -68,7 +68,9 @@ def perform( choices=['standard', 'lite'], description='Turtlebot4 Model'), DeclareLaunchArgument('robot_name', default_value='turtlebot4', - description='Robot name') + description='Robot name'), + DeclareLaunchArgument('namespace', default_value='', + description='robot namespace'), ] @@ -150,6 +152,10 @@ def generate_launch_description(): x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') yaw = LaunchConfiguration('yaw') turtlebot4_node_yaml_file = LaunchConfiguration('param_file') + robot_name = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') + namespaced_robot_description = [namespace, '/robot_description'] + namespaced_dock_description = [namespace, '/standard_dock_description'] # Ignition gazebo ignition_gazebo = IncludeLaunchDescription( @@ -167,18 +173,20 @@ def generate_launch_description(): ) # Robot description - robot_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource([robot_description_launch]), - launch_arguments=[('model', LaunchConfiguration('model')), - ('use_sim_time', LaunchConfiguration('use_sim_time'))] - ) + robot_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_description_launch]), + launch_arguments={'gazebo': 'ignition', 'namespace': namespace}.items()) - # Dock description + # Dock description x_dock = OffsetParser(x, 0.157) yaw_dock = OffsetParser(yaw, 3.1416) dock_description = IncludeLaunchDescription( PythonLaunchDescriptionSource([dock_description_launch]), - launch_arguments={'gazebo': 'ignition'}.items() + condition=IfCondition(LaunchConfiguration('spawn_dock')), + # The robot starts docked + launch_arguments={'x': x_dock, 'y': y, 'z': z, 'yaw': yaw_dock, + 'namespace': namespace, + 'gazebo': 'ignition'}.items() ) # Spawn Turtlebot4 @@ -186,12 +194,12 @@ def generate_launch_description(): package='ros_ign_gazebo', executable='create', arguments=[ - '-name', LaunchConfiguration('robot_name'), + '-name', robot_name, '-x', x, '-y', y, '-z', z, '-Y', yaw, - '-topic', 'robot_description'], + '-topic', namespaced_robot_description], output='screen') # Spawn dock @@ -199,18 +207,19 @@ def generate_launch_description(): package='ros_ign_gazebo', executable='create', arguments=[ - '-name', 'standard_dock', + '-name', (robot_name, '_standard_dock'), '-x', x_dock, '-y', y, '-z', z, '-Y', yaw_dock, - '-topic', 'standard_dock_description'], + '-topic', namespaced_dock_description], output='screen') # ROS Ign bridge turtlebot4_ros_ign_bridge = IncludeLaunchDescription( PythonLaunchDescriptionSource([turtlebot4_ros_ign_bridge_launch]), - launch_arguments=[('model', LaunchConfiguration('model'))] + launch_arguments=[('model', LaunchConfiguration('model'), + ('namespace', namespace))] ) # Rviz2 @@ -226,23 +235,27 @@ def generate_launch_description(): ('nav2', LaunchConfiguration('nav2')), ('localization', LaunchConfiguration('localization')), ('use_sim_time', LaunchConfiguration('use_sim_time')), - ('map', LaunchConfiguration('map'))] + ('map', LaunchConfiguration('map')), + ('namespace', namespace)] ) turtlebot4_node = IncludeLaunchDescription( PythonLaunchDescriptionSource([node_launch]), launch_arguments=[('model', LaunchConfiguration('model')), - ('param_file', turtlebot4_node_yaml_file)] + ('param_file', turtlebot4_node_yaml_file), + ('namespace', namespace)] ) # Create3 nodes create3_nodes = IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_nodes_launch]) + PythonLaunchDescriptionSource([create3_nodes_launch]), + launch_arguments=[('namespace', namespace)] ) create3_ignition_nodes = IncludeLaunchDescription( PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), - launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] + launch_arguments=[('robot_name', LaunchConfiguration('robot_name')), + ('namespace', namespace)] ) # RPLIDAR static transforms diff --git a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py index c988e41..6ffac17 100644 --- a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py +++ b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py @@ -18,7 +18,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch.substitutions.path_join_substitution import PathJoinSubstitution @@ -36,6 +36,8 @@ DeclareLaunchArgument('model', default_value='standard', choices=['standard', 'lite'], description='Turtlebot4 Model'), + DeclareLaunchArgument('namespace', default_value='', + description='robot namespace'), ] @@ -50,7 +52,7 @@ def generate_launch_description(): 'user2' ] - namespace = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') pkg_irobot_create_ignition_bringup = get_package_share_directory( @@ -63,12 +65,14 @@ def generate_launch_description(): PythonLaunchDescriptionSource([create3_ros_ign_bridge_launch]), launch_arguments=[ ('robot_name', LaunchConfiguration('robot_name')), - ('world', LaunchConfiguration('world')) + ('world', LaunchConfiguration('world')), + ('namespace', LaunchConfiguration('namespace')) ] ) # lidar bridge lidar_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -90,8 +94,32 @@ def generate_launch_description(): '/scan') ]) + lidar_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_bridge', + executable='parameter_bridge', + namespace=namespace, + name='lidar_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time + }], + arguments=[ + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/rplidar_link/sensor/rplidar/scan' + + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ], + remappings=[ + (['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/rplidar_link/sensor/rplidar/scan'], + ['/', namespace, '/scan']) + ]) + # Display message bridge hmi_display_msg_bridge = Node( + condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -111,11 +139,34 @@ def generate_launch_description(): '/hmi/display/_raw'), (['/model/', LaunchConfiguration('robot_name'), '/hmi/display/selected'], '/hmi/display/_selected') + ]) + + hmi_display_msg_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + package='ros_ign_bridge', + executable='parameter_bridge', + namespace=namespace, + name='hmi_display_msg_bridge', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=[ + ['/model/', LaunchConfiguration('robot_name'), '/hmi/display/raw' + + '@std_msgs/msg/String' + + ']ignition.msgs.StringMsg'], + ['/model/', LaunchConfiguration('robot_name'), '/hmi/display/selected' + + '@std_msgs/msg/Int32' + + ']ignition.msgs.Int32'] ], - condition=LaunchConfigurationEquals('model', 'standard')) + remappings=[ + (['/model/', LaunchConfiguration('robot_name'), '/hmi/display/raw'], + '/hmi/display/_raw'), + (['/model/', LaunchConfiguration('robot_name'), '/hmi/display/selected'], + ['/', namespace, '/hmi/display/_selected']) + ]) # Buttons message bridge hmi_buttons_msg_bridge = Node( + condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -130,11 +181,29 @@ def generate_launch_description(): remappings=[ (['/model/', LaunchConfiguration('robot_name'), '/hmi/buttons'], '/hmi/buttons/_set') + ]) + + hmi_buttons_msg_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + package='ros_ign_bridge', + executable='parameter_bridge', + namespace=namespace, + name='hmi_buttons_msg_bridge', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=[ + ['/model/', LaunchConfiguration('robot_name'), '/hmi/buttons' + + '@std_msgs/msg/Int32' + + '[ignition.msgs.Int32'] ], - condition=LaunchConfigurationEquals('model', 'standard')) + remappings=[ + (['/model/', LaunchConfiguration('robot_name'), '/hmi/buttons'], + ['/', namespace, '/hmi/buttons/_set']) + ]) # Buttons message bridge hmi_led_msg_bridge = Node( + condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -149,11 +218,29 @@ def generate_launch_description(): remappings=[ (['/model/', LaunchConfiguration('robot_name'), '/hmi/led/' + led], '/hmi/led/_' + led) for led in leds + ]) + + hmi_led_msg_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + package='ros_ign_bridge', + executable='parameter_bridge', + namespace=namespace, + name='hmi_led_msg_bridge', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=[ + ['/model/', LaunchConfiguration('robot_name'), '/hmi/led/' + led + + '@std_msgs/msg/Int32' + + ']ignition.msgs.Int32'] for led in leds ], - condition=LaunchConfigurationEquals('model', 'standard')) + remappings=[ + (['/model/', LaunchConfiguration('robot_name'), '/hmi/led/' + led], + ['/', namespace, '/hmi/led/_' + led]) for led in leds + ]) # Camera sensor bridge oakd_pro_camera_bridge = Node( + condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -203,10 +290,63 @@ def generate_launch_description(): LaunchConfiguration('robot_name'), '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info'], '/color/camera_info') + ]) + + oakd_pro_camera_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + package='ros_ign_bridge', + executable='parameter_bridge', + namespace=namespace, + name='camera_bridge', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=[ + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image' + + '@sensor_msgs/msg/Image' + + '[ignition.msgs.Image'], + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image' + + '@sensor_msgs/msg/Image' + + '[ignition.msgs.Image'], + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points' + + '@sensor_msgs/msg/PointCloud2' + + '[ignition.msgs.PointCloudPacked'], + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info' + + '@sensor_msgs/msg/CameraInfo' + + '[ignition.msgs.CameraInfo'], ], - condition=LaunchConfigurationEquals('model', 'standard')) + remappings=[ + (['/world/', LaunchConfiguration('world'), + '/model/', + LaunchConfiguration('robot_name'), + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image'], + ['/', namespace, '/color/image']), + (['/world/', LaunchConfiguration('world'), + '/model/', + LaunchConfiguration('robot_name'), + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image'], + ['/', namespace, '/stereo/depth']), + (['/world/', LaunchConfiguration('world'), + '/model/', + LaunchConfiguration('robot_name'), + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points'], + ['/', namespace, '/stereo/depth/points']), + (['/world/', LaunchConfiguration('world'), + '/model/', + LaunchConfiguration('robot_name'), + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info'], + ['/', namespace, '/color/camera_info']) + ]) oakd_lite_camera_bridge = Node( + condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'lite'), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -252,8 +392,56 @@ def generate_launch_description(): '/model/', LaunchConfiguration('robot_name'), '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/camera_info'], '/color/camera_info') + ]) + + oakd_lite_camera_bridge_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'lite'), + package='ros_ign_bridge', + executable='parameter_bridge', + namespace=namespace, + name='camera_bridge', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=[ + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/image' + + '@sensor_msgs/msg/Image' + + '[ignition.msgs.Image'], + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/depth_image' + + '@sensor_msgs/msg/Image' + + '[ignition.msgs.Image'], + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/points' + + '@sensor_msgs/msg/PointCloud2' + + '[ignition.msgs.PointCloudPacked'], + ['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/camera_info' + + '@sensor_msgs/msg/CameraInfo' + + '[ignition.msgs.CameraInfo'], ], - condition=LaunchConfigurationEquals('model', 'lite')) + remappings=[ + (['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/image'], + ['/', namespace, '/color/image']), + (['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/depth_image'], + ['/', namespace, '/stereo/depth']), + (['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/points'], + ['/', namespace, '/stereo/depth/points']), + (['/world/', LaunchConfiguration('world'), + '/model/', LaunchConfiguration('robot_name'), + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/camera_info'], + ['/', namespace, '/color/camera_info']) + ]) # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) @@ -264,4 +452,10 @@ def generate_launch_description(): ld.add_action(lidar_bridge) ld.add_action(oakd_pro_camera_bridge) ld.add_action(oakd_lite_camera_bridge) + ld.add_action(hmi_display_msg_bridge_namespaced) + ld.add_action(hmi_buttons_msg_bridge_namespaced) + ld.add_action(hmi_led_msg_bridge_namespaced) + ld.add_action(lidar_bridge_namespaced) + ld.add_action(oakd_pro_camera_bridge_namespaced) + ld.add_action(oakd_lite_camera_bridge_namespaced) return ld diff --git a/turtlebot4_ignition_toolbox/src/hmi_node.cpp b/turtlebot4_ignition_toolbox/src/hmi_node.cpp index 077d11e..a707a76 100644 --- a/turtlebot4_ignition_toolbox/src/hmi_node.cpp +++ b/turtlebot4_ignition_toolbox/src/hmi_node.cpp @@ -28,25 +28,25 @@ Hmi::Hmi() : rclcpp::Node("hmi_node") { display_subscriber_ = create_subscription( - "/hmi/display", + "hmi/display", rclcpp::SensorDataQoS(), std::bind(&Hmi::display_subscriber_callback, this, std::placeholders::_1)); button_subscriber_ = create_subscription( - "/hmi/buttons/_set", + "hmi/buttons/_set", rclcpp::QoS(rclcpp::KeepLast(10)), std::bind(&Hmi::button_subscriber_callback, this, std::placeholders::_1)); button_publisher_ = create_publisher( - "/hmi/buttons", + "hmi/buttons", rclcpp::SensorDataQoS()); display_raw_publisher_ = create_publisher( - "/hmi/display/_raw", + "hmi/display/_raw", rclcpp::QoS(rclcpp::KeepLast(10))); display_selected_publisher_ = create_publisher( - "/hmi/display/_selected", + "hmi/display/_selected", rclcpp::QoS(rclcpp::KeepLast(10))); } From 2f5d88f721def202f4df3997c6a995e3e05ee166 Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Wed, 12 Oct 2022 18:35:41 +0200 Subject: [PATCH 03/16] HMI wip --- .../launch/ignition.launch.py | 2 +- .../launch/turtlebot4_nodes.launch.py | 7 +- .../Turtlebot4Hmi/Turtlebot4Hmi.cc | 98 +++++++++++++++++-- .../Turtlebot4Hmi/Turtlebot4Hmi.hh | 26 ++++- .../Turtlebot4Hmi/Turtlebot4Hmi.qml | 30 +++++- 5 files changed, 149 insertions(+), 14 deletions(-) diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index a278a6c..386d39d 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -305,7 +305,7 @@ def generate_launch_description(): ld.add_action(ignition_gazebo) ld.add_action(turtlebot4_ros_ign_bridge) ld.add_action(rviz2) - ld.add_action(robot_description) + ld.add_action(robot_description_launch) ld.add_action(dock_description) ld.add_action(spawn_robot) ld.add_action(spawn_dock) diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py index a3757c7..c1351da 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py @@ -24,7 +24,9 @@ ARGUMENTS = [ DeclareLaunchArgument('model', default_value='standard', choices=['standard', 'lite'], - description='Turtlebot4 Model') + description='Turtlebot4 Model'), + DeclareLaunchArgument('namespace', default_value='', + description='robot namespace'), ] @@ -42,11 +44,13 @@ def generate_launch_description(): ) turtlebot4_node_yaml_file = LaunchConfiguration('param_file') + namespace = LaunchConfiguration('namespace') # Turtlebot4 node turtlebot4_node = Node( package='turtlebot4_node', name='turtlebot4_node', + namespace=namespace, executable='turtlebot4_node', parameters=[turtlebot4_node_yaml_file, {'model': LaunchConfiguration('model')}], @@ -57,6 +61,7 @@ def generate_launch_description(): turtlebot4_ignition_hmi_node = Node( package='turtlebot4_ignition_toolbox', name='turtlebot4_ignition_hmi_node', + namespace=namespace, executable='turtlebot4_ignition_hmi_node', output='screen', condition=LaunchConfigurationEquals('model', 'standard') diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc index d10532f..734af29 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc @@ -32,11 +32,21 @@ Turtlebot4Hmi::Turtlebot4Hmi() : Plugin() { App()->Engine()->rootContext()->setContextProperty("DisplayListView", &this->display_list_); - this->hmi_button_pub_ = ignition::transport::Node::Publisher(); - this->hmi_button_pub_ = this->node_.Advertise < ignition::msgs::Int32 > (this->hmi_button_topic_); - this->create3_button_pub_ = ignition::transport::Node::Publisher(); - this->create3_button_pub_ = this->node_.Advertise < ignition::msgs::Int32 > ( - this->create3_button_topic_); + this->UpdateTopics(); +} + +Turtlebot4Hmi::~Turtlebot4Hmi() +{ +} + +QString Turtlebot4Hmi::Namespace() const +{ + return QString::fromStdString(this->namespace_); +} + +void Turtlebot4Hmi::UpdateTopics() +{ + // Update subscribers with new topics this->node_.Subscribe(this->display_topic_, &Turtlebot4Hmi::OnRawMessage, this); this->node_.Subscribe(this->display_selected_topic_, &Turtlebot4Hmi::OnSelectedMessage, this); this->node_.Subscribe(this->power_led_topic_, &Turtlebot4Hmi::OnPowerLedMessage, this); @@ -46,22 +56,90 @@ Turtlebot4Hmi::Turtlebot4Hmi() this->node_.Subscribe(this->battery_led_topic_, &Turtlebot4Hmi::OnBatteryLedMessage, this); this->node_.Subscribe(this->user1_led_topic_, &Turtlebot4Hmi::OnUser1LedMessage, this); this->node_.Subscribe(this->user2_led_topic_, &Turtlebot4Hmi::OnUser2LedMessage, this); + + // Update publisher with new topics + this->hmi_button_pub_ = ignition::transport::Node::Publisher(); + this->hmi_button_pub_ = this->node_.Advertise < ignition::msgs::Int32 > (this->hmi_button_topic_); + this->create3_button_pub_ = gz::transport::Node::Publisher(); + this->create3_button_pub_ = this->node_.Advertise< ignition::msgs::Int32 > (this->create3_button_topic_); + if (!this->create3_button_pub_ || !this->hmi_button_pub_) + { + App()->findChild()->notifyWithDuration( + QString::fromStdString("Error when advertising topics"), 4000); + ignerr << "Error when advertising topics" << std::endl; + } + else + { + App()->findChild()->notifyWithDuration( + QString::fromStdString("Advertising new topics.. '"), 4000); + } + } -Turtlebot4Hmi::~Turtlebot4Hmi() +void Turtlebot4Hmi::SetNamespace(const QString &_namespace) { + this->namespace_ = _namespace.toStdString(); + ignmsg << "A new namespace has been entered: '" << + this->namespace_ << " ' " <node_.Unsubscribe(this->display_topic_); + this->node_.Unsubscribe(this->display_selected_topic_); + this->node_.Unsubscribe(this->power_led_topic_); + this->node_.Unsubscribe(this->motors_led_topic_); + this->node_.Unsubscribe(this->comms_led_topic_); + this->node_.Unsubscribe(this->wifi_led_topic_); + this->node_.Unsubscribe(this->battery_led_topic_); + this->node_.Unsubscribe(this->user1_led_topic_); + this->node_.Unsubscribe(this->user2_led_topic_); + + // Change all topics + if (this->namespace_ != "") + { + this->hmi_button_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/buttons"; + this->create3_button_topic_ = "/" + this->namespace_ + "/buttons"; + this->display_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/display/raw"; + this->display_selected_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/display/selected"; + this->power_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/power"; + this->motors_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/motors"; + this->comms_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/comms"; + this->wifi_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/wifi"; + this->battery_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/battery"; + this->user1_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/user1"; + this->user2_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/user2"; + } + else + { + this->hmi_button_topic_ = "/model/turtlebot4/hmi/buttons"; + this->create3_button_topic_ = "/buttons"; + this->display_topic_ = "/model/turtlebot4/hmi/display/raw"; + this->display_selected_topic_ = "/model/turtlebot4/hmi/display/selected"; + this->power_led_topic_ = "/model/turtlebot4/hmi/led/power"; + this->motors_led_topic_ = "/model/turtlebot4/hmi/led/motors"; + this->comms_led_topic_ = "/model/turtlebot4/hmi/led/comms"; + this->wifi_led_topic_ = "/model/turtlebot4/hmi/led/wifi"; + this->battery_led_topic_ = "/model/turtlebot4/hmi/led/battery"; + this->user1_led_topic_ = "/model/turtlebot4/hmi/led/user1"; + this->user2_led_topic_ = "/model/turtlebot4/hmi/led/user2"; + } + + this->UpdateTopics(); + this->NamespaceChanged(); } void Turtlebot4Hmi::LoadConfig(const tinyxml2::XMLElement * _pluginElem) { - if (!_pluginElem) { - return; - } - if (this->title.empty()) { this->title = "Turtlebot4 HMI"; } + if (_pluginElem) + { + auto topicElem = _pluginElem->FirstChildElement("topic"); + if (nullptr != topicElem && nullptr != topicElem->GetText()) + this->SetNamespace(topicElem->GetText()); + } + this->connect( this, SIGNAL(AddMsg(QString)), this, SLOT(OnAddMsg(QString)), Qt::QueuedConnection); diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh index f7d9053..d147bc0 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh @@ -36,6 +36,14 @@ class Turtlebot4Hmi : public Plugin { Q_OBJECT + // \brief Namespace + Q_PROPERTY( + QString namespace + READ Namespace + WRITE SetNamespace + NOTIFY NamespaceChanged + ) + public: /// \brief Constructor Turtlebot4Hmi(); @@ -44,6 +52,20 @@ public: /// \brief Called by Ignition GUI when plugin is instantiated. /// \param[in] _pluginElem XML configuration for this plugin. void LoadConfig(const tinyxml2::XMLElement * _pluginElem) override; + // \brief Get the namespace as a string, for example + /// '/echo' + /// \return Namespace + Q_INVOKABLE QString Namespace() const; + +public slots: + /// \brief Callback in Qt thread when the namespace changes. + /// \param[in] _namespace variable to indicate the namespace to + /// publish the Button commands and subscribe to robot status. + void SetNamespace(const QString &_namespace); + +signals: + /// \brief Notify that namespace has changed + void NamespaceChanged(); protected slots: /// \brief Callback trigged when the button is pressed. @@ -71,6 +93,7 @@ private: void OnBatteryLedMessage(const ignition::msgs::Int32 & msg); void OnUser1LedMessage(const ignition::msgs::Int32 & msg); void OnUser2LedMessage(const ignition::msgs::Int32 & msg); + void UpdateTopics(); signals: void AddMsg(QString msg); @@ -83,8 +106,9 @@ private: ignition::transport::Node::Publisher hmi_button_pub_; ignition::transport::Node::Publisher create3_button_pub_; + std::string namespace_ = ""; std::string hmi_button_topic_ = "/model/turtlebot4/hmi/buttons"; - std::string create3_button_topic_ = "/create3/buttons"; + std::string create3_button_topic_ = "/buttons"; std::string display_topic_ = "/model/turtlebot4/hmi/display/raw"; std::string display_selected_topic_ = "/model/turtlebot4/hmi/display/selected"; std::string power_led_topic_ = "/model/turtlebot4/hmi/led/power"; diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml index 220d19f..e966d22 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml @@ -31,7 +31,35 @@ Rectangle anchors.fill: parent focus: true Layout.minimumWidth: 400 - Layout.minimumHeight: 525 + Layout.minimumHeight: 575 + + // Namespace input + Label { + id: namespaceLabel + text: "Namespace:" + Layout.fillWidth: true + Layout.margins: 10 + anchors.top: widgetRectangle.top + anchors.topMargin: 10 + anchors.left: widgetRectangle.left + anchors.leftMargin: 10 + } + + TextField { + id: namespaceField + width: 175 + Layout.fillWidth: true + Layout.margins: 10 + text: Turtlebot4Hmi.namespace + placeholderText: qsTr("Namespace to publish...") + anchors.top: namespaceLabel.bottom + anchors.topMargin: 5 + anchors.left: widgetRectangle.left + anchors.leftMargin: 10 + onEditingFinished: { + Turtlebot4Hmi.SetNamespace(text) + } + } Connections { target: Turtlebot4Hmi From 4e2f99fa70cc4f443c197817df1a1244ff374d99 Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Wed, 12 Oct 2022 19:25:27 +0200 Subject: [PATCH 04/16] fix UI for HMI namespace --- .../Turtlebot4Hmi/Turtlebot4Hmi.qml | 70 +++++++++---------- 1 file changed, 33 insertions(+), 37 deletions(-) diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml index e966d22..0158d46 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml @@ -31,35 +31,7 @@ Rectangle anchors.fill: parent focus: true Layout.minimumWidth: 400 - Layout.minimumHeight: 575 - - // Namespace input - Label { - id: namespaceLabel - text: "Namespace:" - Layout.fillWidth: true - Layout.margins: 10 - anchors.top: widgetRectangle.top - anchors.topMargin: 10 - anchors.left: widgetRectangle.left - anchors.leftMargin: 10 - } - - TextField { - id: namespaceField - width: 175 - Layout.fillWidth: true - Layout.margins: 10 - text: Turtlebot4Hmi.namespace - placeholderText: qsTr("Namespace to publish...") - anchors.top: namespaceLabel.bottom - anchors.topMargin: 5 - anchors.left: widgetRectangle.left - anchors.leftMargin: 10 - onEditingFinished: { - Turtlebot4Hmi.SetNamespace(text) - } - } + Layout.minimumHeight: 600 Connections { target: Turtlebot4Hmi @@ -103,20 +75,46 @@ Rectangle Rectangle { id: create3ButtonsRectangle - border.color: "black" - border.width: 2 - anchors.top: widgetRectangle.top + anchors.top: namespaceRectangle.top anchors.left: widgetRectangle.left focus: true - height: 125 + height: 200 width: 400 + // Namespace input + Label { + id: namespaceLabel + text: "Namespace:" + Layout.fillWidth: true + Layout.margins: 10 + anchors.top: create3ButtonsRectangle.top + anchors.topMargin: 10 + anchors.left: parent.left + anchors.leftMargin: 10 + } + + TextField { + id: namespaceField + width: 175 + Layout.fillWidth: true + Layout.margins: 10 + text: Turtlebot4Hmi.namespace + placeholderText: qsTr("insert namespace") + anchors.top: namespaceLabel.bottom + anchors.topMargin: 5 + anchors.left: parent.left + anchors.leftMargin: 10 + onEditingFinished: { + Turtlebot4Hmi.SetNamespace(text) + } + } + // Buttons Label { id: create3ButtonsLabel text: "Create3" font.pixelSize: 22 - anchors.top: create3ButtonsRectangle.top + anchors.top: namespaceField.bottom anchors.topMargin: 10 anchors.left: parent.left anchors.leftMargin: 10 @@ -187,14 +185,12 @@ Rectangle height: 350 width: 400 color: "transparent" - border.color: "black" - border.width: 2 anchors.top: create3ButtonsRectangle.bottom anchors.left: create3ButtonsRectangle.left Label { id: hmiLabel - text: "Turtlebot4 HMI" + text: "Turtlebot4" font.pixelSize: 22 anchors.top: hmiRectangle.top anchors.topMargin: 10 From 20144c69cb3ffdf9b87a1dc48ca76e472eaf0069 Mon Sep 17 00:00:00 2001 From: techlabs Date: Thu, 13 Oct 2022 18:03:21 +0200 Subject: [PATCH 05/16] fix ')' issue in mian launch and double conditions in bridge --- .../launch/ignition.launch.py | 7 ++++-- .../launch/ros_ign_bridge.launch.py | 25 ++++++++++--------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index 386d39d..d6f9e49 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -67,6 +67,9 @@ def perform( DeclareLaunchArgument('model', default_value='standard', choices=['standard', 'lite'], description='Turtlebot4 Model'), + DeclareLaunchArgument('spawn_dock', default_value='true', + choices=['true', 'false'], + description='Dock Model.'), DeclareLaunchArgument('robot_name', default_value='turtlebot4', description='Robot name'), DeclareLaunchArgument('namespace', default_value='', @@ -218,8 +221,8 @@ def generate_launch_description(): # ROS Ign bridge turtlebot4_ros_ign_bridge = IncludeLaunchDescription( PythonLaunchDescriptionSource([turtlebot4_ros_ign_bridge_launch]), - launch_arguments=[('model', LaunchConfiguration('model'), - ('namespace', namespace))] + launch_arguments=[('model', LaunchConfiguration('model')), + ('namespace', namespace)] ) # Rviz2 diff --git a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py index 6ffac17..8aa32f0 100644 --- a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py +++ b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py @@ -18,9 +18,9 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals +from launch.conditions import LaunchConfigurationEquals, LaunchConfigurationNotEquals, IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PythonExpression from launch.substitutions.path_join_substitution import PathJoinSubstitution from launch_ros.actions import Node @@ -53,6 +53,7 @@ def generate_launch_description(): ] namespace = LaunchConfiguration('namespace') + model = LaunchConfiguration('model') use_sim_time = LaunchConfiguration('use_sim_time') pkg_irobot_create_ignition_bringup = get_package_share_directory( @@ -119,7 +120,7 @@ def generate_launch_description(): # Display message bridge hmi_display_msg_bridge = Node( - condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -142,7 +143,7 @@ def generate_launch_description(): ]) hmi_display_msg_bridge_namespaced = Node( - condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -166,7 +167,7 @@ def generate_launch_description(): # Buttons message bridge hmi_buttons_msg_bridge = Node( - condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -184,7 +185,7 @@ def generate_launch_description(): ]) hmi_buttons_msg_bridge_namespaced = Node( - condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -203,7 +204,7 @@ def generate_launch_description(): # Buttons message bridge hmi_led_msg_bridge = Node( - condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -221,7 +222,7 @@ def generate_launch_description(): ]) hmi_led_msg_bridge_namespaced = Node( - condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -240,7 +241,7 @@ def generate_launch_description(): # Camera sensor bridge oakd_pro_camera_bridge = Node( - condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -293,7 +294,7 @@ def generate_launch_description(): ]) oakd_pro_camera_bridge_namespaced = Node( - condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'standard'), + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -346,7 +347,7 @@ def generate_launch_description(): ]) oakd_lite_camera_bridge = Node( - condition=LaunchConfigurationEquals('namespace', '') and LaunchConfigurationEquals('model', 'lite'), + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'lite'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -395,7 +396,7 @@ def generate_launch_description(): ]) oakd_lite_camera_bridge_namespaced = Node( - condition=LaunchConfigurationNotEquals('namespace', '') and LaunchConfigurationEquals('model', 'lite'), + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'lite'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, From decdf2b0ef15548df3826acd9ce75d0e34239dce Mon Sep 17 00:00:00 2001 From: techlabs Date: Thu, 13 Oct 2022 18:25:41 +0200 Subject: [PATCH 06/16] fix missing argument declaration in launch file --- turtlebot4_ignition_bringup/launch/ignition.launch.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index d6f9e49..99dac6f 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -76,6 +76,10 @@ def perform( description='robot namespace'), ] +for pose_element in ['x', 'y', 'z', 'yaw']: + ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', + description=f'{pose_element} component of the robot pose.')) + def generate_launch_description(): From 26ddc40c46892fb6c79b4c880fbfaf4ebcad5724 Mon Sep 17 00:00:00 2001 From: techlabs Date: Fri, 14 Oct 2022 18:09:47 +0200 Subject: [PATCH 07/16] HMI uses robot_name, dock spawn reversed --- .../launch/ignition.launch.py | 3 +- .../launch/ros_ign_bridge.launch.py | 186 +++++++++--------- .../Turtlebot4Hmi/Turtlebot4Hmi.cc | 28 +-- .../Turtlebot4Hmi/Turtlebot4Hmi.hh | 4 +- .../Turtlebot4Hmi/Turtlebot4Hmi.qml | 4 +- 5 files changed, 114 insertions(+), 111 deletions(-) diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index 99dac6f..76e61ea 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -218,7 +218,7 @@ def generate_launch_description(): '-x', x_dock, '-y', y, '-z', z, - '-Y', yaw_dock, + '-Y', '3.141592', '-topic', namespaced_dock_description], output='screen') @@ -226,6 +226,7 @@ def generate_launch_description(): turtlebot4_ros_ign_bridge = IncludeLaunchDescription( PythonLaunchDescriptionSource([turtlebot4_ros_ign_bridge_launch]), launch_arguments=[('model', LaunchConfiguration('model')), + ('robot_name', robot_name), ('namespace', namespace)] ) diff --git a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py index 8aa32f0..d8d2c52 100644 --- a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py +++ b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py @@ -53,6 +53,8 @@ def generate_launch_description(): ] namespace = LaunchConfiguration('namespace') + robot_name = LaunchConfiguration('robot_name') + world = LaunchConfiguration('world') model = LaunchConfiguration('model') use_sim_time = LaunchConfiguration('use_sim_time') @@ -65,9 +67,9 @@ def generate_launch_description(): create3_bridge = IncludeLaunchDescription( PythonLaunchDescriptionSource([create3_ros_ign_bridge_launch]), launch_arguments=[ - ('robot_name', LaunchConfiguration('robot_name')), - ('world', LaunchConfiguration('world')), - ('namespace', LaunchConfiguration('namespace')) + ('robot_name', robot_name), + ('world', world), + ('namespace', namespace) ] ) @@ -83,14 +85,14 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/rplidar_link/sensor/rplidar/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] ], remappings=[ - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/rplidar_link/sensor/rplidar/scan'], '/scan') ]) @@ -106,14 +108,14 @@ def generate_launch_description(): 'use_sim_time': use_sim_time }], arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/rplidar_link/sensor/rplidar/scan' + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] ], remappings=[ - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/rplidar_link/sensor/rplidar/scan'], ['/', namespace, '/scan']) ]) @@ -128,17 +130,17 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/hmi/display/raw' + + ['/model/', robot_name, '/hmi/display/raw' + '@std_msgs/msg/String' + ']ignition.msgs.StringMsg'], - ['/model/', LaunchConfiguration('robot_name'), '/hmi/display/selected' + + ['/model/', robot_name, '/hmi/display/selected' + '@std_msgs/msg/Int32' + ']ignition.msgs.Int32'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/hmi/display/raw'], + (['/model/', robot_name, '/hmi/display/raw'], '/hmi/display/_raw'), - (['/model/', LaunchConfiguration('robot_name'), '/hmi/display/selected'], + (['/model/', robot_name, '/hmi/display/selected'], '/hmi/display/_selected') ]) @@ -151,17 +153,17 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/hmi/display/raw' + + ['/model/', robot_name, '/hmi/display/raw' + '@std_msgs/msg/String' + ']ignition.msgs.StringMsg'], - ['/model/', LaunchConfiguration('robot_name'), '/hmi/display/selected' + + ['/model/', robot_name, '/hmi/display/selected' + '@std_msgs/msg/Int32' + ']ignition.msgs.Int32'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/hmi/display/raw'], - '/hmi/display/_raw'), - (['/model/', LaunchConfiguration('robot_name'), '/hmi/display/selected'], + (['/model/', robot_name, '/hmi/display/raw'], + ['/', namespace, '/hmi/display/_raw']), + (['/model/', robot_name, '/hmi/display/selected'], ['/', namespace, '/hmi/display/_selected']) ]) @@ -175,12 +177,12 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/hmi/buttons' + + ['/model/', robot_name, '/hmi/buttons' + '@std_msgs/msg/Int32' + '[ignition.msgs.Int32'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/hmi/buttons'], + (['/model/', robot_name, '/hmi/buttons'], '/hmi/buttons/_set') ]) @@ -193,12 +195,12 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/hmi/buttons' + + ['/model/', robot_name, '/hmi/buttons' + '@std_msgs/msg/Int32' + '[ignition.msgs.Int32'] ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/hmi/buttons'], + (['/model/', robot_name, '/hmi/buttons'], ['/', namespace, '/hmi/buttons/_set']) ]) @@ -212,12 +214,12 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/hmi/led/' + led + + ['/model/', robot_name, '/hmi/led/' + led + '@std_msgs/msg/Int32' + ']ignition.msgs.Int32'] for led in leds ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/hmi/led/' + led], + (['/model/', robot_name, '/hmi/led/' + led], '/hmi/led/_' + led) for led in leds ]) @@ -230,12 +232,12 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/model/', LaunchConfiguration('robot_name'), '/hmi/led/' + led + + ['/model/', robot_name, '/hmi/led/' + led + '@std_msgs/msg/Int32' + ']ignition.msgs.Int32'] for led in leds ], remappings=[ - (['/model/', LaunchConfiguration('robot_name'), '/hmi/led/' + led], + (['/model/', robot_name, '/hmi/led/' + led], ['/', namespace, '/hmi/led/_' + led]) for led in leds ]) @@ -249,46 +251,46 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image' + '@sensor_msgs/msg/Image' + '[ignition.msgs.Image'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image' + '@sensor_msgs/msg/Image' + '[ignition.msgs.Image'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points' + '@sensor_msgs/msg/PointCloud2' + '[ignition.msgs.PointCloudPacked'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info' + '@sensor_msgs/msg/CameraInfo' + '[ignition.msgs.CameraInfo'], ], remappings=[ - (['/world/', LaunchConfiguration('world'), + (['/world/', world, '/model/', - LaunchConfiguration('robot_name'), + robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image'], '/color/image'), - (['/world/', LaunchConfiguration('world'), + (['/world/', world, '/model/', - LaunchConfiguration('robot_name'), + robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image'], '/stereo/depth'), - (['/world/', LaunchConfiguration('world'), + (['/world/', world, '/model/', - LaunchConfiguration('robot_name'), + robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points'], '/stereo/depth/points'), - (['/world/', LaunchConfiguration('world'), + (['/world/', world, '/model/', - LaunchConfiguration('robot_name'), + robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info'], '/color/camera_info') ]) @@ -302,46 +304,46 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image' + '@sensor_msgs/msg/Image' + '[ignition.msgs.Image'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image' + '@sensor_msgs/msg/Image' + '[ignition.msgs.Image'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points' + '@sensor_msgs/msg/PointCloud2' + '[ignition.msgs.PointCloudPacked'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info' + '@sensor_msgs/msg/CameraInfo' + '[ignition.msgs.CameraInfo'], ], remappings=[ - (['/world/', LaunchConfiguration('world'), + (['/world/', world, '/model/', - LaunchConfiguration('robot_name'), + robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image'], ['/', namespace, '/color/image']), - (['/world/', LaunchConfiguration('world'), + (['/world/', world, '/model/', - LaunchConfiguration('robot_name'), + robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image'], ['/', namespace, '/stereo/depth']), - (['/world/', LaunchConfiguration('world'), + (['/world/', world, '/model/', - LaunchConfiguration('robot_name'), + robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points'], ['/', namespace, '/stereo/depth/points']), - (['/world/', LaunchConfiguration('world'), + (['/world/', world, '/model/', - LaunchConfiguration('robot_name'), + robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info'], ['/', namespace, '/color/camera_info']) ]) @@ -355,42 +357,42 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/image' + '@sensor_msgs/msg/Image' + '[ignition.msgs.Image'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/depth_image' + '@sensor_msgs/msg/Image' + '[ignition.msgs.Image'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/points' + '@sensor_msgs/msg/PointCloud2' + '[ignition.msgs.PointCloudPacked'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/camera_info' + '@sensor_msgs/msg/CameraInfo' + '[ignition.msgs.CameraInfo'], ], remappings=[ - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/image'], '/color/image'), - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/depth_image'], '/stereo/depth'), - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/points'], '/stereo/depth/points'), - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/camera_info'], '/color/camera_info') ]) @@ -404,42 +406,42 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[ - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/image' + '@sensor_msgs/msg/Image' + '[ignition.msgs.Image'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/depth_image' + '@sensor_msgs/msg/Image' + '[ignition.msgs.Image'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/points' + '@sensor_msgs/msg/PointCloud2' + '[ignition.msgs.PointCloudPacked'], - ['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + ['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/camera_info' + '@sensor_msgs/msg/CameraInfo' + '[ignition.msgs.CameraInfo'], ], remappings=[ - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/image'], ['/', namespace, '/color/image']), - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/depth_image'], ['/', namespace, '/stereo/depth']), - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/points'], ['/', namespace, '/stereo/depth/points']), - (['/world/', LaunchConfiguration('world'), - '/model/', LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/camera_info'], ['/', namespace, '/color/camera_info']) ]) diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc index 734af29..b403f5c 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc @@ -71,7 +71,7 @@ void Turtlebot4Hmi::UpdateTopics() else { App()->findChild()->notifyWithDuration( - QString::fromStdString("Advertising new topics.. '"), 4000); + QString::fromStdString("Advertising new topics based on robot name: " + this->namespace_), 4000); } } @@ -79,7 +79,7 @@ void Turtlebot4Hmi::UpdateTopics() void Turtlebot4Hmi::SetNamespace(const QString &_namespace) { this->namespace_ = _namespace.toStdString(); - ignmsg << "A new namespace has been entered: '" << + ignmsg << "A new robot name has been entered: '" << this->namespace_ << " ' " <namespace_ != "") { - this->hmi_button_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/buttons"; - this->create3_button_topic_ = "/" + this->namespace_ + "/buttons"; - this->display_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/display/raw"; - this->display_selected_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/display/selected"; - this->power_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/power"; - this->motors_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/motors"; - this->comms_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/comms"; - this->wifi_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/wifi"; - this->battery_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/battery"; - this->user1_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/user1"; - this->user2_led_topic_ = "/" + this->namespace_ + "/model/turtlebot4/hmi/led/user2"; + this->hmi_button_topic_ = "/model/" + this->namespace_ + "/hmi/buttons"; + this->create3_button_topic_ = "/model/" + this->namespace_ + "/buttons"; + this->display_topic_ = "/model/" + this->namespace_ + "hmi/display/raw"; + this->display_selected_topic_ = "/model/" + this->namespace_ + "hmi/display/selected"; + this->power_led_topic_ = "/model/" + this->namespace_ + "hmi/led/power"; + this->motors_led_topic_ = "/model/" + this->namespace_ + "hmi/led/motors"; + this->comms_led_topic_ = "/model/" + this->namespace_ + "hmi/led/comms"; + this->wifi_led_topic_ = "/model/" + this->namespace_ + "hmi/led/wifi"; + this->battery_led_topic_ = "/model/" + this->namespace_ + "hmi/led/battery"; + this->user1_led_topic_ = "/model/" + this->namespace_ + "hmi/led/user1"; + this->user2_led_topic_ = "/model/" + this->namespace_ + "hmi/led/user2"; } else { this->hmi_button_topic_ = "/model/turtlebot4/hmi/buttons"; - this->create3_button_topic_ = "/buttons"; + this->create3_button_topic_ = "model/turtlebot4/buttons"; this->display_topic_ = "/model/turtlebot4/hmi/display/raw"; this->display_selected_topic_ = "/model/turtlebot4/hmi/display/selected"; this->power_led_topic_ = "/model/turtlebot4/hmi/led/power"; diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh index d147bc0..b1f1096 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh @@ -106,9 +106,9 @@ private: ignition::transport::Node::Publisher hmi_button_pub_; ignition::transport::Node::Publisher create3_button_pub_; - std::string namespace_ = ""; + std::string namespace_ = "turtlebot4"; std::string hmi_button_topic_ = "/model/turtlebot4/hmi/buttons"; - std::string create3_button_topic_ = "/buttons"; + std::string create3_button_topic_ = "/model/turtlebot4/buttons"; std::string display_topic_ = "/model/turtlebot4/hmi/display/raw"; std::string display_selected_topic_ = "/model/turtlebot4/hmi/display/selected"; std::string power_led_topic_ = "/model/turtlebot4/hmi/led/power"; diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml index 0158d46..39a450b 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml @@ -84,7 +84,7 @@ Rectangle // Namespace input Label { id: namespaceLabel - text: "Namespace:" + text: "Robot Name:" Layout.fillWidth: true Layout.margins: 10 anchors.top: create3ButtonsRectangle.top @@ -99,7 +99,7 @@ Rectangle Layout.fillWidth: true Layout.margins: 10 text: Turtlebot4Hmi.namespace - placeholderText: qsTr("insert namespace") + placeholderText: qsTr("insert robot name") anchors.top: namespaceLabel.bottom anchors.topMargin: 5 anchors.left: parent.left From 91adb0f46a1eb39113e80d46eb968964e37192e6 Mon Sep 17 00:00:00 2001 From: techlabs Date: Mon, 17 Oct 2022 17:00:57 +0200 Subject: [PATCH 08/16] fixing lidar and camera frames --- .../launch/ignition.launch.py | 63 ++++++++++++++++--- 1 file changed, 56 insertions(+), 7 deletions(-) diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index 76e61ea..da16136 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -22,9 +22,9 @@ from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable -from launch.conditions import IfCondition, LaunchConfigurationEquals +from launch.conditions import IfCondition, LaunchConfigurationEquals, LaunchConfigurationNotEquals from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression from launch_ros.actions import Node @@ -160,6 +160,7 @@ def generate_launch_description(): yaw = LaunchConfiguration('yaw') turtlebot4_node_yaml_file = LaunchConfiguration('param_file') robot_name = LaunchConfiguration('robot_name') + model = LaunchConfiguration('model') namespace = LaunchConfiguration('namespace') namespaced_robot_description = [namespace, '/robot_description'] namespaced_dock_description = [namespace, '/standard_dock_description'] @@ -218,7 +219,7 @@ def generate_launch_description(): '-x', x_dock, '-y', y, '-z', z, - '-Y', '3.141592', + '-Y', yaw_dock, '-topic', namespaced_dock_description], output='screen') @@ -244,7 +245,8 @@ def generate_launch_description(): ('localization', LaunchConfiguration('localization')), ('use_sim_time', LaunchConfiguration('use_sim_time')), ('map', LaunchConfiguration('map')), - ('namespace', namespace)] + ('namespace', namespace), + ('use_namespace', 'true')] ) turtlebot4_node = IncludeLaunchDescription( @@ -268,7 +270,9 @@ def generate_launch_description(): # RPLIDAR static transforms rplidar_stf = Node( + condition=LaunchConfigurationEquals('namespace', ''), name='rplidar_stf', + namespace=namespace, package='tf2_ros', executable='static_transform_publisher', output='screen', @@ -277,9 +281,23 @@ def generate_launch_description(): 'rplidar_link', [LaunchConfiguration('robot_name'), '/rplidar_link/rplidar']] ) + rplidar_stf_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + name='rplidar_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0.0', '0.0', + ['/', namespace, '/rplidar_link'], [LaunchConfiguration('robot_name'), '/rplidar_link/rplidar']] + ) + # OAKD static transforms oakd_pro_stf = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), name='camera_stf', + namespace=namespace, package='tf2_ros', executable='static_transform_publisher', output='screen', @@ -287,12 +305,27 @@ def generate_launch_description(): '0', '0', '0', '0', '0', '0', 'oakd_pro_rgb_camera_optical_frame', [LaunchConfiguration('robot_name'), '/oakd_pro_rgb_camera_frame/rgbd_camera'] + ] + ) + + oakd_pro_stf_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'standard'"])), + name='camera_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0', '0', + ['/', namespace, '/oakd_pro_rgb_camera_optical_frame'], + [LaunchConfiguration('robot_name'), '/oakd_pro_rgb_camera_frame/rgbd_camera'] ], - condition=LaunchConfigurationEquals('model', 'standard') ) oakd_lite_stf = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'lite'"])), name='camera_stf', + namespace=namespace, package='tf2_ros', executable='static_transform_publisher', output='screen', @@ -300,10 +333,23 @@ def generate_launch_description(): '0', '0', '0', '0', '0', '0', 'oakd_lite_rgb_camera_optical_frame', [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] - ], - condition=LaunchConfigurationEquals('model', 'lite') + ] ) + oakd_lite_stf_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'lite'"])), + name='camera_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0', '0', + ['/', namespace, '/oakd_lite_rgb_camera_optical_frame'], + [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] + ] + ) + # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) ld.add_action(ign_resource_path) @@ -324,4 +370,7 @@ def generate_launch_description(): ld.add_action(rplidar_stf) ld.add_action(oakd_pro_stf) ld.add_action(oakd_lite_stf) + ld.add_action(rplidar_stf_namespaced) + ld.add_action(oakd_pro_stf_namespaced) + ld.add_action(oakd_lite_stf_namespaced) return ld From d5c1132d71522d9db20721900c48b24239918f9c Mon Sep 17 00:00:00 2001 From: techlabs Date: Mon, 17 Oct 2022 19:15:42 +0200 Subject: [PATCH 09/16] separate ignition launch and turtlebot4-spawn to call robot spawn separatly --- .../launch/ignition.launch.py | 230 ++----------- .../launch/turtlebot4_spawn.launch.py | 313 ++++++++++++++++++ 2 files changed, 333 insertions(+), 210 deletions(-) create mode 100644 turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index da16136..07d1352 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -22,9 +22,8 @@ from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable -from launch.conditions import IfCondition, LaunchConfigurationEquals, LaunchConfigurationNotEquals from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -92,11 +91,7 @@ def generate_launch_description(): 'turtlebot4_description') pkg_turtlebot4_navigation = get_package_share_directory( 'turtlebot4_navigation') - pkg_turtlebot4_viz = get_package_share_directory( - 'turtlebot4_viz') - pkg_irobot_create_common_bringup = get_package_share_directory( - 'irobot_create_common_bringup') pkg_irobot_create_description = get_package_share_directory( 'irobot_create_description') pkg_irobot_create_ignition_bringup = get_package_share_directory( @@ -125,22 +120,6 @@ def generate_launch_description(): # Paths ign_gazebo_launch = PathJoinSubstitution( [pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py']) - turtlebot4_ros_ign_bridge_launch = PathJoinSubstitution( - [pkg_turtlebot4_ignition_bringup, 'launch', 'ros_ign_bridge.launch.py']) - rviz_launch = PathJoinSubstitution( - [pkg_turtlebot4_viz, 'launch', 'view_robot.launch.py']) - nav_launch = PathJoinSubstitution( - [pkg_turtlebot4_navigation, 'launch', 'nav_bringup.launch.py']) - node_launch = PathJoinSubstitution( - [pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_nodes.launch.py']) - create3_nodes_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'create3_nodes.launch.py']) - create3_ignition_nodes_launch = PathJoinSubstitution( - [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ignition_nodes.launch.py']) - robot_description_launch = PathJoinSubstitution( - [pkg_turtlebot4_description, 'launch', 'robot_description.launch.py']) - dock_description_launch = PathJoinSubstitution( - [pkg_irobot_create_common_bringup, 'launch', 'dock_description.launch.py']) # Parameters param_file_cmd = DeclareLaunchArgument( @@ -156,14 +135,8 @@ def generate_launch_description(): description='Full path to map yaml file to load') # Launch configurations - x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') - yaw = LaunchConfiguration('yaw') - turtlebot4_node_yaml_file = LaunchConfiguration('param_file') robot_name = LaunchConfiguration('robot_name') - model = LaunchConfiguration('model') namespace = LaunchConfiguration('namespace') - namespaced_robot_description = [namespace, '/robot_description'] - namespaced_dock_description = [namespace, '/standard_dock_description'] # Ignition gazebo ignition_gazebo = IncludeLaunchDescription( @@ -180,175 +153,27 @@ def generate_launch_description(): ] ) - # Robot description - robot_description_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([robot_description_launch]), - launch_arguments={'gazebo': 'ignition', 'namespace': namespace}.items()) - - # Dock description - x_dock = OffsetParser(x, 0.157) - yaw_dock = OffsetParser(yaw, 3.1416) - dock_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource([dock_description_launch]), - condition=IfCondition(LaunchConfiguration('spawn_dock')), - # The robot starts docked - launch_arguments={'x': x_dock, 'y': y, 'z': z, 'yaw': yaw_dock, + # Turtlebot4 + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(pkg_turtlebot4_ignition_bringup, 'launch', + 'turtlebot4_spawn.launch.py')), + launch_arguments={'x': LaunchConfiguration('x'), + 'y': LaunchConfiguration('y'), + 'z': LaunchConfiguration('z'), + 'yaw': LaunchConfiguration('yaw'), + 'robot_name': robot_name, + 'robot_description': '/robot_description', + 'world' : LaunchConfiguration('world'), + 'model' : LaunchConfiguration('model'), + 'slam' : LaunchConfiguration('slam'), + 'localization' : LaunchConfiguration('localization'), + 'nav2' : LaunchConfiguration('nav2'), + 'rviz' : LaunchConfiguration('rviz'), 'namespace': namespace, - 'gazebo': 'ignition'}.items() - ) - - # Spawn Turtlebot4 - spawn_robot = Node( - package='ros_ign_gazebo', - executable='create', - arguments=[ - '-name', robot_name, - '-x', x, - '-y', y, - '-z', z, - '-Y', yaw, - '-topic', namespaced_robot_description], - output='screen') - - # Spawn dock - spawn_dock = Node( - package='ros_ign_gazebo', - executable='create', - arguments=[ - '-name', (robot_name, '_standard_dock'), - '-x', x_dock, - '-y', y, - '-z', z, - '-Y', yaw_dock, - '-topic', namespaced_dock_description], - output='screen') - - # ROS Ign bridge - turtlebot4_ros_ign_bridge = IncludeLaunchDescription( - PythonLaunchDescriptionSource([turtlebot4_ros_ign_bridge_launch]), - launch_arguments=[('model', LaunchConfiguration('model')), - ('robot_name', robot_name), - ('namespace', namespace)] - ) - - # Rviz2 - rviz2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource([rviz_launch]), - condition=IfCondition(LaunchConfiguration('rviz')), - ) - - # Navigation - navigation = IncludeLaunchDescription( - PythonLaunchDescriptionSource([nav_launch]), - launch_arguments=[('slam', LaunchConfiguration('slam')), - ('nav2', LaunchConfiguration('nav2')), - ('localization', LaunchConfiguration('localization')), - ('use_sim_time', LaunchConfiguration('use_sim_time')), - ('map', LaunchConfiguration('map')), - ('namespace', namespace), - ('use_namespace', 'true')] - ) - - turtlebot4_node = IncludeLaunchDescription( - PythonLaunchDescriptionSource([node_launch]), - launch_arguments=[('model', LaunchConfiguration('model')), - ('param_file', turtlebot4_node_yaml_file), - ('namespace', namespace)] - ) - - # Create3 nodes - create3_nodes = IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_nodes_launch]), - launch_arguments=[('namespace', namespace)] - ) - - create3_ignition_nodes = IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), - launch_arguments=[('robot_name', LaunchConfiguration('robot_name')), - ('namespace', namespace)] - ) - - # RPLIDAR static transforms - rplidar_stf = Node( - condition=LaunchConfigurationEquals('namespace', ''), - name='rplidar_stf', - namespace=namespace, - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '0', '0', '0', '0', '0.0', '0.0', - 'rplidar_link', [LaunchConfiguration('robot_name'), '/rplidar_link/rplidar']] - ) - - rplidar_stf_namespaced = Node( - condition=LaunchConfigurationNotEquals('namespace', ''), - name='rplidar_stf', - namespace=namespace, - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '0', '0', '0', '0', '0.0', '0.0', - ['/', namespace, '/rplidar_link'], [LaunchConfiguration('robot_name'), '/rplidar_link/rplidar']] - ) - - # OAKD static transforms - oakd_pro_stf = Node( - condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), - name='camera_stf', - namespace=namespace, - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '0', '0', '0', '0', '0', '0', - 'oakd_pro_rgb_camera_optical_frame', - [LaunchConfiguration('robot_name'), '/oakd_pro_rgb_camera_frame/rgbd_camera'] - ] - ) - - oakd_pro_stf_namespaced = Node( - condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'standard'"])), - name='camera_stf', - namespace=namespace, - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '0', '0', '0', '0', '0', '0', - ['/', namespace, '/oakd_pro_rgb_camera_optical_frame'], - [LaunchConfiguration('robot_name'), '/oakd_pro_rgb_camera_frame/rgbd_camera'] - ], - ) - - oakd_lite_stf = Node( - condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'lite'"])), - name='camera_stf', - namespace=namespace, - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '0', '0', '0', '0', '0', '0', - 'oakd_lite_rgb_camera_optical_frame', - [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] - ] - ) - - oakd_lite_stf_namespaced = Node( - condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'lite'"])), - name='camera_stf', - namespace=namespace, - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '0', '0', '0', '0', '0', '0', - ['/', namespace, '/oakd_lite_rgb_camera_optical_frame'], - [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] - ] + 'spawn_dock': LaunchConfiguration('spawn_dock'), + }.items() ) + # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) @@ -357,20 +182,5 @@ def generate_launch_description(): ld.add_action(param_file_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(ignition_gazebo) - ld.add_action(turtlebot4_ros_ign_bridge) - ld.add_action(rviz2) - ld.add_action(robot_description_launch) - ld.add_action(dock_description) ld.add_action(spawn_robot) - ld.add_action(spawn_dock) - ld.add_action(create3_nodes) - ld.add_action(create3_ignition_nodes) - ld.add_action(turtlebot4_node) - ld.add_action(navigation) - ld.add_action(rplidar_stf) - ld.add_action(oakd_pro_stf) - ld.add_action(oakd_lite_stf) - ld.add_action(rplidar_stf_namespaced) - ld.add_action(oakd_pro_stf_namespaced) - ld.add_action(oakd_lite_stf_namespaced) return ld diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py new file mode 100644 index 0000000..0e27e54 --- /dev/null +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py @@ -0,0 +1,313 @@ +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +from launch.conditions import IfCondition, LaunchConfigurationEquals, LaunchConfigurationNotEquals +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression + +from launch_ros.actions import Node + + +class OffsetParser(Substitution): + def __init__( + self, + number: SomeSubstitutionsType, + offset: float, + ) -> None: + self.__number = number + self.__offset = offset + + def perform( + self, + context: LaunchContext = None, + ) -> str: + number = float(self.__number.perform(context)) + return f'{number + self.__offset}' + + +ARGUMENTS = [ + DeclareLaunchArgument('rviz', default_value='false', + choices=['true', 'false'], + description='Start rviz.'), + DeclareLaunchArgument('slam', default_value='off', + choices=['off', 'sync', 'async'], + description='Whether to run a SLAM'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], + description='Whether to run localization'), + DeclareLaunchArgument('nav2', default_value='false', + choices=['true', 'false'], + description='Run nav2'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('world', default_value='depot', + description='Ignition World'), + DeclareLaunchArgument('model', default_value='standard', + choices=['standard', 'lite'], + description='Turtlebot4 Model'), + DeclareLaunchArgument('spawn_dock', default_value='true', + choices=['true', 'false'], + description='Dock Model.'), + DeclareLaunchArgument('robot_name', default_value='turtlebot4', + description='Robot name'), + DeclareLaunchArgument('namespace', default_value='', + description='robot namespace'), +] + +for pose_element in ['x', 'y', 'z', 'yaw']: + ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', + description=f'{pose_element} component of the robot pose.')) + + +def generate_launch_description(): + + # Directories + pkg_turtlebot4_ignition_bringup = get_package_share_directory( + 'turtlebot4_ignition_bringup') + pkg_turtlebot4_description = get_package_share_directory( + 'turtlebot4_description') + pkg_turtlebot4_navigation = get_package_share_directory( + 'turtlebot4_navigation') + pkg_turtlebot4_viz = get_package_share_directory( + 'turtlebot4_viz') + pkg_irobot_create_common_bringup = get_package_share_directory( + 'irobot_create_common_bringup') + pkg_irobot_create_ignition_bringup = get_package_share_directory( + 'irobot_create_ignition_bringup') + + # Paths + turtlebot4_ros_ign_bridge_launch = PathJoinSubstitution( + [pkg_turtlebot4_ignition_bringup, 'launch', 'ros_ign_bridge.launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_turtlebot4_viz, 'launch', 'view_robot.launch.py']) + nav_launch = PathJoinSubstitution( + [pkg_turtlebot4_navigation, 'launch', 'nav_bringup.launch.py']) + node_launch = PathJoinSubstitution( + [pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_nodes.launch.py']) + create3_nodes_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'create3_nodes.launch.py']) + create3_ignition_nodes_launch = PathJoinSubstitution( + [pkg_irobot_create_ignition_bringup, 'launch', 'create3_ignition_nodes.launch.py']) + robot_description_launch = PathJoinSubstitution( + [pkg_turtlebot4_description, 'launch', 'robot_description.launch.py']) + dock_description_launch = PathJoinSubstitution( + [pkg_irobot_create_common_bringup, 'launch', 'dock_description.launch.py']) + + # Parameters + param_file_cmd = DeclareLaunchArgument( + 'param_file', + default_value=PathJoinSubstitution( + [pkg_turtlebot4_ignition_bringup, 'config', 'turtlebot4_node.yaml']), + description='Turtlebot4 Robot param file') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=PathJoinSubstitution( + [pkg_turtlebot4_navigation, 'maps', 'depot.yaml']), + description='Full path to map yaml file to load') + + # Launch configurations + x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z') + yaw = LaunchConfiguration('yaw') + turtlebot4_node_yaml_file = LaunchConfiguration('param_file') + robot_name = LaunchConfiguration('robot_name') + model = LaunchConfiguration('model') + namespace = LaunchConfiguration('namespace') + namespaced_robot_description = [namespace, '/robot_description'] + namespaced_dock_description = [namespace, '/standard_dock_description'] + + + # Robot description + robot_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([robot_description_launch]), + launch_arguments={'gazebo': 'ignition', 'namespace': namespace}.items()) + + # Dock description + x_dock = OffsetParser(x, 0.157) + yaw_dock = OffsetParser(yaw, 3.1416) + dock_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource([dock_description_launch]), + condition=IfCondition(LaunchConfiguration('spawn_dock')), + # The robot starts docked + launch_arguments={'x': x_dock, 'y': y, 'z': z, 'yaw': yaw_dock, + 'namespace': namespace, + 'gazebo': 'ignition'}.items() + ) + + # Spawn Turtlebot4 + spawn_robot = Node( + package='ros_ign_gazebo', + executable='create', + arguments=[ + '-name', robot_name, + '-x', x, + '-y', y, + '-z', z, + '-Y', yaw, + '-topic', namespaced_robot_description], + output='screen') + + # Spawn dock + spawn_dock = Node( + package='ros_ign_gazebo', + executable='create', + arguments=[ + '-name', (robot_name, '_standard_dock'), + '-x', x_dock, + '-y', y, + '-z', z, + '-Y', yaw_dock, + '-topic', namespaced_dock_description], + output='screen') + + # ROS Ign bridge + turtlebot4_ros_ign_bridge = IncludeLaunchDescription( + PythonLaunchDescriptionSource([turtlebot4_ros_ign_bridge_launch]), + launch_arguments=[('model', LaunchConfiguration('model')), + ('robot_name', robot_name), + ('namespace', namespace)] + ) + + # Rviz2 + rviz2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]), + condition=IfCondition(LaunchConfiguration('rviz')), + ) + + # Navigation + navigation = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav_launch]), + launch_arguments=[('slam', LaunchConfiguration('slam')), + ('nav2', LaunchConfiguration('nav2')), + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', LaunchConfiguration('use_sim_time')), + ('map', LaunchConfiguration('map')), + ('namespace', namespace), + ('use_namespace', 'true')] + ) + + turtlebot4_node = IncludeLaunchDescription( + PythonLaunchDescriptionSource([node_launch]), + launch_arguments=[('model', LaunchConfiguration('model')), + ('param_file', turtlebot4_node_yaml_file), + ('namespace', namespace)] + ) + + # Create3 nodes + create3_nodes = IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_nodes_launch]), + launch_arguments=[('namespace', namespace)] + ) + + create3_ignition_nodes = IncludeLaunchDescription( + PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), + launch_arguments=[('robot_name', LaunchConfiguration('robot_name')), + ('namespace', namespace)] + ) + + # RPLIDAR static transforms + rplidar_stf = Node( + condition=LaunchConfigurationEquals('namespace', ''), + name='rplidar_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0.0', '0.0', + 'rplidar_link', [LaunchConfiguration('robot_name'), '/rplidar_link/rplidar']] + ) + + rplidar_stf_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + name='rplidar_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0.0', '0.0', + ['/', namespace, '/rplidar_link'], [LaunchConfiguration('robot_name'), '/rplidar_link/rplidar']] + ) + + # OAKD static transforms + oakd_pro_stf = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), + name='camera_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0', '0', + 'oakd_pro_rgb_camera_optical_frame', + [LaunchConfiguration('robot_name'), '/oakd_pro_rgb_camera_frame/rgbd_camera'] + ] + ) + + oakd_pro_stf_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'standard'"])), + name='camera_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0', '0', + ['/', namespace, '/oakd_pro_rgb_camera_optical_frame'], + [LaunchConfiguration('robot_name'), '/oakd_pro_rgb_camera_frame/rgbd_camera'] + ], + ) + + oakd_lite_stf = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'lite'"])), + name='camera_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0', '0', + 'oakd_lite_rgb_camera_optical_frame', + [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] + ] + ) + + oakd_lite_stf_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'lite'"])), + name='camera_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0', '0', + ['/', namespace, '/oakd_lite_rgb_camera_optical_frame'], + [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] + ] + ) + + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) + ld.add_action(param_file_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(turtlebot4_ros_ign_bridge) + ld.add_action(rviz2) + ld.add_action(robot_description_launch) + ld.add_action(dock_description) + ld.add_action(spawn_robot) + ld.add_action(spawn_dock) + ld.add_action(create3_nodes) + ld.add_action(create3_ignition_nodes) + ld.add_action(turtlebot4_node) + ld.add_action(navigation) + ld.add_action(rplidar_stf) + ld.add_action(oakd_pro_stf) + ld.add_action(oakd_lite_stf) + ld.add_action(rplidar_stf_namespaced) + ld.add_action(oakd_pro_stf_namespaced) + ld.add_action(oakd_lite_stf_namespaced) + return ld From 79e5356143ccdf0f8e790b8361aad30d1dea9275 Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Tue, 18 Oct 2022 15:47:49 +0200 Subject: [PATCH 10/16] fix subscription HMI --- .../Turtlebot4Hmi/Turtlebot4Hmi.cc | 19 +++++++++---------- .../Turtlebot4Hmi/Turtlebot4Hmi.hh | 3 +++ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc index b403f5c..03daf73 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc @@ -73,7 +73,6 @@ void Turtlebot4Hmi::UpdateTopics() App()->findChild()->notifyWithDuration( QString::fromStdString("Advertising new topics based on robot name: " + this->namespace_), 4000); } - } void Turtlebot4Hmi::SetNamespace(const QString &_namespace) @@ -98,15 +97,15 @@ void Turtlebot4Hmi::SetNamespace(const QString &_namespace) { this->hmi_button_topic_ = "/model/" + this->namespace_ + "/hmi/buttons"; this->create3_button_topic_ = "/model/" + this->namespace_ + "/buttons"; - this->display_topic_ = "/model/" + this->namespace_ + "hmi/display/raw"; - this->display_selected_topic_ = "/model/" + this->namespace_ + "hmi/display/selected"; - this->power_led_topic_ = "/model/" + this->namespace_ + "hmi/led/power"; - this->motors_led_topic_ = "/model/" + this->namespace_ + "hmi/led/motors"; - this->comms_led_topic_ = "/model/" + this->namespace_ + "hmi/led/comms"; - this->wifi_led_topic_ = "/model/" + this->namespace_ + "hmi/led/wifi"; - this->battery_led_topic_ = "/model/" + this->namespace_ + "hmi/led/battery"; - this->user1_led_topic_ = "/model/" + this->namespace_ + "hmi/led/user1"; - this->user2_led_topic_ = "/model/" + this->namespace_ + "hmi/led/user2"; + this->display_topic_ = "/model/" + this->namespace_ + "/hmi/display/raw"; + this->display_selected_topic_ = "/model/" + this->namespace_ + "/hmi/display/selected"; + this->power_led_topic_ = "/model/" + this->namespace_ + "/hmi/led/power"; + this->motors_led_topic_ = "/model/" + this->namespace_ + "/hmi/led/motors"; + this->comms_led_topic_ = "/model/" + this->namespace_ + "/hmi/led/comms"; + this->wifi_led_topic_ = "/model/" + this->namespace_ + "/hmi/led/wifi"; + this->battery_led_topic_ = "/model/" + this->namespace_ + "/hmi/led/battery"; + this->user1_led_topic_ = "/model/" + this->namespace_ + "/hmi/led/user1"; + this->user2_led_topic_ = "/model/" + this->namespace_ + "/hmi/led/user2"; } else { diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh index b1f1096..b1e87c1 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.hh @@ -103,12 +103,15 @@ private slots: private: ignition::transport::Node node_; + ignition::transport::Node::Publisher hmi_button_pub_; ignition::transport::Node::Publisher create3_button_pub_; std::string namespace_ = "turtlebot4"; + std::string hmi_button_topic_ = "/model/turtlebot4/hmi/buttons"; std::string create3_button_topic_ = "/model/turtlebot4/buttons"; + std::string display_topic_ = "/model/turtlebot4/hmi/display/raw"; std::string display_selected_topic_ = "/model/turtlebot4/hmi/display/selected"; std::string power_led_topic_ = "/model/turtlebot4/hmi/led/power"; From 737854538e5fab2991cf7d0f23f4eb5138c1eec3 Mon Sep 17 00:00:00 2001 From: techlabs Date: Thu, 20 Oct 2022 15:26:26 +0200 Subject: [PATCH 11/16] adjust yaw for robot spawning --- turtlebot4_ignition_bringup/launch/ignition.launch.py | 4 +++- turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index 07d1352..79a9890 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -73,9 +73,11 @@ def perform( description='Robot name'), DeclareLaunchArgument('namespace', default_value='', description='robot namespace'), + DeclareLaunchArgument('yaw', default_value='3.145', + description='robot yaw rotation at spawn'), ] -for pose_element in ['x', 'y', 'z', 'yaw']: +for pose_element in ['x', 'y', 'z']: ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', description=f'{pose_element} component of the robot pose.')) diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py index 0e27e54..ee9318c 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py @@ -55,9 +55,11 @@ def perform( description='Robot name'), DeclareLaunchArgument('namespace', default_value='', description='robot namespace'), + DeclareLaunchArgument('yaw', default_value='3.145', + description='robot yaw rotation at spawn'), ] -for pose_element in ['x', 'y', 'z', 'yaw']: +for pose_element in ['x', 'y', 'z']: ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', description=f'{pose_element} component of the robot pose.')) From 1f6fd887b43b69f319d2b983dd90197f767cf4fc Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Fri, 21 Oct 2022 17:23:27 +0200 Subject: [PATCH 12/16] fix HMI entries config file --- .../config/turtlebot4_node.yaml | 123 +++++++++--------- .../launch/ignition.launch.py | 23 ---- .../launch/ros_ign_bridge.launch.py | 2 +- .../launch/turtlebot4_spawn.launch.py | 2 +- 4 files changed, 64 insertions(+), 86 deletions(-) diff --git a/turtlebot4_ignition_bringup/config/turtlebot4_node.yaml b/turtlebot4_ignition_bringup/config/turtlebot4_node.yaml index 6fadb80..348f8ea 100644 --- a/turtlebot4_ignition_bringup/config/turtlebot4_node.yaml +++ b/turtlebot4_ignition_bringup/config/turtlebot4_node.yaml @@ -1,62 +1,63 @@ -turtlebot4_node: - ros__parameters: - wifi: - interface: "wlan0" - - # Supported Functions: - # Dock - # Undock - # Wall Follow Left - # Wall Follow Right - # Power - # EStop - - # Menu Functions: - # Scroll Up - # Scroll Down - # Back - # Select - # Help - +/**: + turtlebot4_node: + ros__parameters: + wifi: + interface: "wlan0" + + # Supported Functions: + # Dock + # Undock + # Wall Follow Left + # Wall Follow Right + # Power + # EStop + + # Menu Functions: + # Scroll Up + # Scroll Down + # Back + # Select + # Help + + # Buttons: + # create3_1 + # create3_power + # create3_2 + # hmi_1 + # hmi_2 + # hmi_3 + # hmi_4 + + # Format: + # button: ["SHORT_PRESS_FUNC", "LONG_PRESS_FUNC", "LONG_PRESS_DURATION_MS"] + + buttons: + create3_1: ["Dock", "Wall Follow Left", "2000"] + create3_power: ["EStop", "Power", "3000"] + create3_2: ["Undock", "Wall Follow Right", "2000"] + + hmi_1: ["Select"] + hmi_2: ["Back"] + hmi_3: ["Scroll Up"] + hmi_4: ["Scroll Down"] + + # Menu entry must match a function + menu: + entries: ["Dock", "Undock", "EStop", "Wall Follow Left", "Wall Follow Right", "Power", "Help"] + + # Controller button functions # Buttons: - # create3_1 - # create3_power - # create3_2 - # hmi_1 - # hmi_2 - # hmi_3 - # hmi_4 - - # Format: - # button: ["SHORT_PRESS_FUNC", "LONG_PRESS_FUNC", "LONG_PRESS_DURATION_MS"] - - buttons: - create3_1: ["Dock", "Wall Follow Left", "2000"] - create3_power: ["EStop", "Power", "3000"] - create3_2: ["Undock", "Wall Follow Right", "2000"] - - hmi_1: ["Select"] - hmi_2: ["Back"] - hmi_3: ["Scroll Up"] - hmi_4: ["Scroll Down"] - - # Menu entry must match a function - menu: - entries: ["Dock", "Undock", "EStop", "Wall Follow Left", "Wall Follow Right", "Power", "Help"] - - # Controller button functions - # Buttons: - # a b x y - # up down left right - # l1 l2 l3 r1 r2 r3 - # share options home - - controller: - b: ["EStop"] - a: ["Select"] - x: ["Back"] - up: ["Scroll Up"] - down: ["Scroll Down"] - l2: ["Wall Follow Left"] - r2: ["Wall Follow Right"] - home: ["Dock", "Undock", "3000"] + # a b x y + # up down left right + # l1 l2 l3 r1 r2 r3 + # share options home + + controller: + b: ["EStop"] + a: ["Select"] + x: ["Back"] + up: ["Scroll Up"] + down: ["Scroll Down"] + l2: ["Wall Follow Left"] + r2: ["Wall Follow Right"] + home: ["Dock", "Undock", "3000"] diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index 79a9890..588bb11 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -25,9 +25,6 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node - - class OffsetParser(Substitution): def __init__( self, @@ -44,7 +41,6 @@ def perform( number = float(self.__number.perform(context)) return f'{number + self.__offset}' - ARGUMENTS = [ DeclareLaunchArgument('rviz', default_value='false', choices=['true', 'false'], @@ -81,7 +77,6 @@ def perform( ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', description=f'{pose_element} component of the robot pose.')) - def generate_launch_description(): # Directories @@ -91,8 +86,6 @@ def generate_launch_description(): 'turtlebot4_ignition_gui_plugins') pkg_turtlebot4_description = get_package_share_directory( 'turtlebot4_description') - pkg_turtlebot4_navigation = get_package_share_directory( - 'turtlebot4_navigation') pkg_irobot_create_description = get_package_share_directory( 'irobot_create_description') @@ -123,19 +116,6 @@ def generate_launch_description(): ign_gazebo_launch = PathJoinSubstitution( [pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py']) - # Parameters - param_file_cmd = DeclareLaunchArgument( - 'param_file', - default_value=PathJoinSubstitution( - [pkg_turtlebot4_ignition_bringup, 'config', 'turtlebot4_node.yaml']), - description='Turtlebot4 Robot param file') - - declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value=PathJoinSubstitution( - [pkg_turtlebot4_navigation, 'maps', 'depot.yaml']), - description='Full path to map yaml file to load') - # Launch configurations robot_name = LaunchConfiguration('robot_name') namespace = LaunchConfiguration('namespace') @@ -176,13 +156,10 @@ def generate_launch_description(): }.items() ) - # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) ld.add_action(ign_resource_path) ld.add_action(ign_gui_plugin_path) - ld.add_action(param_file_cmd) - ld.add_action(declare_map_yaml_cmd) ld.add_action(ignition_gazebo) ld.add_action(spawn_robot) return ld diff --git a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py index d8d2c52..bbbec4a 100644 --- a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py +++ b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py @@ -204,7 +204,7 @@ def generate_launch_description(): ['/', namespace, '/hmi/buttons/_set']) ]) - # Buttons message bridge + # Led message bridge hmi_led_msg_bridge = Node( condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py index ee9318c..97b11ed 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py @@ -2,7 +2,7 @@ from launch import LaunchContext, LaunchDescription, SomeSubstitutionsType, Substitution from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition, LaunchConfigurationEquals, LaunchConfigurationNotEquals from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression From 0fa7aa290b8f4846fb419971864587a59977382f Mon Sep 17 00:00:00 2001 From: techlabs Date: Mon, 24 Oct 2022 18:02:06 +0200 Subject: [PATCH 13/16] namespacing option for navigation --- .../launch/turtlebot4_spawn.launch.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py index 97b11ed..1cd7689 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py @@ -182,6 +182,18 @@ def generate_launch_description(): # Navigation navigation = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav_launch]), + condition=LaunchConfigurationEquals('namespace', ''), + launch_arguments=[('slam', LaunchConfiguration('slam')), + ('nav2', LaunchConfiguration('nav2')), + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', LaunchConfiguration('use_sim_time')), + ('map', LaunchConfiguration('map')), + ('use_namespace', 'false')] + ) + + navigation_namespaced = IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav_launch]), + condition=LaunchConfigurationNotEquals('namespace', ''), launch_arguments=[('slam', LaunchConfiguration('slam')), ('nav2', LaunchConfiguration('nav2')), ('localization', LaunchConfiguration('localization')), @@ -306,6 +318,7 @@ def generate_launch_description(): ld.add_action(create3_ignition_nodes) ld.add_action(turtlebot4_node) ld.add_action(navigation) + ld.add_action(navigation_namespaced) ld.add_action(rplidar_stf) ld.add_action(oakd_pro_stf) ld.add_action(oakd_lite_stf) From e7be24d4288abc3f145b98a242482b93f06d291f Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Mon, 28 Nov 2022 18:59:00 +0100 Subject: [PATCH 14/16] multi_robot launch with params for 2 robots --- .../config/nav2_multirobot_params_1.yaml | 288 ++++++++++++++++++ .../config/nav2_multirobot_params_2.yaml | 288 ++++++++++++++++++ .../launch/ignition.launch.py | 1 - .../launch/ros_ign_bridge.launch.py | 24 +- .../launch/test_multi_robot_launch.py | 189 ++++++++++++ .../launch/turtlebot4_spawn.launch.py | 71 ++--- 6 files changed, 801 insertions(+), 60 deletions(-) create mode 100644 turtlebot4_ignition_bringup/config/nav2_multirobot_params_1.yaml create mode 100644 turtlebot4_ignition_bringup/config/nav2_multirobot_params_2.yaml create mode 100755 turtlebot4_ignition_bringup/launch/test_multi_robot_launch.py diff --git a/turtlebot4_ignition_bringup/config/nav2_multirobot_params_1.yaml b/turtlebot4_ignition_bringup/config/nav2_multirobot_params_1.yaml new file mode 100644 index 0000000..371b223 --- /dev/null +++ b/turtlebot4_ignition_bringup/config/nav2_multirobot_params_1.yaml @@ -0,0 +1,288 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # if enable_groot_monitoring is set to True, ports need to be different for each robot !! + enable_groot_monitoring: False + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: "goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: True + global_frame: odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["obstacle_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + scan: + topic: /robot1/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + observation_sources: scan + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: True + robot_radius: 0.22 + obstacle_layer: + enabled: True + scan: + topic: /robot1/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + observation_sources: scan + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + save_map_timeout: 5.0 + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/turtlebot4_ignition_bringup/config/nav2_multirobot_params_2.yaml b/turtlebot4_ignition_bringup/config/nav2_multirobot_params_2.yaml new file mode 100644 index 0000000..6396005 --- /dev/null +++ b/turtlebot4_ignition_bringup/config/nav2_multirobot_params_2.yaml @@ -0,0 +1,288 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # if enable_groot_monitoring is set to True, ports need to be different for each robot !! + enable_groot_monitoring: False + groot_zmq_publisher_port: 1789 + groot_zmq_server_port: 1887 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: "goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: True + global_frame: odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["obstacle_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + scan: + topic: /robot2/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + observation_sources: scan + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: True + robot_radius: 0.22 + obstacle_layer: + enabled: True + scan: + topic: /robot2/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + observation_sources: scan + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + save_map_timeout: 5.0 + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index 588bb11..c3d6d9e 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -144,7 +144,6 @@ def generate_launch_description(): 'z': LaunchConfiguration('z'), 'yaw': LaunchConfiguration('yaw'), 'robot_name': robot_name, - 'robot_description': '/robot_description', 'world' : LaunchConfiguration('world'), 'model' : LaunchConfiguration('model'), 'slam' : LaunchConfiguration('slam'), diff --git a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py index bbbec4a..5456f31 100644 --- a/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py +++ b/turtlebot4_ignition_bringup/launch/ros_ign_bridge.launch.py @@ -274,23 +274,19 @@ def generate_launch_description(): ], remappings=[ (['/world/', world, - '/model/', - robot_name, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image'], '/color/image'), (['/world/', world, - '/model/', - robot_name, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image'], '/stereo/depth'), (['/world/', world, - '/model/', - robot_name, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points'], '/stereo/depth/points'), (['/world/', world, - '/model/', - robot_name, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info'], '/color/camera_info') ]) @@ -327,23 +323,19 @@ def generate_launch_description(): ], remappings=[ (['/world/', world, - '/model/', - robot_name, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image'], ['/', namespace, '/color/image']), (['/world/', world, - '/model/', - robot_name, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image'], ['/', namespace, '/stereo/depth']), (['/world/', world, - '/model/', - robot_name, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points'], ['/', namespace, '/stereo/depth/points']), (['/world/', world, - '/model/', - robot_name, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info'], ['/', namespace, '/color/camera_info']) ]) diff --git a/turtlebot4_ignition_bringup/launch/test_multi_robot_launch.py b/turtlebot4_ignition_bringup/launch/test_multi_robot_launch.py new file mode 100755 index 0000000..4ac4cb1 --- /dev/null +++ b/turtlebot4_ignition_bringup/launch/test_multi_robot_launch.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, GroupAction +from launch_ros.actions import PushRosNamespace +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('world', default_value='depot', + description='Ignition World'), + DeclareLaunchArgument('model', default_value='standard', + choices=['standard', 'lite'], + description='Turtlebot4 Model'), + DeclareLaunchArgument('spawn_dock', default_value='true', + choices=['true', 'false'], + description='Dock Model.'), +] + +def generate_launch_description(): + + # Directories + pkg_turtlebot4_ignition_bringup = get_package_share_directory( + 'turtlebot4_ignition_bringup') + pkg_turtlebot4_ignition_gui_plugins = get_package_share_directory( + 'turtlebot4_ignition_gui_plugins') + pkg_turtlebot4_description = get_package_share_directory( + 'turtlebot4_description') + pkg_turtlebot4_navigation = get_package_share_directory( + 'turtlebot4_navigation') + pkg_navigation2_bringup = get_package_share_directory( + 'nav2_bringup') + + pkg_irobot_create_description = get_package_share_directory( + 'irobot_create_description') + pkg_irobot_create_ignition_bringup = get_package_share_directory( + 'irobot_create_ignition_bringup') + pkg_irobot_create_ignition_plugins = get_package_share_directory( + 'irobot_create_ignition_plugins') + + pkg_ros_ign_gazebo = get_package_share_directory( + 'ros_ign_gazebo') + + # Set ignition resource path + ign_resource_path = SetEnvironmentVariable( + name='IGN_GAZEBO_RESOURCE_PATH', + value=[ + os.path.join(pkg_turtlebot4_ignition_bringup, 'worlds'), ':' + + os.path.join(pkg_irobot_create_ignition_bringup, 'worlds'), ':' + + str(Path(pkg_turtlebot4_description).parent.resolve()), ':' + + str(Path(pkg_irobot_create_description).parent.resolve())]) + + ign_gui_plugin_path = SetEnvironmentVariable( + name='IGN_GUI_PLUGIN_PATH', + value=[ + os.path.join(pkg_turtlebot4_ignition_gui_plugins, 'lib'), ':' + + os.path.join(pkg_irobot_create_ignition_plugins, 'lib')]) + + # Paths + ign_gazebo_launch = PathJoinSubstitution( + [pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py']) + + + map_yaml_file = os.path.join(pkg_turtlebot4_navigation, 'maps/depot.yaml') + bt_xml_file = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + 'navigate_to_pose_w_replanning_and_recovery.xml') + + robot1_params_file = os.path.join(pkg_turtlebot4_ignition_bringup, + 'config/nav2_multirobot_params_1.yaml') + robot2_params_file = os.path.join(pkg_turtlebot4_ignition_bringup, + 'config/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 Ignition gazebo server for simulation + ignition_gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ign_gazebo_launch]), + launch_arguments=[ + ('ign_args', [ + LaunchConfiguration('world'), '.sdf', + ' -v 4', + ' --gui-config ', PathJoinSubstitution( + [pkg_turtlebot4_ignition_bringup, + 'gui', + LaunchConfiguration('model'), + 'gui.config'])]) + ] + ) + + # Define commands for spawing the robots into Gazebo + spawn_robots_cmds = [] + for robot in robots: + spawn_robots_cmds.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + pkg_turtlebot4_ignition_bringup, + 'launch', + 'turtlebot4_spawn.launch.py' + )), + launch_arguments={ + 'x': TextSubstitution(text=str(robot['x_pose'])), + 'y': TextSubstitution(text=str(robot['y_pose'])), + 'z': TextSubstitution(text=str(robot['z_pose'])), + 'robot_name': TextSubstitution(text=robot['name']), + 'namespace': TextSubstitution(text=robot['name']), + 'model' : LaunchConfiguration('model'), + 'spawn_dock': LaunchConfiguration('spawn_dock'), + }.items() + ) + ) + + # Define commands for launching the robot state publishers + # robot_state_pubs_cmds = [] + # for robot in robots: + # robot_state_pubs_cmds.append( + # Node( + # package='robot_state_publisher', + # executable='robot_state_publisher', + # namespace=TextSubstitution(text=robot['name']), + # output='screen', + # parameters=[{'use_sim_time': 'True'}], + # remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + # arguments=[urdf])) + + # Define commands for launching the navigation instances + nav_instances_cmds = [] + for robot in robots: + params_file = eval(f"{robot['name']}_params_file") + + group = GroupAction([ + # Instances use the robot's name for namespace + PushRosNamespace(robot['name']), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_navigation2_bringup, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'namespace': robot['name'], + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'bt_xml_file': bt_xml_file, + 'autostart': 'True', + 'use_remappings': 'True'}.items()) + ]) + nav_instances_cmds.append(group) + + + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) + #ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),) + #ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),) + ld.add_action(ign_resource_path) + ld.add_action(ign_gui_plugin_path) + ld.add_action(ignition_gazebo) + 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 diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py index 1cd7689..dbcb5e0 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py @@ -31,20 +31,9 @@ def perform( DeclareLaunchArgument('rviz', default_value='false', choices=['true', 'false'], description='Start rviz.'), - DeclareLaunchArgument('slam', default_value='off', - choices=['off', 'sync', 'async'], - description='Whether to run a SLAM'), - DeclareLaunchArgument('localization', default_value='false', - choices=['true', 'false'], - description='Whether to run localization'), - DeclareLaunchArgument('nav2', default_value='false', - choices=['true', 'false'], - description='Run nav2'), DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], description='use_sim_time'), - DeclareLaunchArgument('world', default_value='depot', - description='Ignition World'), DeclareLaunchArgument('model', default_value='standard', choices=['standard', 'lite'], description='Turtlebot4 Model'), @@ -57,6 +46,7 @@ def perform( description='robot namespace'), DeclareLaunchArgument('yaw', default_value='3.145', description='robot yaw rotation at spawn'), + ] for pose_element in ['x', 'y', 'z']: @@ -85,8 +75,6 @@ def generate_launch_description(): [pkg_turtlebot4_ignition_bringup, 'launch', 'ros_ign_bridge.launch.py']) rviz_launch = PathJoinSubstitution( [pkg_turtlebot4_viz, 'launch', 'view_robot.launch.py']) - nav_launch = PathJoinSubstitution( - [pkg_turtlebot4_navigation, 'launch', 'nav_bringup.launch.py']) node_launch = PathJoinSubstitution( [pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_nodes.launch.py']) create3_nodes_launch = PathJoinSubstitution( @@ -121,7 +109,6 @@ def generate_launch_description(): namespaced_robot_description = [namespace, '/robot_description'] namespaced_dock_description = [namespace, '/standard_dock_description'] - # Robot description robot_description_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([robot_description_launch]), @@ -151,6 +138,18 @@ def generate_launch_description(): '-Y', yaw, '-topic', namespaced_robot_description], output='screen') + + spawn_robot_namespaced = Node( + package='ros_ign_gazebo', + executable='create', + arguments=[ + '-name', robot_name, + '-x', x, + '-y', y, + '-z', z, + '-Y', yaw, + '-topic', namespaced_robot_description], + output='screen') # Spawn dock spawn_dock = Node( @@ -177,30 +176,7 @@ def generate_launch_description(): rviz2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([rviz_launch]), condition=IfCondition(LaunchConfiguration('rviz')), - ) - - # Navigation - navigation = IncludeLaunchDescription( - PythonLaunchDescriptionSource([nav_launch]), - condition=LaunchConfigurationEquals('namespace', ''), - launch_arguments=[('slam', LaunchConfiguration('slam')), - ('nav2', LaunchConfiguration('nav2')), - ('localization', LaunchConfiguration('localization')), - ('use_sim_time', LaunchConfiguration('use_sim_time')), - ('map', LaunchConfiguration('map')), - ('use_namespace', 'false')] - ) - - navigation_namespaced = IncludeLaunchDescription( - PythonLaunchDescriptionSource([nav_launch]), - condition=LaunchConfigurationNotEquals('namespace', ''), - launch_arguments=[('slam', LaunchConfiguration('slam')), - ('nav2', LaunchConfiguration('nav2')), - ('localization', LaunchConfiguration('localization')), - ('use_sim_time', LaunchConfiguration('use_sim_time')), - ('map', LaunchConfiguration('map')), - ('namespace', namespace), - ('use_namespace', 'true')] + launch_arguments=[('namespace', namespace)] ) turtlebot4_node = IncludeLaunchDescription( @@ -244,7 +220,7 @@ def generate_launch_description(): output='screen', arguments=[ '0', '0', '0', '0', '0.0', '0.0', - ['/', namespace, '/rplidar_link'], [LaunchConfiguration('robot_name'), '/rplidar_link/rplidar']] + [namespace, '/rplidar_link'], [LaunchConfiguration('robot_name'), '/rplidar_link/rplidar']] ) # OAKD static transforms @@ -271,7 +247,7 @@ def generate_launch_description(): output='screen', arguments=[ '0', '0', '0', '0', '0', '0', - ['/', namespace, '/oakd_pro_rgb_camera_optical_frame'], + [namespace, '/oakd_pro_rgb_camera_optical_frame'], [LaunchConfiguration('robot_name'), '/oakd_pro_rgb_camera_frame/rgbd_camera'] ], ) @@ -299,11 +275,21 @@ def generate_launch_description(): output='screen', arguments=[ '0', '0', '0', '0', '0', '0', - ['/', namespace, '/oakd_lite_rgb_camera_optical_frame'], + [namespace, '/oakd_lite_rgb_camera_optical_frame'], [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] ] ) + # Force robot state publisher + # robot_state_publisher = Node( + # package='robot_state_publisher', + # executable='robot_state_publisher', + # namespace=namespace, + # output='screen', + # parameters=[{'use_sim_time': 'True'}], + # remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + # arguments=[urdf]) + # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) ld.add_action(param_file_cmd) @@ -317,12 +303,11 @@ def generate_launch_description(): ld.add_action(create3_nodes) ld.add_action(create3_ignition_nodes) ld.add_action(turtlebot4_node) - ld.add_action(navigation) - ld.add_action(navigation_namespaced) ld.add_action(rplidar_stf) ld.add_action(oakd_pro_stf) ld.add_action(oakd_lite_stf) ld.add_action(rplidar_stf_namespaced) ld.add_action(oakd_pro_stf_namespaced) ld.add_action(oakd_lite_stf_namespaced) + #ld.add_action(robot_state_publisher) return ld From cb7d6aa01437bcc25e32b6baea5f6a9832cc1662 Mon Sep 17 00:00:00 2001 From: "badr.moutalib" Date: Thu, 1 Dec 2022 14:32:41 +0100 Subject: [PATCH 15/16] remove starting slash for robot description nodes --- .../launch/ignition.launch.py | 16 +------- .../launch/turtlebot4_spawn.launch.py | 39 +++++++++++-------- 2 files changed, 25 insertions(+), 30 deletions(-) diff --git a/turtlebot4_ignition_bringup/launch/ignition.launch.py b/turtlebot4_ignition_bringup/launch/ignition.launch.py index c3d6d9e..a933b1c 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -45,15 +45,6 @@ def perform( DeclareLaunchArgument('rviz', default_value='false', choices=['true', 'false'], description='Start rviz.'), - DeclareLaunchArgument('slam', default_value='off', - choices=['off', 'sync', 'async'], - description='Whether to run a SLAM'), - DeclareLaunchArgument('localization', default_value='false', - choices=['true', 'false'], - description='Whether to run localization'), - DeclareLaunchArgument('nav2', default_value='false', - choices=['true', 'false'], - description='Run nav2'), DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], description='use_sim_time'), @@ -143,15 +134,12 @@ def generate_launch_description(): 'y': LaunchConfiguration('y'), 'z': LaunchConfiguration('z'), 'yaw': LaunchConfiguration('yaw'), - 'robot_name': robot_name, 'world' : LaunchConfiguration('world'), 'model' : LaunchConfiguration('model'), - 'slam' : LaunchConfiguration('slam'), - 'localization' : LaunchConfiguration('localization'), - 'nav2' : LaunchConfiguration('nav2'), 'rviz' : LaunchConfiguration('rviz'), - 'namespace': namespace, 'spawn_dock': LaunchConfiguration('spawn_dock'), + 'namespace': namespace, + 'robot_name': robot_name, }.items() ) diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py index dbcb5e0..751303c 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py @@ -106,8 +106,8 @@ def generate_launch_description(): robot_name = LaunchConfiguration('robot_name') model = LaunchConfiguration('model') namespace = LaunchConfiguration('namespace') - namespaced_robot_description = [namespace, '/robot_description'] - namespaced_dock_description = [namespace, '/standard_dock_description'] + prefixed_robot_description = ['/', namespace, '/robot_description'] + prefixed_dock_description = ['/', namespace, '/standard_dock_description'] # Robot description robot_description_launch = IncludeLaunchDescription( @@ -128,6 +128,7 @@ def generate_launch_description(): # Spawn Turtlebot4 spawn_robot = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='ros_ign_gazebo', executable='create', arguments=[ @@ -136,10 +137,11 @@ def generate_launch_description(): '-y', y, '-z', z, '-Y', yaw, - '-topic', namespaced_robot_description], + '-topic', '/robot_description'], output='screen') spawn_robot_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), package='ros_ign_gazebo', executable='create', arguments=[ @@ -148,11 +150,12 @@ def generate_launch_description(): '-y', y, '-z', z, '-Y', yaw, - '-topic', namespaced_robot_description], + '-topic', prefixed_robot_description], output='screen') # Spawn dock spawn_dock = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='ros_ign_gazebo', executable='create', arguments=[ @@ -161,7 +164,20 @@ def generate_launch_description(): '-y', y, '-z', z, '-Y', yaw_dock, - '-topic', namespaced_dock_description], + '-topic', '/standard_dock_description'], + output='screen') + + spawn_dock_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_gazebo', + executable='create', + arguments=[ + '-name', (robot_name, '_standard_dock'), + '-x', x_dock, + '-y', y, + '-z', z, + '-Y', yaw_dock, + '-topic', prefixed_dock_description], output='screen') # ROS Ign bridge @@ -280,16 +296,6 @@ def generate_launch_description(): ] ) - # Force robot state publisher - # robot_state_publisher = Node( - # package='robot_state_publisher', - # executable='robot_state_publisher', - # namespace=namespace, - # output='screen', - # parameters=[{'use_sim_time': 'True'}], - # remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], - # arguments=[urdf]) - # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) ld.add_action(param_file_cmd) @@ -299,7 +305,9 @@ def generate_launch_description(): ld.add_action(robot_description_launch) ld.add_action(dock_description) ld.add_action(spawn_robot) + ld.add_action(spawn_robot_namespaced) ld.add_action(spawn_dock) + ld.add_action(spawn_dock_namespaced) ld.add_action(create3_nodes) ld.add_action(create3_ignition_nodes) ld.add_action(turtlebot4_node) @@ -309,5 +317,4 @@ def generate_launch_description(): ld.add_action(rplidar_stf_namespaced) ld.add_action(oakd_pro_stf_namespaced) ld.add_action(oakd_lite_stf_namespaced) - #ld.add_action(robot_state_publisher) return ld From cfc15a0d12a331ec38182eb911d05dcca2d67cea Mon Sep 17 00:00:00 2001 From: "hugo.borne" Date: Tue, 7 Feb 2023 14:06:37 +0100 Subject: [PATCH 16/16] add remapping to node declaration --- .../launch/turtlebot4_nodes.launch.py | 8 ++++-- .../launch/turtlebot4_spawn.launch.py | 28 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py index c1351da..99ca4fd 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py @@ -29,9 +29,9 @@ description='robot namespace'), ] - def generate_launch_description(): + # Directories pkg_turtlebot4_ignition_bringup = get_package_share_directory('turtlebot4_ignition_bringup') @@ -42,19 +42,21 @@ def generate_launch_description(): [pkg_turtlebot4_ignition_bringup, 'config', 'turtlebot4_node.yaml']), description='Turtlebot4 Robot param file' ) - turtlebot4_node_yaml_file = LaunchConfiguration('param_file') namespace = LaunchConfiguration('namespace') + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] # Turtlebot4 node turtlebot4_node = Node( package='turtlebot4_node', name='turtlebot4_node', namespace=namespace, + remappings=remappings, executable='turtlebot4_node', parameters=[turtlebot4_node_yaml_file, {'model': LaunchConfiguration('model')}], - output='screen', + output='screen' ) # Turtlebot4 Ignition Hmi node diff --git a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py index 751303c..732e1da 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py @@ -106,8 +106,6 @@ def generate_launch_description(): robot_name = LaunchConfiguration('robot_name') model = LaunchConfiguration('model') namespace = LaunchConfiguration('namespace') - prefixed_robot_description = ['/', namespace, '/robot_description'] - prefixed_dock_description = ['/', namespace, '/standard_dock_description'] # Robot description robot_description_launch = IncludeLaunchDescription( @@ -150,7 +148,7 @@ def generate_launch_description(): '-y', y, '-z', z, '-Y', yaw, - '-topic', prefixed_robot_description], + '-topic', ['/', namespace, '/robot_description']], output='screen') # Spawn dock @@ -177,7 +175,7 @@ def generate_launch_description(): '-y', y, '-z', z, '-Y', yaw_dock, - '-topic', prefixed_dock_description], + '-topic', ['/', namespace, '/standard_dock_description']], output='screen') # ROS Ign bridge @@ -283,17 +281,17 @@ def generate_launch_description(): ) oakd_lite_stf_namespaced = Node( - condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'lite'"])), - name='camera_stf', - namespace=namespace, - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=[ - '0', '0', '0', '0', '0', '0', - [namespace, '/oakd_lite_rgb_camera_optical_frame'], - [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] - ] + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", model, "' == 'lite'"])), + name='camera_stf', + namespace=namespace, + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=[ + '0', '0', '0', '0', '0', '0', + [namespace, '/oakd_lite_rgb_camera_optical_frame'], + [LaunchConfiguration('robot_name'), '/oakd_lite_rgb_camera_frame/rgbd_camera'] + ] ) # Define LaunchDescription variable