0
$\begingroup$

I'm trying to set up Nav2 (ROS2 Humble + Ubuntu 22.04.3 LTS) to work in a simulation environment.

When I start the nav2 launch file, I see this on RVIZ, if from the Nav2 launch file I remove the comments on the remapping of the tf it will stop. With the remapping it will not work. All the tf are published on the /tf topic.

You can see here what is happening to the robot.

Thanks.

This question is also publish here.

Nav2 launch file

#!/usr/bin/env python3

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from nav2_common.launch import ReplaceString, RewrittenYaml


def generate_launch_description():
    # Specify the name of the package
    pkg_name = "scout_navigation"
    namespace = "scout_mini"

    config_file_dir = os.path.join(get_package_share_directory(pkg_name), "config")

    # Arguments and parameters
    use_sim_time = LaunchConfiguration("use_sim_time", default="true")
    map_name = LaunchConfiguration("map_name", default="workshop_big_cartographer.yaml")

    declare_use_sim_time_arg = DeclareLaunchArgument(
        "use_sim_time",
        default_value=use_sim_time,
        description="Use simulation (Gazebo) clock if true",
    )

    declare_map_name_arg = DeclareLaunchArgument(
        "map_name", default_value=map_name, description="Specify map name"
    )

    # Add namespace to robot_localization parameter files
    namespaced_ekf_localization_params = ReplaceString(
        source_file=os.path.join(config_file_dir, "ekf_localization_with_gps.yaml"),
        replacements={"namespace": namespace},
    )

    namespaced_nav2_params = ReplaceString(
        source_file=os.path.join(config_file_dir, "nav2_params.yaml"),
        replacements={"/namespace": ("/", namespace)},
    )

    namespaced_nav2_params = RewrittenYaml(
        source_file=namespaced_nav2_params,
        root_key=namespace,
        param_rewrites={
            "yaml_filename": PathJoinSubstitution(
                [get_package_share_directory(pkg_name), "maps", map_name]
            )
        },
        convert_types=True,
    )

    # Nodes
    rviz2_node = Node(
        namespace=namespace,
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        arguments=[
            "-d",
            os.path.join(
                get_package_share_directory(pkg_name),
                "rviz",
                "scout_mini_navigation.rviz",
            ),
        ],
        parameters=[{"use_sim_time": use_sim_time}],
        output="screen",
        # remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
    )

    robot_localization_local_node = Node(
        namespace=namespace,
        package="robot_localization",
        executable="ekf_node",
        name="ekf_local_filter_node",
        output="screen",
        parameters=[namespaced_ekf_localization_params, {"use_sim_time": use_sim_time}],
        remappings=[
            #    ("/tf", "tf"),
            #    ("/tf_static", "tf_static"),
            ("odometry/filtered", "odometry/filtered/local"),
        ],
    )

    robot_localization_global_node = Node(
        namespace=namespace,
        package="robot_localization",
        executable="ekf_node",
        name="ekf_global_filter_node",
        output="screen",
        parameters=[namespaced_ekf_localization_params, {"use_sim_time": use_sim_time}],
        remappings=[
            #    ("/tf", "tf"),
            #    ("/tf_static", "tf_static"),
            ("odometry/filtered", "odometry/filtered/global"),
        ],
    )

    navsat_transform_node = Node(
        namespace=namespace,
        package="robot_localization",
        executable="navsat_transform_node",
        name="navsat_transform_node",
        output="screen",
        parameters=[namespaced_ekf_localization_params, {"use_sim_time": use_sim_time}],
        remappings=[
            ("imu/data", "imu"),
            ("gps/fix", "gps"),
            #    ("/tf", "tf"),
            #    ("/tf_static", "tf_static"),
            ("odometry/filtered", "odometry/filtered/global"),
        ],
    )

    nav2_bt_node = Node(
        namespace=namespace,
        package="nav2_bt_navigator",
        executable="bt_navigator",
        name="bt_navigator",
        output="screen",
        respawn=False,
        respawn_delay=2.0,
        parameters=[namespaced_nav2_params, {"use_sim_time": use_sim_time}],
        arguments=["--ros-args", "--log-level", "info"],
        # remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
    )

    nav2_planner_node = Node(
        namespace=namespace,
        package="nav2_planner",
        executable="planner_server",
        name="planner_server",
        output="screen",
        respawn=False,
        respawn_delay=2.0,
        parameters=[namespaced_nav2_params, {"use_sim_time": use_sim_time}],
        arguments=["--ros-args", "--log-level", "info"],
        # remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
    )

    nav2_controller_node = Node(
        namespace=namespace,
        package="nav2_controller",
        executable="controller_server",
        name="controller_server",
        output="screen",
        respawn=False,
        respawn_delay=2.0,
        parameters=[namespaced_nav2_params, {"use_sim_time": use_sim_time}],
        arguments=["--ros-args", "--log-level", "info"],
        remappings=[
            # ("/tf", "tf"),
            # ("/tf_static", "tf_static"),
            ("/cmd_vel", "cmd_vel_nav"),
        ],
    )

    nav2_map_server_node = Node(
        namespace=namespace,
        package="nav2_map_server",
        executable="map_server",
        name="map_server",
        output="screen",
        respawn=False,
        respawn_delay=2.0,
        parameters=[namespaced_nav2_params, {"use_sim_time": use_sim_time}],
        arguments=["--ros-args", "--log-level", "info"],
        # remappings=[("/tf", "tf")],
    )

    nav2_amcl_node = Node(
        namespace=namespace,
        package="nav2_amcl",
        executable="amcl",
        name="amcl",
        output="screen",
        respawn=False,
        respawn_delay=2.0,
        parameters=[namespaced_nav2_params, {"use_sim_time": use_sim_time}],
        arguments=["--ros-args", "--log-level", "info"],
        # remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
    )

    nav2_behavior_server_node = Node(
        namespace=namespace,
        package="nav2_behaviors",
        executable="behavior_server",
        name="behavior_server",
        output="screen",
        respawn=False,
        respawn_delay=2.0,
        parameters=[namespaced_nav2_params, {"use_sim_time": use_sim_time}],
        arguments=["--ros-args", "--log-level", "info"],
        # remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
    )

    nav2_smoother_server_node = Node(
        namespace=namespace,
        package="nav2_smoother",
        executable="smoother_server",
        name="smoother_server",
        output="screen",
        respawn=False,
        respawn_delay=2.0,
        parameters=[namespaced_nav2_params, {"use_sim_time": use_sim_time}],
        arguments=["--ros-args", "--log-level", "info"],
        # remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
    )

    nav2_waypoint_follower_node = Node(
        namespace=namespace,
        package="nav2_waypoint_follower",
        executable="waypoint_follower",
        name="waypoint_follower",
        output="screen",
        respawn=False,
        respawn_delay=2.0,
        parameters=[namespaced_nav2_params, {"use_sim_time": use_sim_time}],
        arguments=["--ros-args", "--log-level", "info"],
        # remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
    )

    nav2_lifecycle_manager_node = Node(
        namespace=namespace,
        package="nav2_lifecycle_manager",
        executable="lifecycle_manager",
        name="lifecycle_manager_navigation",
        output="screen",
        arguments=["--ros-args", "--log-level", "info"],
        parameters=[
            {"use_sim_time": use_sim_time},
            {"autostart": True},
            {"bond_timeout": 4.0},
            {"attempt_respawn_reconnection": True},
            {"bond_respawn_max_duration": 10.0},
            {
                "node_names": [
                    "map_server",
                    "controller_server",
                    "planner_server",
                    "amcl",
                    "behavior_server",
                    "smoother_server",
                    "waypoint_follower",
                    "bt_navigator",
                ]
            },
        ],
    )

    ld = LaunchDescription()

    # Declare the launch options
    ld.add_action(declare_use_sim_time_arg)
    ld.add_action(declare_map_name_arg)

    # Add the commands to the launch description
    ld.add_action(robot_localization_local_node)
    ld.add_action(robot_localization_global_node)
    # ld.add_action(navsat_transform_node) # TODO: questa linea e' commentata per evitare di attivare tutte le volte il servizio GPS.
    ld.add_action(nav2_map_server_node)
    ld.add_action(nav2_controller_node)
    ld.add_action(nav2_planner_node)
    ld.add_action(nav2_behavior_server_node)
    ld.add_action(nav2_amcl_node)
    ld.add_action(nav2_smoother_server_node)
    ld.add_action(nav2_waypoint_follower_node)
    ld.add_action(nav2_bt_node)
    ld.add_action(nav2_lifecycle_manager_node)

    ld.add_action(rviz2_node)

    return ld

Nav2 params file

### NAV2 config file ###
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: True
      width: 3
      height: 3
      resolution: 0.05
      footprint: "[ [0.290, 0.3075], [0.290, -0.3075], [-0.290, 0.3075], [-0.290, -0.3075] ]" # set on the size of the Scout Mini 615x580mm
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /namespace/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /namespace/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.1
      always_send_full_costmap: True
    
  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:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint: "[ [0.290, 0.3075], [0.290, -0.3075], [-0.290, 0.3075], [-0.290, -0.3075] ]" # set on the size of the Scout Mini 615x580mm
      resolution: 0.05
      track_unknown_space: True
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /namespace/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /namespace/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.1
      always_send_full_costmap: True

  global_costmap_client:
    ros__parameters:
      use_sim_time: True

  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      use_astar: false
      allow_unknown: true
      tolerance: 0.5

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
    failure_tolerance: 0.3
    odom_topic: /namespace/odom
    progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older
    goal_checker_plugin: ["goal_checker"]
    controller_plugins: ["FollowPath"]
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 2.77
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 2.77 # this is set for Scout Mini (max speed 10km/h)
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      # TODO: I don't know what to set for this 6 parameters, which is the maximum accelaratin for the robot?
      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
      xy_goal_tolerance: 0.25
      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: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /namespace/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.
    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_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

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

amcl:
  ros__parameters:
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    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: 25.0 # 25 meters for indoor is the max for RPlidar A3, put -1 as default value and it will take the max value available
    laser_min_range: 0.2 # 0.2 meters is the min for RPlidar A3, -1 as default value and it will take the min value available
    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 # not used in likelihood mode
    z_rand: 0.5
    z_short: 0.05 # not used in likelihood mode
    scan_topic: /namespace/scan
    map_topic: /namespace/map
    set_initial_pose: true # this should be set to false in real world scenario. 
    always_reset_initial_pose: false
    first_map_only: false
    initial_pose:
      x: 0.0
      y: 0.0
      z: 0.0
      yaw: 3.14

map_server:
  ros__parameters:
    use_sim_time: True
    yaml_filename: "workshop.yaml"
    topic_name: /namespace/map
    frame_id: map

behavior_server:
  ros__parameters:
    local_costmap_topic: /namespace/local_costmap/costmap_raw
    local_footprint_topic: /namespace/local_costmap/published_footprint
    global_costmap_topic: /namespace/global_costmap/costmap_raw
    global_footprint_topic: /namespace/global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait", "assisted_teleop"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    assisted_teleop:
      plugin: "nav2_behaviors/AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link
    transform_timeout: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

waypoint_follower:
  ros__parameters:
    loop_rate: 100
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"   
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 2000 

smoother_server:
  ros__parameters:
    costmap_topic: /namespace/global_costmap/costmap_raw
    footprint_topic: /namespace/global_costmap/published_footprint
    robot_base_frame: base_link
    transform_timeout: 0.1
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      do_refinement: True
$\endgroup$

1 Answer 1

0
$\begingroup$

It looks like you have conflicting TF publishers that are publishing the same transform to different spots resulting in the jitter.

If you visualize in different frames of reference (base_link, odom, map, etc) you should be able to identify what transform is being republished differently (because in some frame it'll stop happening; at which that and below is fine).

$\endgroup$
1
  • $\begingroup$ Selecting the base_link frame the robot is stopped and the map is moving up and down. $\endgroup$ Commented Feb 20 at 9:59

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.