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/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 f8b12e5..a933b1c 100644 --- a/turtlebot4_ignition_bringup/launch/ignition.launch.py +++ b/turtlebot4_ignition_bringup/launch/ignition.launch.py @@ -22,13 +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.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node - - class OffsetParser(Substitution): def __init__( self, @@ -45,20 +41,10 @@ def perform( 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'), @@ -67,10 +53,20 @@ 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') + 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']: + ARGUMENTS.append(DeclareLaunchArgument(pose_element, default_value='0.0', + description=f'{pose_element} component of the robot pose.')) def generate_launch_description(): @@ -81,13 +77,7 @@ 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_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( @@ -116,40 +106,10 @@ 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( - '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') + namespace = LaunchConfiguration('namespace') # Ignition gazebo ignition_gazebo = IncludeLaunchDescription( @@ -166,141 +126,27 @@ 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'))] - ) - - # 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() - ) - - # Spawn Turtlebot4 - spawn_robot = Node( - package='ros_ign_gazebo', - executable='create', - arguments=[ - '-name', LaunchConfiguration('robot_name'), - '-x', x, - '-y', y, - '-z', z, - '-Y', yaw, - '-topic', 'robot_description'], - output='screen') - - # Spawn dock - spawn_dock = Node( - package='ros_ign_gazebo', - executable='create', - arguments=[ - '-name', 'standard_dock', - '-x', x_dock, - '-y', y, - '-z', z, - '-Y', yaw_dock, - '-topic', 'standard_dock_description'], - output='screen') - - # ROS Ign bridge - turtlebot4_ros_ign_bridge = IncludeLaunchDescription( - PythonLaunchDescriptionSource([turtlebot4_ros_ign_bridge_launch]), - launch_arguments=[('model', LaunchConfiguration('model'))] - ) - - # 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'))] - ) - - turtlebot4_node = IncludeLaunchDescription( - PythonLaunchDescriptionSource([node_launch]), - launch_arguments=[('model', LaunchConfiguration('model')), - ('param_file', turtlebot4_node_yaml_file)] - ) - - # Create3 nodes - create3_nodes = IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_nodes_launch]) - ) - - create3_ignition_nodes = IncludeLaunchDescription( - PythonLaunchDescriptionSource([create3_ignition_nodes_launch]), - launch_arguments=[('robot_name', LaunchConfiguration('robot_name'))] + # 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'), + 'world' : LaunchConfiguration('world'), + 'model' : LaunchConfiguration('model'), + 'rviz' : LaunchConfiguration('rviz'), + 'spawn_dock': LaunchConfiguration('spawn_dock'), + 'namespace': namespace, + 'robot_name': robot_name, + }.items() ) - - # RPLIDAR static transforms - rplidar_stf = Node( - name='rplidar_stf', - 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']] - ) - - # OAKD static transforms - oakd_pro_stf = Node( - name='camera_stf', - 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'] - ], - condition=LaunchConfigurationEquals('model', 'standard') - ) - - oakd_lite_stf = Node( - name='camera_stf', - 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'] - ], - condition=LaunchConfigurationEquals('model', 'lite') - ) - + # 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(turtlebot4_ros_ign_bridge) - ld.add_action(rviz2) - ld.add_action(robot_description) - 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) 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 c988e41..5456f31 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 +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 @@ -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,10 @@ def generate_launch_description(): 'user2' ] - namespace = LaunchConfiguration('robot_name') + namespace = LaunchConfiguration('namespace') + robot_name = LaunchConfiguration('robot_name') + world = LaunchConfiguration('world') + model = LaunchConfiguration('model') use_sim_time = LaunchConfiguration('use_sim_time') pkg_irobot_create_ignition_bringup = get_package_share_directory( @@ -62,13 +67,15 @@ def generate_launch_description(): create3_bridge = IncludeLaunchDescription( PythonLaunchDescriptionSource([create3_ros_ign_bridge_launch]), launch_arguments=[ - ('robot_name', LaunchConfiguration('robot_name')), - ('world', LaunchConfiguration('world')) + ('robot_name', robot_name), + ('world', world), + ('namespace', namespace) ] ) # lidar bridge lidar_bridge = Node( + condition=LaunchConfigurationEquals('namespace', ''), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -78,20 +85,44 @@ 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') ]) + 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/', world, + '/model/', robot_name, + '/link/rplidar_link/sensor/rplidar/scan' + + '@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan'] + ], + remappings=[ + (['/world/', world, + '/model/', robot_name, + '/link/rplidar_link/sensor/rplidar/scan'], + ['/', namespace, '/scan']) + ]) + # Display message bridge hmi_display_msg_bridge = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -99,23 +130,46 @@ 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') + ]) + + hmi_display_msg_bridge_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", 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/', robot_name, '/hmi/display/raw' + + '@std_msgs/msg/String' + + ']ignition.msgs.StringMsg'], + ['/model/', robot_name, '/hmi/display/selected' + + '@std_msgs/msg/Int32' + + ']ignition.msgs.Int32'] ], - condition=LaunchConfigurationEquals('model', 'standard')) + remappings=[ + (['/model/', robot_name, '/hmi/display/raw'], + ['/', namespace, '/hmi/display/_raw']), + (['/model/', robot_name, '/hmi/display/selected'], + ['/', namespace, '/hmi/display/_selected']) + ]) # Buttons message bridge hmi_buttons_msg_bridge = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -123,18 +177,36 @@ 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') + ]) + + hmi_buttons_msg_bridge_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", 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/', robot_name, '/hmi/buttons' + + '@std_msgs/msg/Int32' + + '[ignition.msgs.Int32'] ], - condition=LaunchConfigurationEquals('model', 'standard')) + remappings=[ + (['/model/', robot_name, '/hmi/buttons'], + ['/', 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', executable='parameter_bridge', namespace=namespace, @@ -142,18 +214,36 @@ 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 + ]) + + hmi_led_msg_bridge_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", 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/', robot_name, '/hmi/led/' + led + + '@std_msgs/msg/Int32' + + ']ignition.msgs.Int32'] for led in leds ], - condition=LaunchConfigurationEquals('model', 'standard')) + remappings=[ + (['/model/', robot_name, '/hmi/led/' + led], + ['/', namespace, '/hmi/led/_' + led]) for led in leds + ]) # Camera sensor bridge oakd_pro_camera_bridge = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'standard'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -161,52 +251,97 @@ 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'), - '/model/', - LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image'], '/color/image'), - (['/world/', LaunchConfiguration('world'), - '/model/', - LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image'], '/stereo/depth'), - (['/world/', LaunchConfiguration('world'), - '/model/', - LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points'], '/stereo/depth/points'), - (['/world/', LaunchConfiguration('world'), - '/model/', - LaunchConfiguration('robot_name'), + (['/world/', world, + '/model/', robot_name, '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info'], '/color/camera_info') + ]) + + oakd_pro_camera_bridge_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", 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/', world, + '/model/', robot_name, + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image' + + '@sensor_msgs/msg/Image' + + '[ignition.msgs.Image'], + ['/world/', world, + '/model/', robot_name, + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image' + + '@sensor_msgs/msg/Image' + + '[ignition.msgs.Image'], + ['/world/', world, + '/model/', robot_name, + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points' + + '@sensor_msgs/msg/PointCloud2' + + '[ignition.msgs.PointCloudPacked'], + ['/world/', world, + '/model/', 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/', world, + '/model/', robot_name, + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/image'], + ['/', namespace, '/color/image']), + (['/world/', world, + '/model/', robot_name, + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/depth_image'], + ['/', namespace, '/stereo/depth']), + (['/world/', world, + '/model/', robot_name, + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/points'], + ['/', namespace, '/stereo/depth/points']), + (['/world/', world, + '/model/', robot_name, + '/link/oakd_pro_rgb_camera_frame/sensor/rgbd_camera/camera_info'], + ['/', namespace, '/color/camera_info']) + ]) oakd_lite_camera_bridge = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' == '' and '", model, "' == 'lite'"])), package='ros_ign_bridge', executable='parameter_bridge', namespace=namespace, @@ -214,46 +349,94 @@ 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') + ]) + + oakd_lite_camera_bridge_namespaced = Node( + condition=IfCondition(PythonExpression(["'", namespace, "' != '' and '", 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/', world, + '/model/', robot_name, + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/image' + + '@sensor_msgs/msg/Image' + + '[ignition.msgs.Image'], + ['/world/', world, + '/model/', robot_name, + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/depth_image' + + '@sensor_msgs/msg/Image' + + '[ignition.msgs.Image'], + ['/world/', world, + '/model/', robot_name, + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/points' + + '@sensor_msgs/msg/PointCloud2' + + '[ignition.msgs.PointCloudPacked'], + ['/world/', world, + '/model/', 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/', world, + '/model/', robot_name, + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/image'], + ['/', namespace, '/color/image']), + (['/world/', world, + '/model/', robot_name, + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/depth_image'], + ['/', namespace, '/stereo/depth']), + (['/world/', world, + '/model/', robot_name, + '/link/oakd_lite_rgb_camera_frame/sensor/rgbd_camera/points'], + ['/', namespace, '/stereo/depth/points']), + (['/world/', world, + '/model/', 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 +447,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_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_nodes.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py index a3757c7..99ca4fd 100644 --- a/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_nodes.launch.py @@ -24,12 +24,14 @@ ARGUMENTS = [ DeclareLaunchArgument('model', default_value='standard', choices=['standard', 'lite'], - description='Turtlebot4 Model') + description='Turtlebot4 Model'), + DeclareLaunchArgument('namespace', default_value='', + description='robot namespace'), ] - def generate_launch_description(): + # Directories pkg_turtlebot4_ignition_bringup = get_package_share_directory('turtlebot4_ignition_bringup') @@ -40,23 +42,28 @@ 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 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_bringup/launch/turtlebot4_spawn.launch.py b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py new file mode 100644 index 0000000..732e1da --- /dev/null +++ b/turtlebot4_ignition_bringup/launch/turtlebot4_spawn.launch.py @@ -0,0 +1,318 @@ +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 +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('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + 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'), + DeclareLaunchArgument('yaw', default_value='3.145', + description='robot yaw rotation at spawn'), + +] + +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.')) + + +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']) + 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') + + # 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( + condition=LaunchConfigurationEquals('namespace', ''), + package='ros_ign_gazebo', + executable='create', + arguments=[ + '-name', robot_name, + '-x', x, + '-y', y, + '-z', z, + '-Y', yaw, + '-topic', '/robot_description'], + output='screen') + + spawn_robot_namespaced = Node( + condition=LaunchConfigurationNotEquals('namespace', ''), + package='ros_ign_gazebo', + executable='create', + arguments=[ + '-name', robot_name, + '-x', x, + '-y', y, + '-z', z, + '-Y', yaw, + '-topic', ['/', namespace, '/robot_description']], + output='screen') + + # Spawn dock + spawn_dock = Node( + condition=LaunchConfigurationEquals('namespace', ''), + package='ros_ign_gazebo', + executable='create', + arguments=[ + '-name', (robot_name, '_standard_dock'), + '-x', x_dock, + '-y', y, + '-z', z, + '-Y', yaw_dock, + '-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', ['/', namespace, '/standard_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')), + launch_arguments=[('namespace', namespace)] + ) + + 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_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) + 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/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 diff --git a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.cc index d10532f..03daf73 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,89 @@ 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 based on robot name: " + this->namespace_), 4000); + } } -Turtlebot4Hmi::~Turtlebot4Hmi() +void Turtlebot4Hmi::SetNamespace(const QString &_namespace) { + this->namespace_ = _namespace.toStdString(); + ignmsg << "A new robot name 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_ = "/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_ = "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"; + 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..b1e87c1 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); @@ -80,11 +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_ = "/create3/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 220d19f..39a450b 100644 --- a/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml +++ b/turtlebot4_ignition_gui_plugins/Turtlebot4Hmi/Turtlebot4Hmi.qml @@ -31,7 +31,7 @@ Rectangle anchors.fill: parent focus: true Layout.minimumWidth: 400 - Layout.minimumHeight: 525 + Layout.minimumHeight: 600 Connections { target: Turtlebot4Hmi @@ -75,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: "Robot Name:" + 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 robot name") + 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 @@ -159,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 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))); }