Make it work with Gazebo

[OdedHorowits]

Originally posted by @suchetanrs in https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker/issues/2#issuecomment-2009664893

Also, cmake should be able to find it since we do make install in the dockerfile itself. Did your image get built correctly?

Hello, regarding that, the following line in your Dockerfile builds ORB_SLAM and its deps. according to the build.sh file - RUN . /opt/ros/humble/setup.sh && cd /home/orb/ORB_SLAM3 && mkdir build && ./build.sh Whenever it reaches a make install I get make: *** No rule to make target 'install'. Stop. And indeed, there is no rule for make install.

It that a normal procedure?

[suchetanrs]

Hi, thanks for your ticket. The https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker/issues/2#issuecomment-2009664893 is for the Pangolin dependency. make install is necessary in this case. For the error you faced in build.sh, yes, you are right. make install is not needed for the DBoW2 and g2o dependencies. The shared library files for these dependencies are located with a hard-coded path here. You can ignore the make: *** No rule to make target 'install'. Stop. for these two dependencies for now. I will update it in a future commit.

Please feel free to close the issue in case you do not have any further questions.

[OdedHorowits]

Hi, Thanks for the response. Actually I do have another question. I had built your docker images and added gazebo and aerostack2. For some reason I cannot connect the camera of the gazebo drone to the orb-slam gui. I made a bridge between the gazebo topic where the camera broadcasts the image msg to the topic that the orb-slam gui expects, but all I get there is 'waiting for an image'. How do I make the connection?

Thank you

[suchetanrs]

Hi, I will need more information to help with a solution. Did you try echoing the topic and checking if messages are being published? I just checked the launch file, if you used the unirobot.launch.py the robot_namespace is hard-coded to scout_2 here. So the wrapper will expect the images to be published at /scout_2/camera/image_raw and /scout_2/camera/depth/image_raw. You can change the robot_namespace to the desired namespace in the launch file. This might be the issue.

Try checking with ros2 node info the topic that the wrapper is subscribing to and the topic you are publishing to. If this is not the issue, let me know.

Alternatively, if you do not want the hard-coded robot namespace and run the wrapper without a namespace, you can do the following: export ROBOT_NAMESPACE="" && export ROBOT_Y="1.0" && export ROBOT_X="1.0" This will set the required environment variables. Post this, you can start the wrapper using rgbd.launch.py instead of unirobot.launch.py

[OdedHorowits]

Hi So, at unirobot.launch.py I did change the robot namespace to 'drone0', as the default of aerostack2. The camera indeed sends data, I confirm that by using rviz, I can see the image stream there (the gray box above the drone represents the camera):

Typing ros2 topic list gives me:

/clicked_point
/clock
/drone0/actuator_command/pose
/drone0/actuator_command/thrust
/drone0/actuator_command/trajectory
/drone0/actuator_command/twist
/drone0/alert_event
/drone0/controller/info
/drone0/ground_truth/pose
/drone0/ground_truth/twist
/drone0/motion_reference/pose
/drone0/motion_reference/trajectory
/drone0/motion_reference/twist
/drone0/platform/info
/drone0/self_localization/pose
/drone0/self_localization/twist
/drone0/sensor_measurements/air_pressure
/drone0/sensor_measurements/battery
/drone0/sensor_measurements/gps
/drone0/sensor_measurements/imu
/drone0/sensor_measurements/magnetic_field
/drone0/sensor_measurements/rgbd_camera/camera_info
/drone0/sensor_measurements/rgbd_camera/depth
/drone0/sensor_measurements/rgbd_camera/image_raw
/drone0/sensor_measurements/rgbd_camera/points
/goal_pose
/gz/drone0/arm
/gz/drone0/cmd_vel
/initialpose
/parameter_events
/rosout
/tf
/tf_static

So, the camera publishes to /drone0/sensor_measurements/rgbd_camera/image_raw and rviz listen to that topic. Launching the orb-slam wrapper using ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py I get the following additional ros2 topics:

/drone0/imu
/drone0/map_data
/drone0/map_points
/drone0/odom

You probably ask 'where are drone0/camera/image_raw and drone0/camera/depth', so it used to be presented and I tried to make a bridge-node that subscribes to /drone0/sensor_measurements/rgbd_camera/image_raw and publishes to drone0/camera/image_raw but it didn't do the trick, so I changed it (and recompiled of course) at /root/colcon_ws/src/orb_slam3_ros2_wrapper/src/rgbd/rgbd-slam-node.cpp to be:

namespace ORB_SLAM3_Wrapper
{
    RgbdSlamNode::RgbdSlamNode(const std::string &strVocFile,
                               const std::string &strSettingsFile,
                               ORB_SLAM3::System::eSensor sensor)
        : Node("ORB_SLAM3_RGBD_ROS2")
    {
        // ROS Subscribers
        rgbSub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, "sensor_measurements/rgbd_camera/image_raw");
        depthSub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, "sensor_measurements/rgbd_camera/depth");
...

So now, when I type ros2 topic info /drone0/sensor_measurements/rgbd_camera/image_raw I get:

Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 2

Closing rviz, I get:

Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 1

Closing orb-slam GUIs, I get:

Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 0

I.e. the orb-slam wrapper is subscribing to that topic. Nevertheless, its viewer shows nothing.

I attach also the output from the orb-slam launching terminal, maybe there is a problem you can see there:

Regarding ros2 node ___. Launching orb_slam wrapper only gives:

root@Ubuntu:~# ros2 node list
/drone0/ORB_SLAM3_RGBD_ROS2
/drone0/transform_listener_impl_5809caf31440

root@Ubuntu:~# ros2 node info /drone0/ORB_SLAM3_RGBD_ROS2
/drone0/ORB_SLAM3_RGBD_ROS2
  Subscribers:
    /drone0/imu: sensor_msgs/msg/Imu
    /drone0/odom: nav_msgs/msg/Odometry
    /drone0/sensor_measurements/rgbd_camera/depth: sensor_msgs/msg/Image
    /drone0/sensor_measurements/rgbd_camera/image_raw: sensor_msgs/msg/Image
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /drone0/map_data: slam_msgs/msg/MapData
    /drone0/map_points: sensor_msgs/msg/PointCloud2
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /drone0/ORB_SLAM3_RGBD_ROS2/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /drone0/ORB_SLAM3_RGBD_ROS2/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /drone0/ORB_SLAM3_RGBD_ROS2/get_parameters: rcl_interfaces/srv/GetParameters
    /drone0/ORB_SLAM3_RGBD_ROS2/list_parameters: rcl_interfaces/srv/ListParameters
    /drone0/ORB_SLAM3_RGBD_ROS2/set_parameters: rcl_interfaces/srv/SetParameters
    /drone0/ORB_SLAM3_RGBD_ROS2/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
    /drone0/orb_slam3_get_map_data: slam_msgs/srv/GetMap
  Service Clients:

  Action Servers:

  Action Clients:

I.e. it subscribes to /drone0/sensor_measurements/rgbd_camera/image_raw! Launching Gazebo and aerostack2, I get more nodes of course:

root@Ubuntu:/workspace# ros2 node list
/drone0/FollowPathBehavior
/drone0/GoToBehavior
/drone0/LandBehavior
/drone0/ORB_SLAM3_RGBD_ROS2
/drone0/TakeoffBehavior
/drone0/alphanumeric_viewer
/drone0/controller_manager
/drone0/ground_truth_bridge
/drone0/platform
/drone0/ros_gz_bridge
/drone0/state_estimator
/drone0/transform_listener_impl_56ed22410ec0
/drone0/transform_listener_impl_5809caf31440
/drone0/transform_listener_impl_589652b44710
/drone0/transform_listener_impl_5c3e028150c0
/drone0/transform_listener_impl_5c3e02881770
/drone0/transform_listener_impl_5d4aeda9b460
/drone0/transform_listener_impl_5dee0a7761d0
/world/ros_gz_bridge

root@Ubuntu:/workspace# ros2 node info /drone0/ros_gz_bridge
/drone0/ros_gz_bridge
  Subscribers:
    /gz/drone0/arm: std_msgs/msg/Bool
    /gz/drone0/cmd_vel: geometry_msgs/msg/Twist
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /drone0/sensor_measurements/air_pressure: sensor_msgs/msg/FluidPressure
    /drone0/sensor_measurements/battery: sensor_msgs/msg/BatteryState
    /drone0/sensor_measurements/imu: sensor_msgs/msg/Imu
    /drone0/sensor_measurements/magnetic_field: sensor_msgs/msg/MagneticField
    /drone0/sensor_measurements/rgbd_camera/camera_info: sensor_msgs/msg/CameraInfo
    /drone0/sensor_measurements/rgbd_camera/depth: sensor_msgs/msg/Image
    /drone0/sensor_measurements/rgbd_camera/image_raw: sensor_msgs/msg/Image
    /drone0/sensor_measurements/rgbd_camera/points: sensor_msgs/msg/PointCloud2
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /drone0/ros_gz_bridge/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /drone0/ros_gz_bridge/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /drone0/ros_gz_bridge/get_parameters: rcl_interfaces/srv/GetParameters
    /drone0/ros_gz_bridge/list_parameters: rcl_interfaces/srv/ListParameters
    /drone0/ros_gz_bridge/set_parameters: rcl_interfaces/srv/SetParameters
    /drone0/ros_gz_bridge/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

[GLtangzero]

I have the same problem, I can't get the image, even I give both the RGB and depth iamge. Check this rqt image. ![Uploading Screenshot from 2024-06-21 20-11-38.png…]()

[suchetanrs]

Hi, @OdedHorowits and @GLtangzero I am really sorry for a late reply to your comment. I was really caught up with some work. Thanks a lot for the very very detailed explanation. It helped me quite a bit to track the bug in the code. I was able to recreate this. Turns out the code would not work if no odometry is provided on the /odom topic. This obviously is quite miserable since it should have the capability to publish the TF between map and base_link nevertheless. This is also why I think it showed waiting for images eventhough it had an active subscription.

Well, for now, I have tried to add a no_odometry_mode in the rgbd-ros-params.yaml file. It's false by default. If you enable this, it will bypass the odometry subscription and start tracking while publishing the transform between map and base_link.

Do a git pull or a clean clone of the wrapper respository and try running it again after following the steps. It could very well be that your issue might be entirely different. Let me know.

I also found some time to upload the simulator I used. It's here https://github.com/suchetanrs/scout-husky-gazebo-ros2 . The readme contains the steps to use the simulator and I have updated the current repository's readme with a few steps. These both should work well out of the box. I'd recommend you try it with the UGV simulator as well just so we are sure that it's atleast running :p

[OdedHorowits]

Thank you very much for your efforts. Plz tell me,what is the rule of the configuration parameter ros_visualization: false in orb_slam3_ros2_wrapper/params/rgbd-ros-params.yaml?

[suchetanrs]

This parameter is used to provide of tracked points etc via RViz in 3D coordinates. Note that the TF will be published regardless of this parameter's value. This is only for visualization purposes. Also, this is unstable and I have not been able to test this fully for now. I have updated the readme with other parameter descriptions if you wish to have a look.

[GLtangzero]

@suchetanrs Hello, thanks for your replay, I think it's better than before but there is still some problems.

First of all, actually I'm not use the project in the docker, I was moved it to a new work space, but it's almost same condition, the work space is also from zang9/ORB_SLAM3_ROS2, because I need to connect the project with the gazebo garden and there are some other ros2 project need to combine.

For now, if we still use the order of ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py, it's still just show waiting for image. But if we try zang9's order like "ros2 run orbslam3_ros2 rgbd /home/tang/colcon_ws/src/orbslam3_ros2/vocabulary/ORBvoc.txt /home/tang/colcon_ws/src/orbslam3_ros2/src/rgbd/TUM1.yaml" there is image in the orbslam3 but there are some problems happened even the image could be seen,:

1. I think we not load the document "rgbd-ros-params.yaml",
2. There are not any point show in the ORBSLAM3 even the point have been catch in the iamge.
3. There are some warning in the terminal (1)if there are images: it will repeat like "Error: TF_SELF_TRANSFORM: Ignoring transform from authority "Authority undetectable" with frame_id and child_frame_id "" because they are the same at line 209 in ./src/buffer_core.cpp Error: TF_NO_CHILD_FRAME_ID: Ignoring transform from authority "Authority undetectable" because child_frame_id not set at line 217 in ./src/buffer_core.cpp Error: TF_NO_FRAME_ID: Ignoring transform with child_frame_id "" from authority "Authority undetectable" because frame_id not set at line 224 in ./src/buffer_core.cpp" (2)if the camera not catch the image: it will repeat: "ORB-SLAM failed: Not initialized."

the rqt image will show like this:

Thanks for your work, I think we could make it work, now I will try test the job in the docker, but I really don't have the experience to use this tool.

[GLtangzero]

@suchetanrs Hello, I just check it, the work which you upload is very good, it is work successfully in the docker both the wrapper and the simulation, I will try to moved it without docker, or build the new project in the docker. And could I ask the rviz2 progress which you show the SLAM? I don't know how to check this image:

There are some warning in the below: In the gazebo-husky-gazebo-ros2 after running :"ros2 launch scout_gazebo start_world.launch.py" [spawner-6] [INFO] [1719200308.333893647] [scout_2.diff_drive_controller_spawner]: Configured and activated diff_drive_controller [spawner-5] [INFO] [1719200308.436446472] [scout_2.joint_state_broadcaster_spawner]: Configured and activated joint_state_broadcaster [INFO] [spawner-6]: process has finished cleanly [pid 8868] [INFO] [spawner-5]: process has finished cleanly [pid 8866]

[navsat_transform_node-8] [INFO] [1719200309.107280808] [scout_2.navsat_transform]: Datum UTM coordinate is (31T, 376764.25, 4824746.89) [navsat_transform_node-8] [INFO] [1719200309.107359742] [scout_2.navsat_transform]: Corrected for magnetic declination of -0.0274017, user-specified offset of 0 and meridian convergence of -0.0183578. Transform heading factor is now -0.138116 [INFO] [SetDatum "{geo_pose: {position: {latitude: 43.5655, longitude: 1.4740, altitude: 149.34}, orientation: {x: 0.079, y: -0.043, z: -0.041, w: 0.959}}}"-10]: process has finished cleanly [pid 8876] [ekf_node-9] Error: TF_SELF_TRANSFORM: Ignoring transform from authority "Authority undetectable" with frame_id and child_frame_id "" because they are the same [ekf_node-9] at line 209 in ./src/buffer_core.cpp [ekf_node-9] Error: TF_NO_CHILD_FRAME_ID: Ignoring transform from authority "Authority undetectable" because child_frame_id not set [ekf_node-9] at line 217 in ./src/buffer_core.cpp [ekf_node-9] Error: TF_NO_FRAME_ID: Ignoring transform with child_frame_id "" from authority "Authority undetectable" because frame_id not set [ekf_node-9] at line 224 in ./src/buffer_core.cpp [navsat_transform_node-8] Error: TF_SELF_TRANSFORM: Ignoring transform from authority "Authority undetectable" with frame_id and child_frame_id "" because they are the same [navsat_transform_node-8] at line 209 in ./src/buffer_core.cpp [navsat_transform_node-8] Error: TF_NO_CHILD_FRAME_ID: Ignoring transform from authority "Authority undetectable" because child_frame_id not set [navsat_transform_node-8] at line 217 in ./src/buffer_core.cpp [navsat_transform_node-8] Error: TF_NO_FRAME_ID: Ignoring transform with child_frame_id "" from authority "Authority undetectable" because frame_id not set [navsat_transform_node-8] at line 224 in ./src/buffer_core.cpp [rviz2-7] Error: TF_SELF_TRANSFORM: Ignoring transform from authority "Authority undetectable" with frame_id and child_frame_id "" because they are the same [rviz2-7] at line 209 in ./src/buffer_core.cpp [rviz2-7] Error: TF_NO_CHILD_FRAME_ID: Ignoring transform from authority "Authority undetectable" because child_frame_id not set [rviz2-7] at line 217 in ./src/buffer_core.cpp [rviz2-7] Error: TF_NO_FRAME_ID: Ignoring transform with child_frame_id "" from authority "Authority undetectable" because frame_id not set [rviz2-7] at line 224 in ./src/buffer_core.cpp

Thanks for your job again, it's really a amazing good project!

[suchetanrs]

Hey @GLtangzero Thanks a lot for such a detailed explanation. Please move these two comments to a new issue. Let's try to resolve stuff there. @OdedHorowits is already booked for this one :p I believe it should work without docker if it compiles. May need some effort.

[OdedHorowits]

Well, for now, I have tried to add a no_odometry_mode in the rgbd-ros-params.yaml file. It's false by default. If you enable this, it will bypass the odometry subscription and start tracking while publishing the transform between map and base_link.

Works.

[OdedHorowits]

@suchetanrs just wanted to ask - how did you find the bug with the /odom topic?

[suchetanrs]

I tried to recreate your issue description and realised that not everyone might have a node that publishes the odometry / imu data. Hence found it. On a side note, did you try to make it work? Any issues? :p

[OdedHorowits]

Hi yes it works, thank you very much. Another way to solve that is to set odomSub_ to subscribe to ground_truth in rgbd-slam-node.cpp:

rgbd-slam-node.cpp 
/**
 * @file rgbd-slam-node.cpp
 * @brief Implementation of the RgbdSlamNode Wrapper class.
 * @author Suchetan R S (rssuchetan@gmail.com)
 */
#include "rgbd-slam-node.hpp"

#include <opencv2/core/core.hpp>

namespace ORB_SLAM3_Wrapper
{
    RgbdSlamNode::RgbdSlamNode(const std::string &strVocFile,
                               const std::string &strSettingsFile,
                               ORB_SLAM3::System::eSensor sensor)
        : Node("ORB_SLAM3_RGBD_ROS2")
    {
        // ROS Subscribers
        rgbSub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, "sensor_measurements/rgbd_camera/image_raw");
        depthSub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(this, "sensor_measurements/rgbd_camera/depth");
        syncApproximate_ = std::make_shared<message_filters::Synchronizer<approximate_sync_policy>>(approximate_sync_policy(10), *rgbSub_, *depthSub_);
        syncApproximate_->registerCallback(&RgbdSlamNode::RGBDCallback, this);
        imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>("sensor_measurements/imu", 1000, std::bind(&RgbdSlamNode::ImuCallback, this, std::placeholders::_1));
        odomSub_ = this->create_subscription<nav_msgs::msg::Odometry>("ground_truth", 1000, std::bind(&RgbdSlamNode::OdomCallback, this, std::placeholders::_1));

[suchetanrs]

Great!