1
$\begingroup$

I tried to create a navigation algorithm using nav2 and GPS data for the global odometry. With the help of the robot_localization package, I successfully created global odometry data by fusing the data from the wheel encoder, IMU, and GPS. When I am trying to add a navigation algorithm for waypoint navigation, nothing is published on the /map topic, and no costmap is generated. No node publishes anything on the /map topic, so I can't run the navigation algorithm: enter image description here

This is my launch file:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, Command
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_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
   # Get the launch directory
   bringup_dir = get_package_share_directory('nav2_bringup')
   gps_wpf_dir = get_package_share_directory("nav2_gps_waypoint_follower_demo")
   launch_dir = os.path.join(gps_wpf_dir, 'launch')
   params_dir = os.path.join(gps_wpf_dir, "config")
   nav2_params = os.path.join(params_dir, "nav2_no_map_params.yaml")
   configured_params = RewrittenYaml(
       source_file=nav2_params, root_key="", param_rewrites="", convert_types=True
   )

   pkg_share = get_package_share_directory('nav2_gps_waypoint_follower_demo')
   pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
   path_lafette = os.path.join(pkg_share, 'urdf', 'Lafette_small_urdf.urdf')

   use_rviz = LaunchConfiguration('use_rviz')

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


   launch_gazebo_cmd = IncludeLaunchDescription(
       PythonLaunchDescriptionSource(os.path.join(pkg_share, 'launch', 'launch_lafette_gazebo.launch.py'))
   )


   robot_localization_cmd = IncludeLaunchDescription(
       PythonLaunchDescriptionSource(
           os.path.join(launch_dir, 'dual_ekf_navsat.launch.py'))
   )
   
   navigation2_cmd = IncludeLaunchDescription(
       PythonLaunchDescriptionSource(os.path.join(bringup_dir, "launch", "navigation_launch.py")),
       launch_arguments={
           "use_sim_time": "True",
           "params_file": configured_params,
           "autostart": "True",
       }.items(),
   )

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


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


   # rviz launch
   ld.add_action(declare_use_rviz_cmd)
   ld.add_action(rviz_cmd)

   #interfaces
   ld.add_action(launch_gazebo_cmd)

   # robot localization launch
   ld.add_action(robot_localization_cmd)

   # navigation2 launch
   ld.add_action(navigation2_cmd)

   return ld

dual_ekf_navsat.launch.py looks like:

from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros.actions
import os
import launch.actions


def generate_launch_description():
   gps_wpf_dir = get_package_share_directory("nav2_gps_waypoint_follower_demo")
   rl_params_file = os.path.join(gps_wpf_dir, "config", "dual_ekf_navsat_params.yaml")

   return LaunchDescription(
       [
           launch.actions.DeclareLaunchArgument(
               "output_final_position", default_value="false"
           ),
           launch.actions.DeclareLaunchArgument(
               "output_location", default_value="~/dual_ekf_navsat_example_debug.txt"
           ),
           launch_ros.actions.Node(
               package="robot_localization",
               executable="ekf_node",
               name="ekf_filter_node_odom",
               output="screen",
               parameters=[rl_params_file, {"use_sim_time": True}],
               remappings=[("odometry/filtered", "odometry/local")],
           ),
           launch_ros.actions.Node(
               package="robot_localization",
               executable="ekf_node",
               name="ekf_filter_node_map",
               output="screen",
               parameters=[rl_params_file, {"use_sim_time": True}],
               remappings=[("odometry/filtered", "odometry/global")],
           ),
           launch_ros.actions.Node(
               package="robot_localization",
               executable="navsat_transform_node",
               name="navsat_transform",
               output="screen",
               parameters=[rl_params_file, {"use_sim_time": True}],
               remappings=[
                   ("imu/data", "imu/data"),
                   ("gps/fix", "gps/fix"),
                   ("gps/filtered", "gps/filtered"),
                   ("odometry/gps", "odometry/gps"),
                   ("odometry/filtered", "odometry/global"),
               ],
           ),
       ]
   )

The configuration file for the EKF in "dual_ekf_navsat_params.yaml" looks like

# Sensor fusion for the odom frame
ekf_filter_node_odom:
 ros__parameters:
   frequency: 30.0
   two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments
   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 # the frame to be fused

   odom0: odom
   odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  true,
                 false, false, true,
                 false, false, false]
   odom0_queue_size: 10
   odom0_differential: false
   odom0_relative: false

   imu0: imu
   imu0_config: [false, false, false,
                 false,  false,  true,
                 false, false, false,
                 false,  false,  false,
                 false,  false,  false]
   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_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.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.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.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.5,    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.5,    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.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.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.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.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.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.3]


# Sensor fusion for the map frame
ekf_filter_node_map:
 ros__parameters:
   frequency: 30.0
   two_d_mode: true  # Recommended to use 2d mode for nav2 in mostly planar environments
   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: map # the frame to be fused

   odom0: odom
   odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  true,
                 false, false, true,
                 false, false, false]
   odom0_queue_size: 10
   odom0_differential: false
   odom0_relative: false

   odom1: odometry/gps
   odom1_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
   odom1_queue_size: 10
   odom1_differential: false
   odom1_relative: false

   imu0: imu
   imu0_config: [false, false, false,
                 false,  false,  true,
                 false, false, false,
                 false,  false,  false,
                 false,  false,  false]
   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_relative: false
   imu0_queue_size: 10
   imu0_remove_gravitational_acceleration: true

   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.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.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.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.5,     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.5,     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.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.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.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.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.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.3]

navsat_transform:
 ros__parameters:
   frequency: 30.0
   delay: 3.0
   magnetic_declination_radians: 0.0
   yaw_offset: 0.0
   zero_altitude: true
   broadcast_utm_transform: true
   publish_filtered_gps: true
   use_odometry_yaw: true
   wait_for_datum: false 

If you could guide me on what might help get the /map topic published, I would be grateful.

I use ros2 humble.

$\endgroup$

0

Your Answer

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