diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/config/nav2_params.yaml b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/config/nav2_params.yaml index 5fb762c..1c57066 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/config/nav2_params.yaml +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/config/nav2_params.yaml @@ -16,6 +16,9 @@ bt_navigator: # for details on this file. New to Galactic after NavigateThroughPoses was added. # default_nav_through_poses_bt_xml: CONFIGURED IN LAUNCH FILE + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Reference frame. (default: map) global_frame: map @@ -52,53 +55,85 @@ bt_navigator: groot_zmq_server_port: 1667 # List of behavior tree node shared libraries. (default: - # nav2_compute_path_to_pose_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_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_compute_path_to_pose_action_bt_node + # - nav2_compute_path_through_poses_action_bt_node + # - nav2_smooth_path_action_bt_node + # - nav2_follow_path_action_bt_node + # - nav2_spin_action_bt_node + # - nav2_wait_action_bt_node + # - nav2_assisted_teleop_action_bt_node + # - nav2_back_up_action_bt_node + # - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node + # - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition + # - 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 + # - nav2_controller_cancel_bt_node + # - nav2_path_longer_on_approach_bt_node + # - nav2_wait_cancel_bt_node + # - nav2_spin_cancel_bt_node + # - nav2_back_up_cancel_bt_node + # - nav2_assisted_teleop_cancel_bt_node + # - nav2_drive_on_heading_cancel_bt_node plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_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_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + # - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node - # - nav2_goal_updated_controller_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node # TODO(lsinterbotix): configure to use mobile_base battery - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node @@ -106,12 +141,32 @@ bt_navigator: - nav2_planner_selector_bt_node - nav2_controller_selector_bt_node - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + # - nav2_assisted_teleop_cancel_bt_node + # - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False # The Controller Server implements the server for handling the controller requests for the stack # and host a map of plugin implementations. It will take in path and plugin names for controller, # progress checker and goal checker to use and call the appropriate plugins. controller_server: ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Frequency to run controller (Hz). (default 20.0) controller_frequency: 20.0 @@ -434,6 +489,9 @@ controller_server: local_costmap: local_costmap: ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Whether to send full costmap every update, rather than updates. (default: false) always_send_full_costmap: True @@ -510,7 +568,7 @@ local_costmap: # How long to store messages in a buffer to add to costmap before removing them (s). # (default: 0.0) - observation_persistence: 0.0 + observation_persistence: 0.2 # Expected rate to get new data from sensor. (default: 0.0) expected_update_rate: 0.0 @@ -546,19 +604,15 @@ local_costmap: # Minimum range to raytrace clear obstacles from costmap. (default: 0.0) raytrace_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - # https://navigation.ros.org/configuration/packages/costmap-plugins/static.html - - # QoS settings for map topic. (default: True) - map_subscribe_transient_local: True - # The Costmap 2D package implements a 2D grid-based costmap for environmental representations and a # number of sensor processing plugins. It is used in the planner and controller servers for # creating the space to check for collisions or higher cost areas to negotiate around. global_costmap: global_costmap: ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Whether to send full costmap every update, rather than updates. (default: false) always_send_full_costmap: True @@ -631,13 +685,25 @@ global_costmap: # loaded in the namespace. plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - # List of mapped costmap filter names for parameter namespaces and names. Note: Costmap - # filters are also loadable plugins just as ordinary costmap layers. This separation is made - # to avoid plugin and filter interference and places these filters on top of the combined - # layered costmap. As with plugins, each costmap filter namespace defined in this list needs - # to have a plugin parameter defining the type of filter plugin to be loaded in the - # namespace. - # filters: + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + # https://navigation.ros.org/configuration/packages/costmap-plugins/static.html + + # Whether the static_layer plugin is enabled. (default: True) + enabled: True + + # Subscribe to static map updates after receiving first. (default: False) + subscribe_to_updates: False + + # QoS settings for map topic. (default: True) + map_subscribe_transient_local: True + + # TF tolerance. (default: 0.0) + transform_tolerance: 1.0 + + # Map topic to subscribe to. If left empty the map topic will default to the global + # map_topic parameter in costmap_2d_ros. (default: "") + map_topic: "/map" obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -667,7 +733,7 @@ global_costmap: # How long to store messages in a buffer to add to costmap before removing them (s). # (default: 0.0) - observation_persistence: 0.0 + observation_persistence: 0.2 # Expected rate to get new data from sensor. (default: 0.0) expected_update_rate: 0.0 @@ -702,27 +768,7 @@ global_costmap: # Minimum range to raytrace clear obstacles from costmap. (default: 0.0) raytrace_min_range: 0.0 - - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - # https://navigation.ros.org/configuration/packages/costmap-plugins/static.html - - # Whether the static_layer plugin is enabled. (default: True) - enabled: True - - # Subscribe to static map updates after receiving first. (default: False) - subscribe_to_updates: False - - # QoS settings for map topic. (default: True) - map_subscribe_transient_local: True - - # TF tolerance. (default: 0.0) - transform_tolerance: 0.0 - - # Map topic to subscribe to. If left empty the map topic will default to the global - # map_topic parameter in costmap_2d_ros. (default: "") - map_topic: "" - + inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" # https://navigation.ros.org/configuration/packages/costmap-plugins/inflation.html @@ -749,6 +795,9 @@ global_costmap: map_saver: ros__parameters: # https://navigation.ros.org/configuration/packages/configuring-map-server.html + + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False # Timeout to attempt saving the map (seconds). (default: 2) save_map_timeout: 5.0 @@ -769,7 +818,11 @@ planner_server: ros__parameters: # Expected planner frequency. If the current frequency is less than the expected frequency, # display the warning message. - expected_planner_frequency: 20.0 + expected_planner_frequency: 5.0 + + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -784,7 +837,6 @@ planner_server: # points of the path. (default: false) use_final_approach_orientation: false -# TODO (lsinterbotix): Update to behavior_server on Humble # The Behavior Server implements the server for handling recovery behavior requests and hosting a # vector of plugins implementing various C++ behaviors. It is also possible to implement # independent behavior servers for each custom behavior, but this server will allow multiple @@ -797,20 +849,32 @@ behavior_server: ros__parameters: # https://navigation.ros.org/configuration/packages/configuring-behavior-server.html?highlight=recoveries_server - # Raw costmap topic for collision checking. (default: local_costmap/costmap_raw) - costmap_topic: local_costmap/costmap_raw + # Raw local costmap topic for collision checking. (default: local_costmap/costmap_raw) + local_costmap_topic: local_costmap/costmap_raw + + # Topic for footprint in the local costmap frame. (default: local_costmap/published_footprint) + local_footprint_topic: local_costmap/published_footprint - # Topic for footprint in the costmap frame. (default: local_costmap/published_footprint) - footprint_topic: local_costmap/published_footprint + # Raw global costmap topic for collision checking. (default: global_costmap/costmap_raw) + global_costmap_topic: global_costmap/costmap_raw + + # Topic for footprint in the global costmap frame. (default: global_costmap/published_footprint) + global_footprint_topic: global_costmap/published_footprint # Frequency to run behavior plugins. (default: 10.0) cycle_frequency: 10.0 # TF transform tolerance. (default: 0.1) - transform_tolerance: 0.1 + transform_tolerance: 0.3 + + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False - # Reference frame. (default: odom) - global_frame: locobot/odom + # Locl reference frame. (default: odom) + local_frame: locobot/odom + + # Global reference frame. (default: map) + global_frame: map # Robot base frame. (default: base_link) robot_base_frame: locobot/base_link @@ -818,7 +882,7 @@ behavior_server: # List of plugin names to use, also matches action server names. Note: Each plugin namespace # defined in this list needs to have a plugin parameter defining the type of plugin to be # loaded in the namespace. (default: {“spin”, “back_up”, “drive_on_heading”, “wait”}) - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_behaviors/Spin" @@ -851,6 +915,9 @@ waypoint_follower: ros__parameters: # https://navigation.ros.org/configuration/packages/configuring-waypoint-follower.html + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Whether to fail action task if a single waypoint fails. If false, will continue to next # waypoint. (default: True) stop_on_failure: True @@ -873,3 +940,88 @@ waypoint_follower: # Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If # zero, robot will directly continue to next waypoint. (default: 0) waypoint_pause_duration: 200 + +# The Map Server implements the server for handling the map load requests for the stack and host a map topic. +# It also implements a map saver server which will run in the background and save maps based on service requests. +# There exists a map saver CLI similar to ROS 1 as well for a single map save. +map_server: + ros__parameters: + # https://navigation.ros.org/configuration/packages/configuring-map-server.html + + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + + # Path to map yaml file. + yaml_filename: "" + + # Topic to publish loaded map to. (default: "map") + topic_name: "map" + + # Frame to publish loaded map in. (default: "map") + frame_id: "map" + +map_saver: + ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + + # Timeout to attempt saving the map (seconds). (default: 2.0) + save_map_timeout: 5.0 + + # Free space maximum probability threshold value for occupancy grid. (default: 0.25) + free_thresh_default: 0.25 + + # Occupied space minimum probability threshhold value for occupancy grid. (default: 0.65) + occupied_thresh_default: 0.65 + +# For localization mode with AMCL (without using slam_toolbox) +amcl: + ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "locobot/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: 16.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "locobot/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: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 0.3 + 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: locobot/scan + map_topic: map + set_initial_pose: true + always_reset_initial_pose: false + first_map_only: false + initial_pose: + x: 0.0 + y: 0.0 + z: 0.0 + yaw: 0.0 \ No newline at end of file diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py index 1b44108..215fdc7 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py @@ -41,12 +41,10 @@ from launch.conditions import ( IfCondition, LaunchConfigurationEquals, - LaunchConfigurationNotEquals, ) from launch.substitutions import ( PathJoinSubstitution, LaunchConfiguration, - TextSubstitution, ) from launch_ros.actions import ( Node, @@ -62,10 +60,10 @@ def launch_setup(context, *args, **kwargs): use_sim_time_launch_arg = LaunchConfiguration('use_sim_time') log_level_launch_arg = LaunchConfiguration('log_level') autostart_launch_arg = LaunchConfiguration('autostart') + use_composition_launch_arg = LaunchConfiguration('use_composition') use_respawn_launch_arg = LaunchConfiguration('use_respawn') nav2_params_file_launch_arg = LaunchConfiguration('nav2_params_file') cmd_vel_topic_launch_arg = LaunchConfiguration('cmd_vel_topic') - use_slam_toolbox_launch_arg = LaunchConfiguration('use_slam_toolbox') map_yaml_file_launch_arg = LaunchConfiguration('map') # Set env var to print messages to stdout immediately set_logging_env_var = SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -83,7 +81,7 @@ def launch_setup(context, *args, **kwargs): ] lifecycle_nodes_localization = [ - 'map_server' + 'map_server', 'amcl' ] @@ -97,14 +95,14 @@ def launch_setup(context, *args, **kwargs): # https://github.com/ros/robot_state_publisher/pull/30 tf_remappings = [ ('/tf', 'tf'), - ('/tf_static', 'tf_static' - ) + ('/tf_static', 'tf_static') ] # Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time_launch_arg, 'autostart': autostart_launch_arg, + 'yaml_filename': map_yaml_file_launch_arg, } configured_params = RewrittenYaml( @@ -181,11 +179,20 @@ def launch_setup(context, *args, **kwargs): ) slam_toolbox_nav2_nodes = GroupAction( - condition=IfCondition(use_slam_toolbox_launch_arg), actions=[ SetParameter('use_sim_time', use_sim_time_launch_arg), Node( - condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization'), + condition=IfCondition(use_composition_launch_arg), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart_launch_arg}], + arguments=['--ros-args', '--log-level', log_level_launch_arg], + remappings=tf_remappings, + output='screen' + ), + Node( + condition=LaunchConfigurationEquals('slam_mode', 'mapping'), package='nav2_map_server', executable='map_saver_server', output='screen', @@ -194,12 +201,11 @@ def launch_setup(context, *args, **kwargs): arguments=[ '--ros-args', '--log-level', log_level_launch_arg ], - parameters=[ - configured_params - ], + parameters=[configured_params], + remappings=tf_remappings ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization'), package='nav2_map_server', executable='map_server', name='map_server', @@ -209,14 +215,11 @@ def launch_setup(context, *args, **kwargs): arguments=[ '--ros-args', '--log-level', log_level_launch_arg ], - parameters=[ - configured_params, - {'yaml_filename': map_yaml_file_launch_arg}, - ], + parameters=[configured_params], remappings=tf_remappings ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization'), package='nav2_amcl', executable='amcl', name='amcl', @@ -228,7 +231,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings ), Node( - condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'mapping'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_slam', @@ -237,12 +240,13 @@ def launch_setup(context, *args, **kwargs): '--ros-args', '--log-level', log_level_launch_arg ], parameters=[ + {'use_sim_time': use_sim_time_launch_arg}, {'autostart': autostart_launch_arg}, {'node_names': lifecycle_nodes_slam}, ] ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', @@ -251,6 +255,7 @@ def launch_setup(context, *args, **kwargs): '--ros-args', '--log-level', log_level_launch_arg ], parameters=[ + {'use_sim_time': use_sim_time_launch_arg}, {'autostart': autostart_launch_arg}, {'node_names': lifecycle_nodes_localization}, ] @@ -286,10 +291,18 @@ def generate_launch_description(): description='automatically startup the Nav2 stack.', ) ) + declared_arguments.append( + DeclareLaunchArgument( + 'use_composition', + default_value='true', + choices=('true', 'false'), + description='Whether to use composed bringup', + ) + ) declared_arguments.append( DeclareLaunchArgument( 'cmd_vel_topic', - default_value=(LaunchConfiguration('robot_name'), '/mobile_base/cmd_vel'), + default_value=(LaunchConfiguration('robot_name'), '/diffdrive_controller/cmd_vel_unstamped'), description="topic to remap /cmd_vel to." ) ) @@ -320,29 +333,10 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - 'use_slam_toolbox', - default_value='false', - choices=('true', 'false'), - description=( - 'whether to use slam_toolbox over rtabmap.' - ) - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - 'slam_toolbox_mode', - default_value='online_async', - choices=( - # 'lifelong', - 'localization', - # 'offline', - 'online_async', - 'online_sync' - ), - description=( - "the node to launch the SLAM in using the slam_toolbox. Currently only " - "'localization', 'online_sync', and 'online_async' modes are supported." - ), + 'slam_mode', + default_value='mapping', + choices=('mapping', 'localization'), + description='the mode to launch the SLAM in using RTAB-MAP or SLAM Toolbox.', ) ) declared_arguments.append( @@ -368,4 +362,4 @@ def generate_launch_description(): ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py index 5ba6ab9..caf4420 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py @@ -62,7 +62,8 @@ def launch_setup(context, *args, **kwargs): xs_driver_logging_level_launch_arg = LaunchConfiguration('xs_driver_logging_level') use_rviz_launch_arg = LaunchConfiguration('use_rviz') slam_toolbox_params_file_launch_arg = LaunchConfiguration('slam_toolbox_params_file') - slam_mode_launch_arg = LaunchConfiguration('slam_mode') + slam_toolbox_mode_launch_arg = LaunchConfiguration('slam_toolbox_mode') + use_camera_launch_arg = LaunchConfiguration('use_camera') camera_tilt_angle_launch_arg = LaunchConfiguration('camera_tilt_angle') launch_driver_launch_arg = LaunchConfiguration('launch_driver') hardware_type_launch_arg = LaunchConfiguration('hardware_type') @@ -93,7 +94,7 @@ def launch_setup(context, *args, **kwargs): 'use_rviz': use_rviz_launch_arg, 'use_base_odom_tf': use_base_odom_tf_launch_arg, 'rviz_frame': 'map', - 'use_camera': 'true', + 'use_camera': use_camera_launch_arg, 'rs_camera_align_depth': 'true', 'use_base': 'true', # 'use_dock': 'true', @@ -114,7 +115,7 @@ def launch_setup(context, *args, **kwargs): shell=True, ) slam_toolbox_online_sync_slam_node = Node( - condition=LaunchConfigurationEquals('slam_mode', 'online_sync'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'online_sync'), package='slam_toolbox', executable='sync_slam_toolbox_node', name='slam_toolbox', @@ -128,7 +129,7 @@ def launch_setup(context, *args, **kwargs): ) slam_toolbox_online_async_slam_node = Node( - condition=LaunchConfigurationEquals('slam_mode', 'online_async'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'online_async'), package='slam_toolbox', executable='async_slam_toolbox_node', name='slam_toolbox', @@ -142,7 +143,7 @@ def launch_setup(context, *args, **kwargs): ) slam_toolbox_localization_slam_node = Node( - condition=LaunchConfigurationEquals('slam_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), package='slam_toolbox', executable='localization_slam_toolbox_node', name='slam_toolbox', @@ -170,7 +171,7 @@ def launch_setup(context, *args, **kwargs): 'autostart': 'true', 'params_file': LaunchConfiguration('nav2_params_file'), 'use_slam_toolbox': 'true', - 'slam_toolbox_mode': slam_mode_launch_arg, + 'slam_mode': 'mapping', 'map': map_yaml_file_launch_arg, }.items(), ) @@ -224,6 +225,14 @@ def generate_launch_description(): description='set the logging level of the X-Series Driver.' ) ) + declared_arguments.append( + DeclareLaunchArgument( + 'use_camera', + default_value='false', + choices=('true', 'false'), + description='if `true`, the RealSense camera nodes are launched.', + ) + ) declared_arguments.append( DeclareLaunchArgument( 'camera_tilt_angle', @@ -274,25 +283,26 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - 'slam_mode', + 'slam_toolbox_mode', default_value='online_async', choices=( # 'lifelong', - 'localization', + # 'localization', # 'offline', 'online_async', 'online_sync' ), description=( "the mode to launch the SLAM in using the slam_toolbox. Currently only " - "'localization', 'online_sync', and 'online_async' modes are supported." + "'online_sync', and 'online_async' modes are supported." + "'localization' mode takes a pose graph map defined in the slam_toolbox_localization.yaml" ), ) ) declared_arguments.append( DeclareLaunchArgument( 'slam_toolbox_params_filename', - default_value=('slam_toolbox_', LaunchConfiguration('slam_mode'), '.yaml') + default_value=('slam_toolbox_', LaunchConfiguration('slam_toolbox_mode'), '.yaml') ) ) declared_arguments.append( diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.pgm b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.pgm new file mode 100644 index 0000000..238a7f7 Binary files /dev/null and b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.pgm differ diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml new file mode 100644 index 0000000..899938d --- /dev/null +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml @@ -0,0 +1,7 @@ +image: interbotix_map.pgm +mode: trinary +resolution: 0.05 +origin: [-9.38, -5.11, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz b/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz index b866b80..01d1c44 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz @@ -9,8 +9,8 @@ Panels: Visualization Manager: Class: "" Displays: - - Alpha: 0.699999988079071 - Cell Size: 0.25 + - Alpha: 0.3 + Cell Size: 0.1 Class: rviz_default_plugins/Grid Color: 255; 255; 255 Enabled: true @@ -24,7 +24,7 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 10 + Plane Cell Count: 300 Reference Frame: Value: true - Alpha: 1 @@ -472,6 +472,130 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Landmark Nav Goals + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker_viz + Value: false + - Class: rviz_common/Group + Displays: + - Class: rtabmap_rviz_plugins/Info + Enabled: true + Name: Info + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locobot/rtabmap/info + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_rviz_plugins/MapCloud + Cloud decimation: 4 + Cloud from scan: false + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.009999999776482582 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Download namespace: rtabmap + Enabled: false + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locobot/rtabmap/mapData + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Class: rtabmap_rviz_plugins/MapGraph + Enabled: true + Global loop closure: 255; 0; 0 + Landmark: 0; 128; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 + Name: MapGraph + Neighbor: 0; 0; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locobot/rtabmap/mapGraph + User: 255; 0; 0 + Value: true + Virtual: 255; 0; 255 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ObstacleCloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locobot/rtabmap/cloud_obstacles + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: false + Name: RTAB-Map - Class: rviz_common/Group Displays: - Alpha: 0.699999988079071 @@ -516,6 +640,34 @@ Visualization Manager: Value: /global_costmap/costmap_updates Use Timestamp: false Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: costmap @@ -537,6 +689,34 @@ Visualization Manager: Value: /local_costmap/costmap_updates Use Timestamp: false Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: false - Alpha: 1 Class: rviz_default_plugins/Polygon Color: 25; 255; 0 @@ -550,12 +730,12 @@ Visualization Manager: Reliability Policy: Reliable Value: /local_costmap/published_footprint Value: true - Enabled: true + Enabled: false Name: Navigation Enabled: true Global Options: - Background Color: 200; 200; 200 - Fixed Frame: map + Background Color: 0; 176; 240 + Fixed Frame: locobot/odom Frame Rate: 30 Name: root Tools: @@ -598,7 +778,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.381117105484009 + Distance: 2.5063586235046387 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -612,21 +792,27 @@ Visualization Manager: Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5553984045982361 + Near Clip Distance: 0.01 + Pitch: 0.7853981852531433 Target Frame: - Value: Orbit (rviz) - Yaw: 0.8953980803489685 + Value: Orbit (rviz_default_plugins) + Yaw: 0.7853981852531433 Saved: ~ Window Geometry: + Camera Image: + collapsed: false Displays: collapsed: false - Height: 923 + Height: 752 Hide Left Dock: false Hide Right Dock: true - Image: + Interbotix Control Panel: + collapsed: false + QMainWindow State: 000000ff00000000fd00000003000000000000016d00000296fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000296000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c00000001910000012e0000012e00000151fb0000000a0049006d00610067006501000002ac000000d20000000000000000fb0000000a0049006d006100670065010000025f0000011f0000000000000000fb0000001800430061006d00650072006100200049006d00610067006500000002b10000012a0000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000347000000af0000005c00ffffff00000001000001000000039efc0200000004fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000000a00560069006500770073000000003d0000039e0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003f70000029600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Tool Properties: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000018b00000368fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001600000221000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c00000002420000013c0000000000000000fb0000000a0049006d006100670065010000023d000001410000002800fffffffb0000000c00430061006d0065007200610100000289000000f50000000000000000fb0000000a0049006d00610067006501000002b6000000c800000000000000000000000100000110000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a20000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004770000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Width: 1544 + Views: + collapsed: true + Width: 1386 X: 72 Y: 27