diff --git a/src/igvc_bringup/launch/bringup.launch.py b/src/igvc_bringup/launch/bringup.launch.py index f2c046be..4ab0386b 100644 --- a/src/igvc_bringup/launch/bringup.launch.py +++ b/src/igvc_bringup/launch/bringup.launch.py @@ -9,6 +9,7 @@ def generate_launch_description(): use_sim = LaunchConfiguration('use_sim') use_mock_hardware = LaunchConfiguration('use_mock_hardware') + use_sim_time = LaunchConfiguration('use_sim_time') return LaunchDescription([ # Launch Arguments @@ -33,6 +34,12 @@ def generate_launch_description(): default_value='true', description='Launch rtabmap for SLAM' ), + + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation time' + ), # Publishers & URDF @@ -43,7 +50,8 @@ def generate_launch_description(): '/publisher.launch.py'] ), launch_arguments={ - 'use_mock_hardware': use_mock_hardware + 'use_mock_hardware': use_mock_hardware, + 'use_sim_time' : use_sim_time, }.items() ), @@ -57,6 +65,9 @@ def generate_launch_description(): ] ), condition=IfCondition(use_sim), + launch_arguments={ + 'use_sim_time' : use_sim_time, + }.items(), ), # ROS2_Control @@ -66,6 +77,9 @@ def generate_launch_description(): '/launch', '/control.launch.py'] ), + launch_arguments={ + 'use_sim_time' : use_sim_time, + }.items(), ), # Real hardware @@ -75,7 +89,10 @@ def generate_launch_description(): '/launch', '/hardware.launch.py'] ), - condition = UnlessCondition(use_mock_hardware) + condition = UnlessCondition(use_mock_hardware), + launch_arguments={ + 'use_sim_time' : use_sim_time, + }.items(), ), # SLAM @@ -85,7 +102,10 @@ def generate_launch_description(): '/launch', '/sim_rtabmap.launch.py'] ), - condition = IfCondition(LaunchConfiguration('use_slam')) + condition = IfCondition(LaunchConfiguration('use_slam')), + launch_arguments={ + 'use_sim_time' : use_sim_time, + }.items(), ), # TODO Nav diff --git a/src/igvc_description/launch/publisher.launch.py b/src/igvc_description/launch/publisher.launch.py index bee0e333..f6ba3c63 100644 --- a/src/igvc_description/launch/publisher.launch.py +++ b/src/igvc_description/launch/publisher.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): declare_use_sim_time_cmd = DeclareLaunchArgument( name='use_sim_time', - default_value='False', + default_value='True', description='Use simulation (Gazebo) clock if true') # Specify the actions @@ -48,6 +48,7 @@ def generate_launch_description(): condition=UnlessCondition(gui), package='joint_state_publisher', executable='joint_state_publisher', + parameters=[{'use_sim_time': use_sim_time}], name='joint_state_publisher') # A GUI to manipulate the joint state values diff --git a/src/igvc_description/rviz/rviz_settings.rviz b/src/igvc_description/rviz/rviz_settings.rviz index 2db5104b..56da1f1a 100644 --- a/src/igvc_description/rviz/rviz_settings.rviz +++ b/src/igvc_description/rviz/rviz_settings.rviz @@ -64,11 +64,11 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true + # base_link: + # Alpha: 1 + # Show Axes: false + # Show Trail: false + # Value: true box_rotate: Alpha: 1 Show Axes: false diff --git a/src/igvc_hardware/config/bot_controllers.yaml b/src/igvc_hardware/config/bot_controllers.yaml index cd4292a6..e09682c1 100644 --- a/src/igvc_hardware/config/bot_controllers.yaml +++ b/src/igvc_hardware/config/bot_controllers.yaml @@ -40,7 +40,7 @@ bot_drive_controller: open_loop: true # TODO eventually make false, but keep true for initial sims position_feedback: false # TODO we don't have sensors implemented atm - enable_odom_tf: true # Whether to publish the transformation between the odom & base frame IDs + enable_odom_tf: false # Whether to publish the transformation between the odom & base frame IDs cmd_vel_timeout: 0.5 # staling threshold for velocity commands publish_limited_velocity: false # TODO what does this do? diff --git a/src/igvc_hardware/launch/control.launch.py b/src/igvc_hardware/launch/control.launch.py index 83a66bed..e6ed80a1 100644 --- a/src/igvc_hardware/launch/control.launch.py +++ b/src/igvc_hardware/launch/control.launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): package="controller_manager", executable="spawner", arguments=["bot_drive_controller", - "--controller-manager", "/controller_manager"], + "--controller-manager", "/controller_manager", "--switch-timeout", "20.0"], remappings=[('~/cmd_vel','/cmd_vel')] ) @@ -56,7 +56,7 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager","--switch-timeout", "20.0"], ) diff --git a/src/igvc_nav/config/nav2_params.yaml b/src/igvc_nav/config/nav2_params.yaml index 22e5f6db..73b77f51 100644 --- a/src/igvc_nav/config/nav2_params.yaml +++ b/src/igvc_nav/config/nav2_params.yaml @@ -1,48 +1,9 @@ -amcl: - ros__parameters: - 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: "nav2_amcl::DifferentialMotionModel" - 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 - bt_navigator: ros__parameters: + enable_stamped_cmd_vel: true global_frame: map - robot_base_frame: base_link - odom_topic: /odom + robot_base_frame: base_footprint + odom_topic: odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 @@ -67,6 +28,7 @@ bt_navigator: controller_server: ros__parameters: + enable_stamped_cmd_vel: true controller_frequency: 20.0 costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.5 @@ -112,7 +74,7 @@ controller_server: wz_max: 1.9 iteration_count: 1 prune_distance: 1.7 - transform_tolerance: 0.1 + transform_tolerance: 5.0 #TODO Change, Excessive temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" @@ -189,81 +151,111 @@ controller_server: # twirling_cost_power: 1 # twirling_cost_weight: 10.0 -local_costmap: - local_costmap: +global_costmap: + global_costmap: ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - rolling_window: true - width: 3 - height: 3 + footprint_padding: 0.03 + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 10.0 #TODO excessive + global_frame: map + robot_base_frame: base_footprint + robot_radius: 0.22 # radius set and used, so no footprint points resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.70 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" + width: 10 + height: 10 + origin_x: -5.0 + origin_y: -5.0 + use_sim_time: true + plugins: ["obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 + observation_sources: scan + footprint_clearing_enabled: true max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: rtabmap - rtabmap: - topic: /rtabmap/odom_local_scan_map + combination_method: 1 + scan: + topic: /scan max_obstacle_height: 2.0 clearing: True marking: True - data_type: "PointCloud2" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True + data_type: "LaserScan" + inf_is_valid: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.55 + cost_scaling_factor: 1.0 + inflate_unknown: false + inflate_around_unknown: true always_send_full_costmap: True + introspection_mode: "disabled" -global_costmap: - global_costmap: +local_costmap: + local_costmap: ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - robot_radius: 0.22 + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint + rolling_window: true + width: 3 + height: 3 resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + footprint_padding: 0.03 + transform_tolerance: 10.0 #TODO Change, Excessive + robot_radius: 0.22 # radius set and used, so no footprint points + use_sim_time: true + plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan + footprint_clearing_enabled: true + max_obstacle_height: 2.0 + combination_method: 1 scan: topic: /scan max_obstacle_height: 2.0 clearing: True marking: True data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 + inf_is_valid: true + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + footprint_clearing_enabled: true + max_obstacle_height: 2.0 + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + unknown_threshold: 15 + mark_threshold: 0 + observation_sources: pointcloud + combination_method: 1 + pointcloud: # no frame set, uses frame from message + topic: /camera/camera/depth/points + max_obstacle_height: 2.0 + min_obstacle_height: 0.0 obstacle_max_range: 2.5 obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + clearing: True + marking: True + data_type: "PointCloud2" + transport_type: "raw" # raw or/ with compression (zlib, draco, zstd) inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.7 + enabled: true + inflation_radius: 0.55 + cost_scaling_factor: 1.0 + inflate_unknown: false + inflate_around_unknown: true always_send_full_costmap: True + introspection_mode: "disabled" # The yaml_filename does not need to be specified since it going to be set by defaults in launch. # If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py @@ -274,6 +266,7 @@ global_costmap: map_saver: ros__parameters: + enable_stamped_cmd_vel: true save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -281,17 +274,19 @@ map_saver: planner_server: ros__parameters: + enable_stamped_cmd_vel: true expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 + tolerance: 2.5 #TODO Change, Excessive use_astar: false allow_unknown: true smoother_server: ros__parameters: + enable_stamped_cmd_vel: true smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -301,6 +296,7 @@ smoother_server: behavior_server: ros__parameters: + enable_stamped_cmd_vel: true local_costmap_topic: local_costmap/costmap_raw global_costmap_topic: global_costmap/costmap_raw local_footprint_topic: local_costmap/published_footprint @@ -319,8 +315,8 @@ behavior_server: plugin: "nav2_behaviors::AssistedTeleop" local_frame: odom global_frame: map - robot_base_frame: base_link - transform_tolerance: 0.1 + robot_base_frame: base_footprint + transform_tolerance: 5.0 #TODO Change, Excessive simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 @@ -328,6 +324,7 @@ behavior_server: waypoint_follower: ros__parameters: + enable_stamped_cmd_vel: true loop_rate: 20 stop_on_failure: false action_server_result_timeout: 900.0 @@ -340,50 +337,126 @@ waypoint_follower: velocity_smoother: ros__parameters: smoothing_frequency: 20.0 - scale_velocities: False + scale_velocities: false feedback: "OPEN_LOOP" - max_velocity: [1.0, 0.0, 2.0] - min_velocity: [-1.0, 0.0, -2.0] - max_accel: [3.0, 0.0, 3.2] - max_decel: [-3.0, 0.0, -3.2] + max_velocity: [0.5, 0.0, 2.5] + min_velocity: [-0.5, 0.0, -2.5] + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 - deadband_velocity: [0.5, 0.0, 0.5] - velocity_timeout: 1.0 - + use_realtime_priority: false + enable_stamped_cmd_vel: true + stamp_smoothed_velocity_with_smoothing_time: true + collision_monitor: ros__parameters: + enabled: true base_frame_id: "base_footprint" odom_frame_id: "odom" cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" + cmd_vel_out_topic: "/bot_drive_controller/cmd_vel" state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 + transform_tolerance: 5.0 #TODO Change, Excessive + source_timeout: 5.0 base_shift_correction: True stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] + enable_stamped_cmd_vel: True + use_realtime_priority: false + robot_radius: 0.22 + polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"] + # Robot overall dimensions from CAD: L=1.013 m, W=0.646 m, max Z=0.974 m (z doesn't matter for polygons). + # Half extents are (0.5065m, 0.323m). + # I'm using PolygonStop +0.075m margin. + # and PolygonSlow goes 0.25m further than PolygonStop. + PolygonStop: + type: "polygon" + points: "[[0.58, 0.40], [0.58, -0.40], [-0.58, -0.40], [-0.58, 0.40]]" + action_type: "stop" + min_points: 4 # max_points: 3 for Humble + visualize: True + polygon_pub_topic: "polygon_stop" + enabled: True + PolygonSlow: + type: "polygon" + points: "[[0.83, 0.65], [0.83, -0.65], [-0.83, -0.65], [-0.83, 0.65]]" + action_type: "slowdown" + min_points: 4 # max_points: 3 for Humble + slowdown_ratio: 0.3 + visualize: True + polygon_pub_topic: "polygon_slowdown" + enabled: True + PolygonLimit: + type: "polygon" + points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]" + action_type: "limit" + min_points: 4 # max_points: 3 for Humble + linear_limit: 0.4 + angular_limit: 0.5 + visualize: True + polygon_pub_topic: "polygon_limit" + enabled: True FootprintApproach: type: "polygon" action_type: "approach" footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 1.2 - simulation_time_step: 0.1 - min_points: 6 + time_before_collision: 2.0 + simulation_time_step: 0.02 + min_points: 6 # max_points: 5 for Humble visualize: False enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "scan" - min_height: 0.15 - max_height: 2.0 + VelocityPolygonStop: + type: "velocity_polygon" + action_type: "stop" + min_points: 6 + visualize: True enabled: True + polygon_pub_topic: "velocity_polygon_stop" + velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"] + holonomic: false + rotation: + points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]" + linsourceear_min: 0.0 + linear_max: 0.05 + theta_min: -1.0 + theta_max: 1.0 + translation_forward: + points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]" + linear_min: 0.0 + linear_max: 1.0 + theta_min: -1.0 + theta_max: 1.0 + translation_backward: + points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]" + linear_min: -1.0 + linear_max: 0.0 + theta_min: -1.0 + theta_max: 1.0 + # This is the last polygon to be checked, it should cover the entire range of robot's velocities + # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity + # is not covered by any of the other sub-polygons + stopped: + points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]" + linear_min: -1.0 + linear_max: 1.0 + theta_min: -1.0 + theta_max: 1.0 + observation_sources: ["pointcloud"] + pointcloud: + type: pointcloud + source_timeout: 3.0 + topic: /scan/points + min_range: 1.0 + clearing: True + marking: True + data_type: "PointCloud2" + transport_type: "raw" # raw or/ with compression (zlib, draco, zstd) docking_server: ros__parameters: + enable_stamped_cmd_vel: true controller_frequency: 50.0 initial_perception_timeout: 5.0 wait_charge_timeout: 5.0 @@ -391,7 +464,7 @@ docking_server: undock_linear_tolerance: 0.05 undock_angular_tolerance: 0.1 max_retries: 3 - base_frame: "base_link" + base_frame: "base_footprint" fixed_frame: "odom" dock_backwards: false dock_prestaging_tolerance: 0.5 @@ -430,13 +503,14 @@ docking_server: use_collision_detection: true costmap_topic: "local_costmap/costmap_raw" footprint_topic: "local_costmap/published_footprint" - transform_tolerance: 0.1 + transform_tolerance: 5.0 #TODO Change, Excessive projection_time: 5.0 simulation_step: 0.1 dock_collision_threshold: 0.3 loopback_simulator: ros__parameters: + enable_stamped_cmd_vel: true base_frame_id: "base_footprint" odom_frame_id: "odom" map_frame_id: "map" diff --git a/src/igvc_nav/launch/igvc_nav.launch.py b/src/igvc_nav/launch/igvc_nav.launch.py index 221c536c..35828998 100644 --- a/src/igvc_nav/launch/igvc_nav.launch.py +++ b/src/igvc_nav/launch/igvc_nav.launch.py @@ -5,6 +5,8 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument def generate_launch_description(): nav2_bringup_package = get_package_share_directory('nav2_bringup') @@ -12,14 +14,23 @@ def generate_launch_description(): nav2_launch_path = os.path.join(nav2_bringup_package , 'launch', 'navigation_launch.py') config_path = os.path.join(igvc_nav_package, 'config', 'nav2_params.yaml') + + use_sim_time = LaunchConfiguration('use_sim_time') nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource(nav2_launch_path), launch_arguments={ - 'params_file' : config_path + 'params_file' : config_path, + 'use_sim_time' : use_sim_time, }.items() + ) return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='true' + ), + nav2 ]) \ No newline at end of file diff --git a/src/igvc_slam/launch/rtabmap.launch.py b/src/igvc_slam/launch/rtabmap.launch.py index 958ad292..083d8c87 100644 --- a/src/igvc_slam/launch/rtabmap.launch.py +++ b/src/igvc_slam/launch/rtabmap.launch.py @@ -40,7 +40,8 @@ def generate_launch_description(): 'camera_info_topic' : '/camera/camera/color/camera_info', 'frame_id' : 'base_footprint', 'publish_tf_odom' : 'true', - 'odom_topic' : '/odom', + 'publish_tf_map' : 'true', + 'odom_topic' : 'odom', 'odom_frame_id' : 'odom', 'approx_sync' : 'true', 'rgbd_sync' : 'true', diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py index 41b3991f..e1acbeb1 100644 --- a/src/igvc_slam/launch/sim_rtabmap.launch.py +++ b/src/igvc_slam/launch/sim_rtabmap.launch.py @@ -24,7 +24,8 @@ def generate_launch_description(): 'frame_id' : 'base_footprint', 'log_level' : 'debug', 'publish_tf_odom' : 'true', - 'odom_topic' : '/odom', + 'publish_tf_map' : 'true', + 'odom_topic' : 'odom', 'odom_frame_id' : 'odom', 'sync_queue_size' : '10', #TODO verify 'topic_queue_size' : '10', #TODO verify @@ -38,9 +39,9 @@ def generate_launch_description(): 'rviz' : 'true', 'use_sim_time' : 'true', 'wait_for_transform' : '0.4', - 'imu_topic' : '/rtabmap/imu', - 'wait_imu_to_init' : 'false', - 'map_topic' : '/map' + 'imu_topic' : '/imu_plugin/out', + 'wait_imu_to_init' : 'true', + 'map_topic' : 'map' }.items() )