Navigation goals are not accepted - unable to navigate the robot

[rayvburn]

Hello @renan028,

first of all, thank you for sharing your work!
I'm running a clean build of Ubuntu 20 with ROS2 Foxy. I've cloned your repository and first thing that I've encountered is compilation problem:

Click for details

```console Finished <<< webots_ros2 [1.00s] --- stderr: tiago_webots_ros2 /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/src/robot_task.cc:68:6: error: no declaration matches ‘void tiago_webots_ros::RobotTask::enableGPS()’ 68 | void RobotTask::enableGPS() { | ^~~~~~~~~ /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/src/robot_task.cc:68:6: note: no functions named ‘void tiago_webots_ros::RobotTask::enableGPS()’ In file included from /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/src/robot_task.cc:1: /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/include/tiago_webots_ros2/robot_task.h:38:7: note: ‘class tiago_webots_ros::RobotTask’ defined here 38 | class RobotTask : public rclcpp::Node { | ^~~~~~~~~ /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/src/robot_task.cc:96:6: error: no declaration matches ‘void tiago_webots_ros::RobotTask::updateGPS(geometry_msgs::msg::PointStamped_ >::SharedPtr)’ 96 | void RobotTask::updateGPS(const geometry_msgs::msg::PointStamped::SharedPtr gps) { | ^~~~~~~~~ /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/src/robot_task.cc:96:6: note: no functions named ‘void tiago_webots_ros::RobotTask::updateGPS(geometry_msgs::msg::PointStamped_ >::SharedPtr)’ In file included from /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/src/robot_task.cc:1: /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/include/tiago_webots_ros2/robot_task.h:38:7: note: ‘class tiago_webots_ros::RobotTask’ defined here 38 | class RobotTask : public rclcpp::Node { | ^~~~~~~~~ /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/src/robot_task.cc: In member function ‘void tiago_webots_ros::RobotTask::enableDevices()’: /home/user/ros2_workspace/tiago_webots_ws/src/tiago_webots_ros2/src/robot_task.cc:105:3: error: ‘enableGPS’ was not declared in this scope 105 | enableGPS(); | ^~~~~~~~~ make[2]: *** [CMakeFiles/tiago_webots_ros2.dir/build.make:63: CMakeFiles/tiago_webots_ros2.dir/src/robot_task.cc.o] Error 1 make[1]: *** [CMakeFiles/Makefile2:107: CMakeFiles/tiago_webots_ros2.dir/all] Error 2 make: *** [Makefile:141: all] Error 2 ```

I build with

colcon build --symlink-install

I've resolved the compilation problem as in this commit. Next, I've rebuilt the workspace and launched:

ros2 launch tiago_webots_ros2 tiago.launch.py

Robot spawns correctly as Webots view seems to be OK, but in Rviz2 costmaps flash once (see below) and disappear.

I can make it appear again if I re-enable the Global Planner and Controller visualization tabs in Rviz2. I noticed that local costmap (similarly to global costmap) uses static_map layer (see screen below).

• Global Costmap's color scheme: costmap, alpha 0.5,
• Local Costmap's color scheme: raw, alpha 0.7

Then, if I try to send a navigation goal, I see this kind of output in the console:

[rviz2-15] Start navigation
[bt_navigator-12] [INFO] [1626124138.768281506] [bt_navigator]: Begin navigating from current location to (0.97, 0.22)
[controller_server-9] [INFO] [1626124138.830222617] [controller_server]: Received a goal, begin computing control effort.
[ERROR] [controller_server-9]: process has died [pid 205580, exit code -11, cmd '/opt/ros/foxy/lib/nav2_controller/controller_server --ros-args -r __node:=controller_server --params-file /home/user/ros2_workspace/tiago_webots_ws/install/tiago_webots_ros2/share/tiago_webots_ros2/config/controller_server_params.yaml'].
[bt_navigator-12] [ERROR] [1626124139.890623033] [bt_navigator]: Action server failed while executing action callback: "send_goal failed"
[bt_navigator-12] [WARN] [1626124139.890667783] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[rviz2-15] [INFO] [1626124152.032647716] [rviz2]: Trying to create a map of size 1984 x 1984 using 1 swatches

I wonder:

• why local costmap uses static_map layer?
• what causes navigation to always fail?

Did you encounter similar problems?

As you might see, I've restructurized your package and I was able to navigate robot with a global costmap. The problem is that I cannot make local costmap working despite proper publishing of LaserScans and TransformFrames. I've tried to launch Turtlebot's navigation with parameters that you've developed and it works OK. I've checked topic names and frame IDs multiple times. I've even changed laser scanner model to RobotisLds01, but no luck - always only global costmap works.

[renan028]

hi @rayvburn , thank you! It is good to know it helped.

When I created this package, webots_ros_2 API was Python only, thus I have started the webots_ros_2 C++ API implementation (https://github.com/renan028/webots_ros2). Webots' maintainers merged my branch in webots_ros_2 master, but there are some differences/improvements.

I have created this package as a first attempt to integrate webots_ros_2 (C++ version) with the navigation stack. And I stopped working on it (and maintain it) in March 2021 (when it worked). If I remember correctly, tiago_webots_ros2 pkg was working with my version. I do not recommend using mine though, but change the current pkg to work with webots_ros2 latest.

I will revisit the code and let you know. Please let me know if you manage to make it work also.

[rayvburn]

I didn't find a solution to the issues related to your package so far.

Nevertheless, I've been trying to figure out what causes local costmap to not show up in my version of the TIAGo Webots-ROS2 interface.
It turned out that it was such a silly thing like laser scanner frame placed at -0.004 height. I thought that base_link frame is published with a z component according to robot's .proto file, but it is at the same height as odom frame (which is at z = 0), see WebotsDifferentialDriveNode:

# Pack & publish odometry
msg = Odometry()
msg.header.stamp = stamp
msg.header.frame_id = self._odometry_frame
msg.child_frame_id = self._robot_base_frame
msg.twist.twist.linear.x = v
msg.twist.twist.angular.z = omega
msg.pose.pose.position.x = position[0]
msg.pose.pose.position.y = position[1]
msg.pose.pose.orientation.z = sin(angle / 2)
msg.pose.pose.orientation.w = cos(angle / 2)
self._odometry_publisher.publish(msg)

I shifted the laser frame upwards and now the package is fully working with newest Webots (from apt) and ROS2 Foxy. So if somebody encounters the issues from the first post, I would recommend to use my version of TIAGo Webots-ROS2 interface.

One extra question
Could you explain, how did you manage to generate such an accurate map as intralogistics.pgm? Was it made with some external tool? I don't expect SLAM to be that accurate.