Ubuntu 20.04 LTS

ROS2 Foxy

Navigation2

Hello,

I followed the Automatic Addison ROS2 & Nav2 Tutorial on his site. He has about 5-10 pages describing a more detailed tutorial than in the Nav2 site. I changed a few things to get it to work with my personal maps.

I have a couple problems, but I think they're all related; like one could fix the rest. Firstly, rviz2 is having a problem with the costmap and displaying the map I provide. I think this is due to there being multiple ros2 nodes with the same name, because that is described in the terminal output during the launch. Below is the terminal output that is important, I'd post the whole thing as a txt but I don't have 5 points.

[rviz2-5] [WARN] [1659053593.580478223] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-5] [WARN] [1659053593.595203736] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-5] [WARN] [1659053593.681472165] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.

This leads to rviz2 looking weird, where the map is correctly placed how I want it but then there's an outline of the borders that is rotated about the lower-left corner of the map. Again, I'd post a picture but I don't have 5 points yet. Since rviz2 depicts this, I believe this is what is throwing off the costmap. The planner server is saying that that the robot is out of bounds of the costmap because I think it's rotated 90 deg CW, which is depicted in rviz2. I searched for some help about multiple rviz nodes being launched but didn't find anything of use.

[planner_server-10] [WARN] [1659053596.443367934] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-10] [WARN] [1659053597.443415877] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-10] [WARN] [1659053598.443491019] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-10] [WARN] [1659053599.443444337] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-10] [WARN] [1659053600.443372279] [nav2_costmap_2d]: Robot is out of bounds of the costmap!
[planner_server-10] [WARN] [1659053601.443978596] [nav2_costmap_2d]: Robot is out of bounds of the costmap!

Here is my launch file:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, EmitEvent, RegisterEventHandler
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from nav2_common.launch import ReplaceString

def generate_launch_description():

  # Set the path to different files and folders.
  pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')   
  pkg_share = FindPackageShare(package='basic_robot').find('basic_robot')
  default_launch_dir = os.path.join(pkg_share, 'launch')
  default_model_path = os.path.join(pkg_share, 'models/basic_bot_v1.urdf')
  robot_localization_file_path = os.path.join(pkg_share, 'config/ekf2.yaml') # Original
  # robot_localization_file_path = os.path.join(pkg_share, 'config/ekf2.yaml') # Drone change
  robot_name_in_urdf = 'basic_robot'
  # default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')
  default_rviz_config_path = os.path.join(pkg_share, 'rviz/nav2_config.rviz')
  world_file_name = 'blocks_world/blocks_world.world'
  world_path = os.path.join(pkg_share, 'worlds', world_file_name)
  nav2_dir = FindPackageShare(package='nav2_bringup').find('nav2_bringup') 
  nav2_launch_dir = os.path.join(nav2_dir, 'launch')
  # static_map_path = os.path.join(pkg_share, 'maps', 'smalltown_world.yaml') # Original
  static_map_path = os.path.join(pkg_share, 'maps', 'blocksworld.yaml') # Drone change
  nav2_params_path = os.path.join(pkg_share, 'params', 'nav2_params.yaml')
  nav2_bt_path = FindPackageShare(package='nav2_bt_navigator').find('nav2_bt_navigator')
  behavior_tree_xml_path = os.path.join(nav2_bt_path, 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')

  # Launch configuration variables specific to simulation
  autostart = LaunchConfiguration('autostart')
  default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
  headless = LaunchConfiguration('headless')
  map_yaml_file = LaunchConfiguration('map')
  model = LaunchConfiguration('model')
  namespace = LaunchConfiguration('namespace')
  params_file = LaunchConfiguration('params_file')
  rviz_config_file = LaunchConfiguration('rviz_config_file')
  slam = LaunchConfiguration('slam')
  use_namespace = LaunchConfiguration('use_namespace')
  use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
  use_rviz = LaunchConfiguration('use_rviz')
  use_sim_time = LaunchConfiguration('use_sim_time')
  use_simulator = LaunchConfiguration('use_simulator')
  world = LaunchConfiguration('world')

  # Map fully qualified names to relative ones so the node's namespace can be prepended.
  # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
  # https://github.com/ros/geometry2/issues/32
  # https://github.com/ros/robot_state_publisher/pull/30
  # TODO(orduno) Substitute with `PushNodeRemapping`
  #              https://github.com/ros2/launch_ros/issues/56
  remappings = [('/tf', 'tf'),
                ('/tf_static', 'tf_static')]

  # Declare the launch arguments
  declare_namespace_cmd = DeclareLaunchArgument(
    name='namespace',
    default_value='',
    description='Top-level namespace')

  declare_use_namespace_cmd = DeclareLaunchArgument(
    name='use_namespace',
    default_value='False',
    description='Whether to apply a namespace to the navigation stack')

  declare_autostart_cmd = DeclareLaunchArgument(
    name='autostart', 
    default_value='true',
    description='Automatically startup the nav2 stack')

  declare_bt_xml_cmd = DeclareLaunchArgument(
    name='default_bt_xml_filename',
    default_value=behavior_tree_xml_path,
    description='Full path to the behavior tree xml file to use')

  declare_map_yaml_cmd = DeclareLaunchArgument(
    name='map',
    default_value=static_map_path,
    description='Full path to map file to load')

  declare_model_path_cmd = DeclareLaunchArgument(
    name='model', 
    default_value=default_model_path, 
    description='Absolute path to robot urdf file')

  declare_params_file_cmd = DeclareLaunchArgument(
    name='params_file',
    default_value=nav2_params_path,
    description='Full path to the ROS2 parameters file to use for all launched nodes')

  declare_rviz_config_file_cmd = DeclareLaunchArgument(
    name='rviz_config_file',
    default_value=default_rviz_config_path,
    description='Full path to the RVIZ config file to use')

  declare_simulator_cmd = DeclareLaunchArgument(
    name='headless',
    default_value='False',
    description='Whether to execute gzclient')

  declare_slam_cmd = DeclareLaunchArgument(
    name='slam',
    default_value='False',
    description='Whether to run SLAM')

  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
    name='use_robot_state_pub',
    default_value='True',
    description='Whether to start the robot state publisher')

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

  declare_use_sim_time_cmd = DeclareLaunchArgument(
    name='use_sim_time',
    default_value='False',
    description='Use simulation (Gazebo) clock if true')

  declare_use_simulator_cmd = DeclareLaunchArgument(
    name='use_simulator',
    default_value='True',
    description='Whether to start the simulator')

  declare_world_cmd = DeclareLaunchArgument(
    name='world',
    default_value=world_path,
    description='Full path to the world model file to load')

  # Specify the actions

  # Start Gazebo server
  start_gazebo_server_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
    condition=IfCondition(use_simulator),
    launch_arguments={'world': world}.items())

  # Start Gazebo client    
  start_gazebo_client_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),
    condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])))

  # Start robot localization using an Extended Kalman filter
  start_robot_localization_cmd = Node(
    package='robot_localization',
    executable='ekf_node',
    name='ekf_filter_node',
    output='screen',
    parameters=[robot_localization_file_path, 
    {'use_sim_time': use_sim_time}])

  # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
  start_robot_state_publisher_cmd = Node(
    condition=IfCondition(use_robot_state_pub),
    package='robot_state_publisher',
    executable='robot_state_publisher',
    namespace=namespace,
    parameters=[{'use_sim_time': use_sim_time, 
    'robot_description': Command(['xacro ', model])}],
    remappings=remappings,
    arguments=[default_model_path])

  # Launch RViz
  start_rviz_cmd = Node(
    condition=IfCondition(use_rviz),
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', rviz_config_file])

  namespaced_rviz_config_file = ReplaceString(
    source_file=rviz_config_file,
    replacements={'<robot_namespace>': ('/', namespace)})

  start_namespaced_rviz_cmd = Node(
    condition=IfCondition(use_namespace),
    package='rviz2',
    executable='rviz2',
    namespace=namespace,
    arguments=['-d', namespaced_rviz_config_file],
    output='screen',
    remappings=[('/tf', 'tf'),
                ('/tf_static', 'tf_static'),
                ('/goal_pose', 'goal_pose'),
                ('/clicked_point', 'clicked_point'),
                ('/initialpose', 'initialpose')])

  exit_event_handler = RegisterEventHandler(
    condition=UnlessCondition(use_namespace),
    event_handler=OnProcessExit(
      target_action=start_rviz_cmd,
      on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))

  exit_event_handler_namespaced = RegisterEventHandler(
    condition=IfCondition(use_namespace),
    event_handler=OnProcessExit(
      target_action=start_namespaced_rviz_cmd,
      on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))

  # Launch the ROS 2 Navigation Stack
  start_ros2_navigation_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(os.path.join(nav2_launch_dir, 'bringup_launch.py')),
    launch_arguments = {'namespace': namespace,
                        'use_namespace': use_namespace,
                        'slam': slam,
                        'map': map_yaml_file,
                        'use_sim_time': use_sim_time,
                        'params_file': params_file,
                        'default_bt_xml_filename': default_bt_xml_filename,
                        'autostart': autostart}.items())

  # Launch the joint state publisher
  joint_state_publisher_node = Node(
    package='joint_state_publisher',
    executable='joint_state_publisher',
    name='joint_state_publisher')

  transform_odom_map = Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'])

  transform_map_base_footprint = Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint'])


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

  # Declare the launch options
  ld.add_action(declare_namespace_cmd)
  ld.add_action(declare_use_namespace_cmd)
  ld.add_action(declare_autostart_cmd)
  ld.add_action(declare_bt_xml_cmd)
  ld.add_action(declare_map_yaml_cmd)
  ld.add_action(declare_model_path_cmd)
  ld.add_action(declare_params_file_cmd)
  ld.add_action(declare_rviz_config_file_cmd)
  ld.add_action(declare_simulator_cmd)
  ld.add_action(declare_slam_cmd)
  ld.add_action(declare_use_robot_state_pub_cmd)  
  ld.add_action(declare_use_rviz_cmd) 
  ld.add_action(declare_use_sim_time_cmd)
  ld.add_action(declare_use_simulator_cmd)
  ld.add_action(declare_world_cmd)

  # Add any actions
  ld.add_action(start_gazebo_server_cmd)
  ld.add_action(start_gazebo_client_cmd)
  ld.add_action(start_robot_localization_cmd)
  ld.add_action(start_robot_state_publisher_cmd)
  ld.add_action(start_rviz_cmd)
  ld.add_action(start_namespaced_rviz_cmd)
  ld.add_action(exit_event_handler)
  ld.add_action(exit_event_handler_namespaced)
  ld.add_action(start_ros2_navigation_cmd)

  ld.add_action(joint_state_publisher_node)
  ld.add_action(transform_odom_map)
  ld.add_action(transform_map_base_footprint)

  return ld

The multiple rviz2 nodes could be coming from rviz being called later down the pipeline after launching bringup_launch.py, which launches other py files from with it; it's a long chain of events to enable navigation and path planning. I tried disabling some rviz2 nodes from my launch files but that didn't work. My main goal is to just get navigation working in my own maps (I'm struggling very hard with this, as I'm brand new to ROS, ROS2, and Nav2); like to set the pose and goal and for it to plan a path. Please let me know if there's anything else I'm missing; if you'd like to see a .urdf, .rviz, .yaml, params file, etc.