I cannot see my robot in rviz after running a python script

[nidia-bucarelli]

Hi! I created a map of my office. Then, I tried to create a package to run the nav_to_pose.py script (from the repository in the link below) using the command line. I aimed to physically move the robot from its dock station to a coordinate (e.g., x=-3, y=-2). After I built the package, I launched the navigation using ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=off localization:=true map:=my_map.yaml and then ros2 run turtlebot4_python_tutorials nav_to_pose. It did not work, and now I cannot see my robot in Rviz2.

I checked ros2 topic list, and I can see the robot topics. I also checked the domain, and all devices are in the same domain (and WIFI network). I don't know how to fix the problem. Any idea on how to debug this problem?

script: https://github.com/turtlebot/turtlebot4_tutorials/blob/galactic/turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_to_pose.py

[roni-kreinin]

Just to clarify, did you clone the turtlebot4_python_tutorials package and just modify the goal pose coordinate?

[roni-kreinin]

Also are there any errors thrown when you launch navigation or run nav_to_pose?

[nidia-bucarelli]

I did not clone the turtlebot4_python_tutorials. I built the package following the tutorial on how to create a package and node. Below are my steps:

mkdir ~/turtlebot4_ws/src -p
cd ~/turtlebot4_ws/src
ros2 pkg create --build-type ament_python --node-name turtlebot4_first_python_node turtlebot4_python_tutorials

## added the dependencies to package.xml
 <depend>rclpy</depend>
  <depend>irobot_create_msgs</depend> 
  <depend>turtlebot4_navigation.turtlebot4_navigator</depend>

## added the nav_to_pose.py script  (with the modified goal pose coordinate) at ~/turtlebot4_ws/src/turtlebot4_python_tutorials/turtlebot4_python_tutorials/

## edited setup.py file to include the entry point:      
 'console_scripts': [
                        'nav_to_pose = turtlebot4_python_tutorials.nav_to_pose:main',

## built the package
cd ~/turtlebot4_ws
colcon build --symlink-install --packages-select turtlebot4_python_tutorials
source install/local_setup.bash

When I run the nav_to_pose, this is what I get:

For the console that runs ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=off localization:=true map:=map_name.yaml , the terminal keeps printing as below:

[amcl-3] [INFO] [1664471649.604717853] [amcl]: initialPoseReceived
[amcl-3] [INFO] [1664471649.605022387] [amcl]: Setting pose (1664471649.605020): 0.000 0.000 0.000
[amcl-3] [INFO] [1664471649.614370025] [amcl]: initialPoseReceived
[amcl-3] [INFO] [1664471649.614636689] [amcl]: Setting pose (1664471649.614634): 0.000 0.000 0.000
.....

I get the error above, after I run nav_to_pose using a second terminal. This terminal keeps printing as below:

ubuntu@ubuntu:~/turtlebot4_ws$ ros2 run turtlebot4_python_tutorials nav_to_pose
[INFO] [1664471645.209922482] [basic_navigator]: Publishing Initial Pose
[INFO] [1664471647.248345674] [basic_navigator]: Setting initial pose
[INFO] [1664471647.254054186] [basic_navigator]: Publishing Initial Pose
[INFO] [1664471647.257376520] [basic_navigator]: Waiting for amcl_pose to be received
[INFO] [1664471647.260328488] [basic_navigator]: Setting initial pose
[INFO] [1664471647.262624555] [basic_navigator]: Publishing Initial Pose
[INFO] [1664471647.265509801] [basic_navigator]: Waiting for amcl_pose to be received
[INFO] [1664471647.267729203] [basic_navigator]: Setting initial pose
[INFO] [1664471647.269771792] [basic_navigator]: Publishing Initial Pose
.....

When I run ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=off localization:=true map:=map_name.yaml, without calling nav_to_pose, I get a timed out INFO when "activating controller server" and "activating planner server". For example:

[lifecycle_manager_navigation]: Activating controller_server

[controller_server-5] [INFO] [1664472119.340939180] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist
......
[bt_navigator-8] [WARN] [1664472121.165487572] [bt_navigator]: New publisher discovered on topic '/odom', offering incompatible QoS. No messages will be sent to it. Last incompatible policy: RELIABILITY_QOS_POLICY
[controller_server-5] [INFO] [1664472121.340930325] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist
[controller_server-5] [INFO] [1664472121.841315847] [local_costmap.local_costmap]: start
[controller_server-5] [INFO] [1664472121.844359567] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[controller_server-5] [WARN] [1664472121.855134315] [controller_server]: New publisher discovered on topic '/odom', offering incompatible QoS. No messages will be sent to it. Last incompatible policy: RELIABILITY_QOS_POLICY
[lifecycle_manager-10] [INFO] [1664472121.962042238] [lifecycle_manager_navigation]: Server controller_server connected with bond.
[lifecycle_manager-10] [INFO] [1664472121.962206051] 

[lifecycle_manager_navigation]: Activating planner_server

[planner_server-6] [INFO] [1664472121.963482330] [planner_server]: Activating
[planner_server-6] [INFO] [1664472121.963624514] [global_costmap.global_costmap]: Activating
[planner_server-6] [INFO] [1664472121.963687513] [global_costmap.global_costmap]: Checking transform
[planner_server-6] [INFO] [1664472121.963851122] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[planner_server-6] [INFO] [1664472122.463924611] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
......

[nidia-bucarelli]

Hi, @roni-kreinin. Do you have any feedback?

[roni-kreinin]

@nidia-bucarelli Can you call ros2 topic echo /tf and check that the odom->base_link transform is being published? It looks to me like the Create3 is not publishing this transform. Test this without having nav2 or anything else running first.

If the issue starts when you do run nav2, I would recommend you try to switch RMW to Fast-DDS. I would recommend using the script mentioned here to change the RMW across the system.

[nidia-bucarelli]

ros2 topic echo/tf prints information about base_link and odom (see below).

  frame_id: odom
  child_frame_id: base_link
  transform:
    translation:
      x: -0.23105789721012115
      y: -0.1916409581899643
      z: 0.008993580937385559
    rotation:
      x: -0.0012695949990302324
      y: 0.0007280179997906089
      z: -0.8812530636787415
      w: 0.4726424813270569
- header:
    stamp:
      sec: 1665024268
      nanosec: 589006463
    frame_id: odom
  child_frame_id: base_footprint
  transform:
    translation:
      x: -0.23108172979509511
      y: -0.1916315528318968
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: -0.8812535405158997
      w: 0.47264382243156433
....

I checked again, and I can see the robot in Rviz when I am generating a map. But, when I try to navigate using Rviz interface and navigation tools, the robot does not want to show up until I use "2D pose estimation", and it shows up with a different orientation. See images below. Also, it does not understand where it is located as it bumps and does crazy things when I set a navigation goal (for example, it goes south rather than north). Also, when I get the robot connected to the controller, it starts moving/driving by itself for a bit without my command.

I had deleted my script/folder before I posted this issue. Given the error I see now, I now can recall that I changed the orientation of my initial pose to be "SOUTH" (e.g., initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.SOUTH). Perhaps I changed some settings... any thoughts/suggestions?

[roni-kreinin]

The initial pose gives localization a starting point to start calculating where the robot is on the map. If there is a slight deviation then it should be able to account for that but if the given pose is completely off then it will lead to behaviour like this.

I would recommend generating a new map where the robot starts on a dock. The direction the robot is facing would then be north with regards to the map, and it would be starting at point (0.0, 0.0). This way you can get repeatable results when you run nav_to_pose. Otherwise, I would recommend using the publish point tool to find the starting point of the robot on your current map, and set the starting angle as close as possible to the real world orientation.

[nidia-bucarelli]

My robot starts on the dock station. This time when trying to generate a new map, the start point does not match my physical location. It is at the edge of the room (if not outside). See the images below. The physical location of the robot matches the robot's location (and orientation) in the virtual map. But, the start point in the map shows an area that is below a desk (and at the edge of the room, if not outside my room).

I am not sure if I understand your second suggestion. I can locate in the physical world where the virtual start point is. Are you suggesting just moving the robot manually to below the desk?

[roni-kreinin]

My second suggestion was to figure out the starting position of the robot on the map, given that it is not (0,0). By using the publish point tool and echoing the /clicked_point topic, you should be able to locate that starting position. Then, use this position as the initial pose in the nav_to_pose script.