Fix gps navigation error jazzy branch

nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml

@@ -10,10 +10,10 @@ ekf_filter_node_odom:
 
     map_frame: map
     odom_frame: odom
-    base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin
+    base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin
     world_frame: odom
 
-    odom0: odom
+    odom0: /demo/odom
     odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  true,
@@ -23,7 +23,7 @@ ekf_filter_node_odom:
     odom0_differential: false
     odom0_relative: false
 
-    imu0: imu
+    imu0: demo/imu
     imu0_config: [false, false, false,
                   false,  false,  true,
                   false, false, false,
@@ -62,10 +62,10 @@ ekf_filter_node_map:
 
     map_frame: map
     odom_frame: odom
-    base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin
+    base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin
     world_frame: map
 
-    odom0: odom
+    odom0: /demo/odom
     odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  true,
@@ -85,7 +85,7 @@ ekf_filter_node_map:
     odom1_differential: false
     odom1_relative: false
 
-    imu0: imu
+    imu0: /demo/imu
     imu0_config: [false, false, false,
                   false,  false,  true,
                   false, false, false,

nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml

@@ -10,70 +10,70 @@ bt_navigator:
   ros__parameters:
     global_frame: map
     robot_base_frame: base_link
-    odom_topic: /odom
+    odom_topic: /odometry/global
     bt_loop_duration: 10
     default_server_timeout: 20
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
-      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
+      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
     navigate_through_poses:
-      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
+      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
     # '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.
-    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_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_are_error_codes_active_condition_bt_node
-      - nav2_would_a_controller_recovery_help_condition_bt_node
-      - nav2_would_a_planner_recovery_help_condition_bt_node
-      - nav2_would_a_smoother_recovery_help_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
-      - nav2_is_battery_charging_condition_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_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_are_error_codes_active_condition_bt_node
+    #   - nav2_would_a_controller_recovery_help_condition_bt_node
+    #   - nav2_would_a_planner_recovery_help_condition_bt_node
+    #   - nav2_would_a_smoother_recovery_help_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
+    #   - nav2_is_battery_charging_condition_bt_node
     error_code_names:
       - compute_path_error_code
       - follow_path_error_code
@@ -249,7 +249,7 @@ planner_server:
     expected_planner_frequency: 20.0
     planner_plugins: ["GridBased"]
     GridBased:
-      plugin: "nav2_navfn_planner/NavfnPlanner"
+      plugin: "nav2_navfn_planner::NavfnPlanner"
       tolerance: 0.5
       use_astar: false
       allow_unknown: true
@@ -272,15 +272,15 @@ behavior_server:
     cycle_frequency: 10.0
     behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
-      plugin: "nav2_behaviors/Spin"
+      plugin: "nav2_behaviors::Spin"
     backup:
-      plugin: "nav2_behaviors/BackUp"
+      plugin: "nav2_behaviors::BackUp"
     drive_on_heading:
-      plugin: "nav2_behaviors/DriveOnHeading"
+      plugin: "nav2_behaviors::DriveOnHeading"
     wait:
-      plugin: "nav2_behaviors/Wait"
+      plugin: "nav2_behaviors::Wait"
     assisted_teleop:
-      plugin: "nav2_behaviors/AssistedTeleop"
+      plugin: "nav2_behaviors::AssistedTeleop"
     local_frame: odom
     global_frame: map
     robot_base_frame: base_link
@@ -309,7 +309,96 @@ velocity_smoother:
     min_velocity: [-0.26, 0.0, -1.0]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
-    odom_topic: "odom"
+    odom_topic: "odometry/global"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
+
+collision_monitor:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    cmd_vel_in_topic: "cmd_vel_smoothed"
+    cmd_vel_out_topic: "cmd_vel"
+    state_topic: "collision_monitor_state"
+    transform_tolerance: 0.2
+    source_timeout: 1.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"]
+    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
+      visualize: False
+      enabled: True
+    observation_sources: ["scan"]
+    scan:
+      type: "scan"
+      topic: "scan"
+      min_height: 0.15
+      max_height: 2.0
+      enabled: True
+
+
+
+docking_server:
+  ros__parameters:
+    controller_frequency: 10.0
+    initial_perception_timeout: 20.0  # Default 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.05
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['rosmaster_x3_dock']
+    rosmaster_x3_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.02
+      staging_x_offset: 0.75
+      staging_yaw_offset: 3.14
+      use_external_detection_pose: true
+      use_battery_status: false
+      use_stall_detection: false
+      stall_velocity_threshold: 1.0
+      stall_effort_threshold: 1.0
+      charging_threshold: 0.5
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: 1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    # dock_database: $(find-pkg-share yahboom_rosmaster_docking)/config/dock_database.yaml
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      beta: 0.4
+      lambda: 2.0
+      v_linear_min: 0.1
+      v_linear_max: 0.15
+      v_angular_max: 0.75
+      slowdown_radius: 0.25
+      use_collision_detection: true
+      costmap_topic: "/local_costmap/costmap_raw"
+      footprint_topic: "/local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3