0
$\begingroup$

Setup: I am using Ubuntu 22.04 and ROS2 Iron, Two 3D Velodynes, a radar.

I am using a ros2 bag in which I recorded the autonomous vehicle launched topics (just to get the sensors data: GPS, odom, imu, lidar, radar, etc) these topics are provided here: https://pastebin.com/Z4jxWML0

Then I run my launch files to provide nav2 modules:

main launch

urdf parser launch file

ISSUES

I am experiencing issues with transitioning the Navigation2 stack from simulation to a real-world environment on a ROS2-based autonomous vehicle project. The local and global costmaps are not appearing when using real data, unlike in the simulation where everything functions as expected.

Symptoms:

  • Local and global costmaps do not appear when using real data, despite appearing normally in simulation.
  • The transformation from odom to base_footprint is missing, which was apparently managed by a Gazebo plugin in the simulation.
  • Only the base_footprint of the robot appears when manually adding a static transform from map to odom.

Troubleshooting Steps Taken:

  • Static Transforms: Implemented static transforms for map to odom and base_footprint to fill in missing transforms not managed by the Gazebo plugin in the real robot setup.

  • Configuration Adjustments (nav2_params.yaml): Modified observation sources in costmap configuration files to real robot topics.

  • Simulation Parameters: Ensured all sim_time parameters are set to False in all configuration files to avoid using simulation time. Topic Throttling:

  • Frequency Adjustment: Attempted to throttle real robot sensor data frequencies to match those in simulation to rule out issues related to message frequency and Quality of Service (QoS) compatibility.

  • TF2 Frames Review: Utilized tf2_tools to view frame relationships, and noted the absence of a transform from map to utm, which occurs naturally in simulation (due navsat node functionality).

Regarding the data and input topics for Navsat node:

Simulation topics: gps/fix, imu and odom

Simulation Configuration File: Simulation Navsat Config File

Tf2 tree in simulation:

enter image description here

Real Robot Topics: real_gps/fix, imu and odom

Real Robot Configuration File: Real Robot Navsat Config File

Real Robot tf2 Frames (without force static transform)

enter image description here

Real Robot Tf2 frames (forcing static transform from odom to base_footprint and from map to odom)

enter image description here

Despite these adjustments, the costmaps fail to appear, suggesting an underlying issue possibly related to the handling or transformation of real sensor data compared to simulated data.

Another peculiar fact that I have noticed, was that for the simulation, even if I comment out all the costmap layers and observation sources, I still get the costmap (local and global produced) as the rosgraph displays below:

enter image description here

However for the real robot that does not happens: The footprint appears just after using the static transforms aforementioned

enter image description here

Finally my terminal output displays:

[navsat_transform_node-5] [INFO] [1715706152.560756579] [navsat_transform]: Datum UTM coordinate is (23K, 337662.32, 7382515.82)
[navsat_transform_node-5] [INFO] [1715706152.647946686] [navsat_transform]: Datum (latitude, longitude, altitude) is (-23.66, -46.59, 0.00)
[navsat_transform_node-5] [INFO] [1715706152.648044421] [navsat_transform]: Datum UTM coordinate is (23K, 337662.33, 7382515.82)
[navsat_transform_node-5] [INFO] [1715706152.763398716] [navsat_transform]: Datum (latitude, longitude, altitude) is (-23.66, -46.59, 0.00)
[navsat_transform_node-5] [INFO] [1715706152.763495528] [navsat_transform]: Datum UTM coordinate is (23K, 337662.35, 7382515.84)
[navsat_transform_node-5] [INFO] [1715706152.911031377] [navsat_transform]: Datum (latitude, longitude, altitude) is (-23.66, -46.59, 0.00)
[navsat_transform_node-5] [INFO] [1715706152.911162837] [navsat_transform]: Datum UTM coordinate is (23K, 337662.37, 7382515.85)

Terminal split about loading the costmap server:

ypoint_follower-11]     Waiting on external lifecycle transitions to activate
[waypoint_follower-11]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[navsat_transform_node-5] [WARN] [1715705945.673438182] [navsat_transform]: Parameter 'broadcast_utm_transform' has been deprecated. Please use 'broadcast_cartesian_transform' instead.
[lifecycle_manager-13] [INFO] [1715705945.690438919] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
[bt_navigator-10] [INFO] [1715705945.693353608] [bt_navigator]: 
[bt_navigator-10]   bt_navigator lifecycle node launched. 
[bt_navigator-10]   Waiting on external lifecycle transitions to activate
[bt_navigator-10]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[waypoint_follower-11] [INFO] [1715705945.696551544] [waypoint_follower]: Creating
[planner_server-8] [INFO] [1715705945.707314098] [planner_server]: 
[planner_server-8]  planner_server lifecycle node launched. 
[planner_server-8]  Waiting on external lifecycle transitions to activate
[planner_server-8]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[ekf_node-4] [ERROR] [1715705945.713663272] [ekf_filter_node_map]: Warning: /imu is listed as an input topic, but all its update variables are false
[smoother_server-7] [INFO] [1715705945.715879221] [smoother_server]: 
[smoother_server-7]     smoother_server lifecycle node launched. 
[smoother_server-7]     Waiting on external lifecycle transitions to activate
[smoother_server-7]     See https://design.ros2.org/articles/node_lifecycle.html for more information.
[behavior_server-9] [INFO] [1715705945.732727322] [behavior_server]: 
[behavior_server-9]     behavior_server lifecycle node launched. 
[behavior_server-9]     Waiting on external lifecycle transitions to activate
[behavior_server-9]     See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-6] [INFO] [1715705945.781287149] [controller_server]: 
[controller_server-6]   controller_server lifecycle node launched. 
[controller_server-6]   Waiting on external lifecycle transitions to activate
[controller_server-6]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[smoother_server-7] [INFO] [1715705945.853210743] [smoother_server]: Creating smoother server
[bt_navigator-10] [INFO] [1715705945.853220797] [bt_navigator]: Creating
[planner_server-8] [INFO] [1715705945.853245434] [planner_server]: Creating
[controller_server-6] [INFO] [1715705945.869668582] [controller_server]: Creating controller server
[planner_server-8] [INFO] [1715705945.873923819] [global_costmap.global_costmap]: 
[planner_server-8]  global_costmap lifecycle node launched. 
[planner_server-8]  Waiting on external lifecycle transitions to activate
[planner_server-8]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-6] [INFO] [1715705945.887981981] [local_costmap.local_costmap]: 
[controller_server-6]   local_costmap lifecycle node launched. 
[controller_server-6]   Waiting on external lifecycle transitions to activate
[controller_server-6]   See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-8] [INFO] [1715705945.889000442] [global_costmap.global_costmap]: Creating Costmap
[lifecycle_manager-13] [INFO] [1715705945.890103671] [lifecycle_manager_navigation]: Starting managed nodes bringup...
[lifecycle_manager-13] [INFO] [1715705945.890139528] [lifecycle_manager_navigation]: Configuring controller_server
[controller_server-6] [INFO] [1715705945.890181386] [local_costmap.local_costmap]: Creating Costmap
[controller_server-6] [INFO] [1715705946.390705429] [controller_server]: Configuring controller interface
[controller_server-6] [INFO] [1715705946.390727834] [controller_server]: getting progress checker plugins..
[controller_server-6] [INFO] [1715705946.391641374] [controller_server]: getting goal checker plugins..
[controller_server-6] [INFO] [1715705946.391748519] [controller_server]: Controller frequency set to 50.0000Hz
[controller_server-6] [INFO] [1715705946.391784022] [local_costmap.local_costmap]: Configuring
[controller_server-6] [INFO] [1715705946.399514949] [local_costmap.local_costmap]: Using plugin "voxel_layer"
[controller_server-6] [INFO] [1715705946.418999573] [local_costmap.local_costmap]: Subscribed to Topics: left/left_scan right/right_scan /scanner/cloud
[controller_server-6] [INFO] [1715705946.449567279] [local_costmap.local_costmap]: Initialized plugin "voxel_layer"
[controller_server-6] [INFO] [1715705946.449594160] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-6] [INFO] [1715705946.450367695] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[controller_server-6] [INFO] [1715705946.463645169] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-6] [INFO] [1715705946.463922242] [controller_server]: Controller Server has progress_checker  progress checkers available.
[controller_server-6] [WARN] [1715705946.464266130] [pluginlib.ClassLoader]: given plugin name 'libfront_cabin_goal_checker' should be 'front_cabin_goal_checker' for better portability
[controller_server-6] [INFO] [1715705946.465526861] [controller_server]: Created goal checker : front_cabin_goal_checker of type nav2_bringup/FrontCabinGoalChecker
[controller_server-6] [INFO] [1715705946.465560001] [controller_server]: Loaded xy_goal_tolerance: 1.500000
[controller_server-6] [INFO] [1715705946.465569450] [controller_server]: Loaded yaw_goal_tolerance: 0.250000
[controller_server-6] [INFO] [1715705946.465574471] [controller_server]: Loaded front_offset: 5.000000
[controller_server-6] [INFO] [1715705946.465580397] [controller_server]: Controller Server has front_cabin_goal_checker  goal checkers available.
[controller_server-6] [INFO] [1715705946.475983109] [controller_server]: Created controller : FollowPath of type nav2_mppi_controller::MPPIController
[controller_server-6] [INFO] [1715705946.478360161] [controller_server]: Controller period is equal to model dt. Control sequence shifting is ON
[controller_server-6] [INFO] [1715705946.496015618] [controller_server]: Critic loaded : mppi::critics::PathFollowCritic
[controller_server-6] [INFO] [1715705946.496638923] [controller_server]: ObstaclesCritic instantiated with 1 power and 35.000000 / 1.000000 weights. Critic will collision check based on footprint cost.
[controller_server-6] [INFO] [1715705946.496664030] [controller_server]: Critic loaded : mppi::critics::ObstaclesCritic
[controller_server-6] [INFO] [1715705946.497411503] [controller_server]: ReferenceTrajectoryCritic instantiated with 2 power and 10.000000 weight
[controller_server-6] [INFO] [1715705946.497649139] [controller_server]: Critic loaded : mppi::critics::PathAlignCritic
[controller_server-6] [INFO] [1715705946.498421300] [controller_server]: PathAngleCritic instantiated with 2 power and 3.600000 weight. Mode set to: Consider Feasible Path Orientations
[controller_server-6] [INFO] [1715705946.498449098] [controller_server]: Critic loaded : mppi::critics::PathAngleCritic
[controller_server-6] [INFO] [1715705946.504617861] [controller_server]: Optimizer reset
[controller_server-6] [INFO] [1715705946.509008386] [MPPIController]: Configured MPPI Controller: FollowPath
[controller_server-6] [INFO] [1715705946.509033605] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-13] [INFO] [1715705946.518091827] [lifecycle_manager_navigation]: Configuring smoother_server
[smoother_server-7] [INFO] [1715705946.518253214] [smoother_server]: Configuring smoother server
[smoother_server-7] [INFO] [1715705946.533303127] [smoother_server]: Created smoother : simple_smoother of type nav2_smoother::SimpleSmoother
[smoother_server-7] [INFO] [1715705946.534196225] [smoother_server]: Smoother Server has simple_smoother  smoothers available.
[lifecycle_manager-13] [INFO] [1715705946.539797209] [lifecycle_manager_navigation]: Configuring planner_server
[planner_server-8] [INFO] [1715705946.540002773] [planner_server]: Configuring
[planner_server-8] [INFO] [1715705946.540033696] [global_costmap.global_costmap]: Configuring
[planner_server-8] [INFO] [1715705946.547330081] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
[planner_server-8] [INFO] [1715705946.553351765] [global_costmap.global_costmap]: Subscribed to Topics: left/left_scan right/right_scan /scanner/cloud
[planner_server-8] [INFO] [1715705946.564061955] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
[planner_server-8] [INFO] [1715705946.564087934] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-8] [INFO] [1715705946.564900006] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-8] [INFO] [1715705946.616520457] [planner_server]: Created global planner plugin GridBased of type nav2_smac_planner/SmacPlannerHybrid
[planner_server-8] [INFO] [1715705946.616560637] [planner_server]: Configuring GridBased of type SmacPlannerHybrid
[planner_server-8] [INFO] [1715705946.639644114] [planner_server]: Configured plugin GridBased of type SmacPlannerHybrid with maximum iterations 1000000, max on approach iterations 1000, and allowing unknown traversal. Tolerance 2.00.Using motion model: Dubin.

Questions:

  1. Could the absence of certain transforms, such as map to utm, critically impact the functionality of the costmap generation?
  2. Are there additional configurations or ROS2 parameters that might need adjustment when transitioning from simulation to real environments?
  3. What diagnostic steps would you recommend to further isolate or resolve the issue with the missing costmaps?
  4. The urdf parsed for Robot Localization is the same as the one I used for simulation (contain the gazebo plugins: ackerman and other sensors related) should I remove the gazebo plugins and use a new urdf for the robot model?
Improve this question
$\endgroup$
7
  • $\begingroup$ You should first fix the issue with the odometry, if your robot doesn't have any way of knowing where it is, it won't be able to navigate. Maybe take a look at the ros2_control diff_drive_controller: control.ros.org/master/doc/ros2_controllers/… $\endgroup$
    – Diego Carvajal
    Commented May 14 at 19:40
  • $\begingroup$ Hi Diego Carvajal thanks for the comment. I have already odom data from my real robot in the same format as the simulation format ( frame_id: odom and child_frame_id: base_footprint) as you can see here in odom_echo file: pastebin.com/xZZa6e9R I don't have however all data in our real /odom that the simulation provides for the simulated /odom topic, since we have a truck without encoders on wheels to measure the distance it has moved from origin. We are relies more on GPS and Imu data than odom. This source is not important for our case, but is required for NavSat... $\endgroup$
    – Marcus Vinicius
    Commented May 14 at 19:47
  • $\begingroup$ The thing here is Nav2 doesn't subscribe to the topic, Nav2 checks the transform between the frames, so you need a node that reads your topic and publishes a transform between the frames, the gazebo plugin used to do that work but with the real robot the transform is not been published. $\endgroup$
    – Diego Carvajal
    Commented May 14 at 19:53
  • $\begingroup$ yes I know, that is one point of the issue. I need to run 2 additional static transforms: ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 odom base_footprint and ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map odom. Then the robot footprint shows up but not the costmaps. To check if the real /odom and /gps data could be the issue, I recorded a ros2 bag from simulation and reproduced in replace of real data, after launch the file, the costmaps are still not being created. Once I find the root of issue I will create a node to transform these frames as you mentioned. $\endgroup$
    – Marcus Vinicius
    Commented May 14 at 20:03
  • $\begingroup$ I am aware something is happening underlying the transforms. I compared using "ros2 node list" from sim vs real robot. The sim provides 5 additional "/transform_listener_impl_xxx..." nodes then real robot. Getting info from it to check what is missing for my real robot, I could not understand what is lacking, the messages are too general: transform_listener_impl_556f29f1d780 Subscribers: /parameter_events: rcl_interfaces/msg/ParameterEvent /tf: tf2_msgs/msg/TFMessage /tf_static: tf2_msgs/msg/TFMessage Publishers: /rosout: rcl_interfaces/msg/Log Service Servers: $\endgroup$
    – Marcus Vinicius
    Commented May 14 at 20:11

2 Answers 2

Reset to default
1
$\begingroup$

Let me put everything as an answer instead of comments.

Transform issue

The problem I see is that your robot publishes to the /odom topic but there is no transform between the odom frame and the base_footprint frame so Nav2 is unable to get the odometry from your robot, this worked in simulation because the gazebo plugin published the /odom topic and broadcasted the transform. You mentioned you used static transforms to be able to see the base of the robot, this works because you are publishing the missing transform however a static transform means your robot will be fixed to a point in space no matter how much the wheels move, what you need is a transform that updates in time with the actual position of your robot, the option here are:

  • Publish the transform using a tf2 broadcaster with the information of your /odom topic.
  • Use ros2_control to control your robot and publish the information of your robot, this will also publish the transform between odom and base_footprint frames, you can check the diff_drive_controller.
  • As you mentioned the ekf_filter_node I assume you are fusing data from different sources for localization, the ekf_node has the option to publish the transform, you can check the parameters.

The /clock topic

The /clock topic is published by gazebo and that is why using the real robot this topic is not published. If the ekf_filter_node is subscribed to it, means the usi_sim_time parameter is set to true so you need to change it.

Improve this answer
$\endgroup$
6
  • $\begingroup$ Hi @Diego Carvajal I have done the broadcaster: pastebin.com/1tHr4reM However I guess the issue remained. The cause is that the ekf_filter_node_map and ekf_filter_node_odom are not properly working. Since they don't output data from the /odometry_local and /odometry_global (which are inputs for the navsat_transform node). After some workaround I adjusted the GPS publishers to fit simulation ros_graph in terms of input and output. Simulation:i.imgur.com/BYIzQi7.png Real_robot: i.imgur.com/ylibFAe.png . I mean I am not understanding why these odom outputs are empty. $\endgroup$
    – Marcus Vinicius
    Commented May 15 at 0:54
  • $\begingroup$ Broadcast of tfs from odom to base_footprint worked. But I needed to create additional transform from map to odom. Maybe this is not correct since the navsat should be doing this...anyway...the utm frame is not being displayed on tf2 frames, since the ekf_filter_node_odom and ekf_filter_node_map are not working. Echoed the /set_pose and /initial_pose both in simulation (where conversion works) and real_robot topics and both are empty, so not receiving messages from this topic seems not to be the root of the issue. Gazebo seems to have decisive role, I don't have a robot example unfortunately. $\endgroup$
    – Marcus Vinicius
    Commented May 15 at 1:00
  • $\begingroup$ Is the /odom topic also empty, or just the outputs from the ekf nodes? $\endgroup$
    – Diego Carvajal
    Commented May 15 at 2:07
  • $\begingroup$ Just the outputs from ekf nodes. Have you experience transitioning these nodes to get real data as well? My team took care to convert CAN msgs to ROS2 gps,Odom and IMU messages is same format. We have just different frequencies. But I still think that the usages of 'gps' namespace into the dual_navsat_ekf_params.yaml to feed odom1 is a trick that just work for sim. I have used rely topics to fit same thing as simulation and avoid gps/fix publish 2 different message types (Odometry and NavsatFix). But since this step is just after get the topics from ekf_filter_node_odom, I should get msg. $\endgroup$
    – Marcus Vinicius
    Commented May 15 at 3:02
  • $\begingroup$ If there is no output from the EKF nodes, the problem should be there, can you add the ekf param files to the question? $\endgroup$
    – Diego Carvajal
    Commented May 15 at 20:50
1
$\begingroup$

I successfully resolved issues related to transitioning the Navigation2 stack from simulation to a real robot for costmap generation. The main adjustments involved configuration settings and ensuring proper namespace and topic management.

Key Fixes

1-Simulation Time Setting:

Set use_sim_time to False in all relevant launch files, aligning the time synchronization across different components of the Navigation2 stack. I had forgotten to set the 'real_navigation_launch.py' node to False.

2- Topic Relay:

Relayed real_gps/fix to gps/fix using ROS 2 topic tools to create a namespace that supports multiple message types, similar to the simulator setup:

 ros2 run topic_tools relay real_gps/fix gps/fix

3- Frame ID Configuration:

Configured appropriate frame_id for each source of navigation data:

  • Odom: frame_id set to odom
  • IMU: frame_id set to base_footprint
  • GPS: After relaying, frame_id for gps/fix set to base_footprint

4. Joint State Publisher:

Integrated a joint_state_publisher in the launch file, previously used in the simulation model. This helped in broadcasting necessary transformations without manually setting a transform from odom to base_footprint.

SYSTEM VERIFICATION: All related topics now return data correctly:

  • /odometry/local and /odometry/global (nav_msgs/msg/Odometry)
  • /odometry/gps (nav_msgs/msg/Odometry)
  • /gps/fix and /gps/filtered (sensor_msgs/msg/NavSatFix)

Timestamp SYNCHRONIZATION

The sensors, initiated via CAN from a microcontroller (Raspberry Pi) and the Navigation2 stack running on a separate computer (NUC), synchronize effectively. ROS 2 handles the timestamp matching seamlessly, negating the need for manual synchronization.

KEY FILES MODIFIED

real_bot_gps_waypoint_follower.launch.py

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
from nav2_common.launch import RewrittenYaml
from launch.actions import ExecuteProcess




def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('nav2_bringup')
    launch_dir = os.path.join(bringup_dir, 'launch')
    params_dir = os.path.join(bringup_dir, "params")

    nav2_params = os.path.join(params_dir, "real_truck_nav2_params.yaml")
    configured_params = RewrittenYaml(
        source_file=nav2_params, root_key="", param_rewrites="", convert_types=True
    )

        # Keep truck static by publishing to /cmd_vel in a loop
    keep_truck_static_cmd = ExecuteProcess(
        cmd=['bash', '-c', "while true; do ros2 topic pub /cmd_vel geometry_msgs/msg/Twist '{linear: {x: -0.8, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}' -1; sleep 0.1; done"],
        output='screen'
    )


    use_rviz = LaunchConfiguration('use_rviz')
    use_mapviz = LaunchConfiguration('use_mapviz')

    declare_use_rviz_cmd = DeclareLaunchArgument(
        'use_rviz',
        default_value='False',
        description='Whether to start RVIZ')

    declare_use_mapviz_cmd = DeclareLaunchArgument(
        'use_mapviz',
        default_value='False',
        description='Whether to start mapviz')

    robot_localization_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'real_bot_dual_ekf_navsat.launch.py'))
    )

    navigation2_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_dir, "launch", "real_navigation_launch.py")
        ),
        launch_arguments={
            "use_sim_time": "False",
            "params_file": configured_params,
            "autostart": "True",
        }.items(),
    )

    rviz_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_dir, "launch", 'rviz_launch.py')),
        condition=IfCondition(use_rviz)
    )

    mapviz_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'mapviz.launch.py')),
        #condition=IfCondition(use_mapviz)
    )

    robot_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'real_truck_environment_launch.py'))
    )


    # Create the launch description and populate
    ld = LaunchDescription()

    # robot launch
    ld.add_action(robot_cmd)

    # robot localization launch
    ld.add_action(robot_localization_cmd)

    # navigation2 launch
    ld.add_action(navigation2_cmd)

    # viz launch
    ld.add_action(declare_use_rviz_cmd)
    ld.add_action(rviz_cmd)
    #ld.add_action(declare_use_mapviz_cmd)
    #ld.add_action(mapviz_cmd)

    #Uncomment when the terrain does not move the truck backwards (plane terrains don't need it)
    #ld.add_action(keep_truck_static_cmd) #FAKE BRAKE


    return ld

real_truck_environment_launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node

def generate_launch_description():
    # Define paths
    bringup_dir = get_package_share_directory('nav2_bringup')

    rviz_config_file = os.path.join(bringup_dir, 'rviz', 'truck.rviz')
    truck_urdf_file = os.path.join(bringup_dir, 'urdf', 'arocs_truck.urdf')
    map_yaml_file = os.path.join(bringup_dir, 'maps', 'infinite_map.yaml')

    # Load URDF file contents into a variable
    with open(truck_urdf_file, 'r') as urdf_file:
        robot_description_content = urdf_file.read()


    # Start RViz2
    start_rviz_cmd = ExecuteProcess(
        cmd=['rviz2', '-d', rviz_config_file],
        output='screen')

    # Robot state publisher
    robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{'use_sim_time': False, 'robot_description': robot_description_content}])

    # Map server node
    start_map_server_cmd = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'yaml_filename': map_yaml_file, 'use_sim_time': False}],
    )

    # Lifecycle manager to handle lifecycle state transitions
    lifecycle_manager_cmd = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager',
        output='screen',
        parameters=[{
            'use_sim_time': True,
            'autostart': True,
            'node_names': ['map_server']
        }]
    )

       # Joint state publisher (Optional if your robot publishes joint states)
    joint_state_publisher_cmd = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen')

    # Create the launch description and add actions
    ld = LaunchDescription()
    ld.add_action(start_rviz_cmd)
    ld.add_action(robot_state_publisher_cmd)
    ld.add_action(joint_state_publisher_cmd)
    #ld.add_action(start_map_server_cmd) # I am using rolling_window = True to avoid need of huge static maps on farm..not practical
    #ld.add_action(lifecycle_manager_cmd) # no previous map for farm.

    return ld

real_bot_dual_ekf_navsat_params.yaml

# For parameter descriptions, please refer to the template parameter files for each node.

ekf_filter_node_odom:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments
    publish_acceleration: false 
    #print_diagnostics: true
    debug: false
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin
    world_frame: odom #map

    odom0: /odom
    odom0_config: [false, false, false,   # Position X, Y, Z
                  false, false, false,    # Orientation roll, pitch, yaw (only yaw is used)
                  true, false, false,   # Velocity X dot, Y dot, Z dot
                  false, false, false,   # Angular Velocity roll dot, pitch dot, yaw dot
                  false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
    odom0_differential: false
    odom0_nodelay: true
    odom0_relative: false
    odom0_queue_size: 10

    imu0: /imu
    imu0_config: [false, false, false,   # Position X, Y, Z
                  false, false, true,    # Orientation roll, pitch, yaw (only yaw is used)
                  false, false, false,   # Velocity X dot, Y dot, Z dot
                  false, false, true,   # Angular Velocity roll dot, pitch dot, yaw dot
                  false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
    imu0_differential: false  # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
    imu0_nodelay: true 
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

    use_control: false

    process_noise_covariance: [1e-3, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    1e-3,  0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    1e-3,  0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.1,   0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.1,   0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.01,  0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1,    0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.1,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.1,   0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.1,   0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.1,   0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.1,   0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.1,   0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1]

ekf_filter_node_map:
  ros__parameters:
    use_sim_time: false 
    sensor_timeout: 0.1
    transform_time_offset: 0.0
    transform_timeout: 0.0
    frequency: 30.0
    two_d_mode: false #true  # Recommended to use 2d mode for nav2 in mostly planar environments
    #print_diagnostics: true
    publish_acceleration: false 
    debug: false
    publish_tf: true
    reset_on_time_jump: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint # the frame id used by the turtlebot's diff drive plugin
    world_frame: map

    odom0: /odom #/odometry/local # = "fused odometry + Imu in previous node"
    odom0_config: [false, false, false,   # Position X, Y, Z
                  false, false, false,    # Orientation roll, pitch, yaw (only yaw is used)
                  true, false, false,   # Velocity X dot, Y dot, Z dot
                  false, false, false,   # Angular Velocity roll dot, pitch dot, yaw dot
                  false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
    odom0_queue_size: 10
    odom0_nodelay: true 
    odom0_differential: false #imu # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
    odom0_relative: false

    imu0: /imu
    imu0_config: [false, false, false,   # Position X, Y, Z
                  false, false, true,    # Orientation roll, pitch, yaw (only yaw is used)
                  false, false, false,   # Velocity X dot, Y dot, Z dot
                  false, false, true,   # Angular Velocity roll dot, pitch dot, yaw dot
                  false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
    imu0_differential: false
    imu0_nodelay: false 
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true


#O problema esta aqui, o gazebo publica gps namespace, porém ele deve poder englobar diferentes tipos de dados (Odometry) e também receber o (NavSatFix). Já quando eu uso real_gps/fix aqui
#Eu envio apeans NavSatFix, mas não tenho um namespace para receber outros tipos de mensagens que vem da Fusão anterior (odometry/local) que o ekf_filter_node_odom está entregaando.

    odom1: odometry/gps # try gps if not fix
    odom1_config: [true, true, false,   # Position X, Y, Z
                  false, false, false,    # Orientation roll, pitch, yaw (only yaw is used)
                  false, false, false,   # Velocity X dot, Y dot, Z dot
                  false, false, false,   # Angular Velocity roll dot, pitch dot, yaw dot
                  false, false, false]   # Acceleration X double dot, Y double dot, Z double dot
    odom1_queue_size: 10
    odom1_nodelay: true
    odom1_differential: false
    odom1_relative: false


    use_control: false

    process_noise_covariance: [1.0,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    1.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    1e-3,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.1,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.1,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.01,   0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.1,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.1,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.1,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.1,    0.0,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.1,    0.0,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.1,    0.0,
                              0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1]

navsat_transform:
  ros__parameters:
    use_sim_time: false
    frequency: 30.0
    delay: 3.0
    magnetic_declination_radians: 0.0 #0.266512
    yaw_offset: 0.0
    zero_altitude: true
    broadcast_utm_transform: true
    publish_filtered_gps: true
    use_odometry_yaw: false #true
    wait_for_datum: false #true 
    #datum: [-23.66, -46.59, 0.0] # pre-set datum if needed, [lat, lon, yaw]

CONCLUSION

These adjustments have enabled the Navigation2 stack to generate global and local costmaps accurately using only localization sensors (GPS, IMU, and Odom). The setup now closely mirrors the simulation environment and effectively manages multiple data types within the same namespace.

This solution could be beneficial for those transitioning from simulation to real-world deployments using the ROS 2 Navigation2 stack.

Result:

enter image description here

My rqt_graph (Navsat Transform node focus)

enter image description here

Improve this answer
$\endgroup$
6
  • $\begingroup$ Glad you were able to solve the issue Marcus. $\endgroup$
    – Diego Carvajal
    Commented May 20 at 0:50
  • $\begingroup$ Hi @DiegoCarvajal, thanks for your assistance! My current solution only worked for a static case because the ROS2 bag data (IMU, ODOM AND GPS) was static. I need a dynamic TF broadcaster. The tutorial you sent is extensive, and my simple Python script didn't work. Do you have a GitHub example? Here's my script: pastebin.com/FDhiAQsA. And the behavior when I stop the script or the ros2 bag ends, my robot starts going back and forth. It seems the old TF data is being compensated... studio.youtube.com/video/QQXS_kR7IRk/edit Should I open a new issue for further help? $\endgroup$
    – Marcus Vinicius
    Commented May 26 at 22:36
  • $\begingroup$ Does your robot move only forward and backward? $\endgroup$
    – Diego Carvajal
    Commented May 27 at 12:48
  • $\begingroup$ Hi in real scenario it moves forward only. I have opened a new issue: robotics.stackexchange.com/questions/111466/… $\endgroup$
    – Marcus Vinicius
    Commented May 27 at 14:06
  • $\begingroup$ @DiegoCarvajal you were correct my broadcaster was still wrong, The fix is in this link robotics.stackexchange.com/questions/111466/… $\endgroup$
    – Marcus Vinicius
    Commented Jun 11 at 10:12

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.