voxl-open-vins-server How to Use, Overall Questions, ROS/ROS2 Findings

zauberflote1

Hello ModalAI and Community,

I have been trying to integrate a moded version of Open Vins and my Sentinel Drone for the past week through ROS or ROS2; however, I was met with extreme difficulties during calibration when inspecting the ROS bags, which I believe are due to the mpa-to-ros and mpa-to-ros2 logic. (For the curious ones, I'll write the code segment below and briefly discuss why this "breaks" the bag. If you're not a developer, this may not be useful. In that case, just scroll down)

mpa-to-ros2
Timestamps (stereo_interface.cpp example)

imgL.header.stamp.nanosec = meta.timestamp_ns;

It is not ideal to use monotonic clock time here due to the jumps. There's a need to apply a similar solution to the one used for the mpa-to-ros node. Also, one should "repeat" the logic done for the IMU_Server to fix the timestamps, i.e., discard messages if, after the conversion to ROS2 time, timestamp<timestamp prev. (Go horse here is to timestamp with the current ROS2 time, but your VINS will not like that...) Note that the information for stereo images comes with the two images already in the same "meta," so there's no need to repeat the logic done in the camera_server for time-sync the cameras. Note 2: You'll need to convert time in all interfaces.

Interface (interface_manager.cpp)
(This might be intended behavior, but it definitely is not suitable for VINS...)

If you preview the ROS2 bag, you'll quickly notice that stereo images are combined into a single image. Depending on your application, this may be good or bad, but if you're trying to calibrate your sensors for optimal accuracy using Kalibr, this won't make the cut + most open-source codes consider two image topics. Below is how I fixed this issue...

In all_interfaces.h

#include "voxl_mpa_to_ros2/interfaces/stereo_interface.h"

in interface_manager.cpp
copy a similar logic done in mpa_to_ros, in the function findPipes, if curType == INT_CAMERA in the switch, add an if statement for covering the case newNode->name is equal to "stereo," then newNode->interface = new StereoInterface(nh, newNode->name);

case INT_CAMERA:
			if(strstr(newNode->name, "stereo")){ // can just use name var because it's already copied but for explicit understanding 
			newNode->interface = new StereoInterface(nh, newNode->name);
			} else {
			       newNode->interface = new CameraInterface(nh, newNode->name);
			}
		break;

There are some extra steps needed for the perfect break, including at the camera_server level, because each image timestamp should be different as per the camera_server logic. This is actually important for calibration proposes.

Subscriber count and ThreadManageInterfaces
IMO, this is why users can't see ROS2 topics on their host computer. The subscriber count is suitable for constrained resource environments and for someone who doesn't need messages apart from logging, but this is currently a problem if you plan on testing an algorithm for navigation, control, or guidance.

ThreadManageInterfaces sleeps for 1s every time it is invoked, and if the subscriber count misses your subscriber during a dt between messages, you'll get a jump in timestamps. Another consequence of this implementation often happens if you try to echo image topics in a host computer, which results in being unable to receive them. Note that you can still receive smaller messages, like IMU. Ideally, there's probably a way to fix it using the subscriber count, but a quick and dirty solution is constantly publishing the topics you'll need. This will save you time and keep the sampling frequency somewhat constant. Hopefully, ModalAI will be able to access this in the future.

Please note that there may be other reasons why image topics are not being received in your host computer, ranging from QOS settings, env setup, publisher rate, etc.

(In short, if you create a simple talker ROS2 node on VOXL2 and use a listener node on your host PC, and can successfully receive messages, your problem is likely partially due to what I described)

mpa-to-ros
Timestamps
One needs to apply the same approach as for ROS2 apart from time conversion. Also, note that the message construction in one of the latest commits forces the image on the left to have the same timestamp as the image on the right. As I mentioned before, this is incorrect behavior from the standpoint of VINS, as one can use calibration tools to find the average offset rather than artificially forcing them to be the same.

Subscriber Count and Interface
Here, the problem is not as bad as in ROS2, but I recommend taking the same approach with regard to constantly publishing the topics you need. Potentially create args or a launch file to pass the values of the topics you need.

The above steps are a simplified approach to "fix" the issues I described. If you have a better solution or another alternative approach, please share. I hope we all can gain from this.

voxl-open-vins-server

When I updated to the latest SDK, I noticed the server came with. This was a pleasant surprise as I was going through this battle to implement a similar architecture. I noticed that the calibration files it uses are the openCV ones, but I couldn't find the IMU calibration files. In any case, I would like to know where I can find the source code for the voxl-open-vins-server so I can start modifying it to leverage both the MPA and external config files like Kalibr yamls.

My apologies if this post was too extensive

--ZBFT

Moderator

@zauberflote1 Hi, that open-vins package is in early alpha stages. So, it's completely undocumented and we aren't supporting it at this point. We'll look into your question though and see if we can find something

zauberflote1

@Moderator Thanks,
a Gitlab repo link would suffice in case documentation is not available.

Cliff Wong

Hi there, yes, unfortunately the repo for OpenVINS is still under construction as it's under active development by the ModalAI team. We'll have it available soon

voxl-open-vins-server does a similar process to the ROS implementation when using camera data--in order to sync frames, data comes in a stereo-type packet. During camera calibration, instead of Kalibr, we are using our own voxl-calibrate-camera implementation that comes with the SDK. And as needed, one can turn off the stereo output in voxl-camera-server and calibrate each camera individually. That would be best on the ROS side as well where you can use split the images from camera-server like this before publishing to Kalibr (where 2 topics work best for openvins), then calibrate independently.

You could try voxl-calibrate-camera instead and yes it will output opencv-format intrinsics where you can copy the needed values into the ROS launch file. We create extrinsics using mechanical design. In both cases, OpenVINS will refine them at runtime during the first few seconds of operation.

You can upgrade to the latest dev version via editing your /etc/apt/sources.list.d/modalai.list on voxl2 to point to the dev repo. Then run apt update && apt upgrade voxl-open-vins-server. Looking at the /etc/modalai/voxl-open-server.conf may give you some ideas on how the roslaunch file can be setup as well.

zauberflote1

@Cliff-Wong Hey Cliff,
Thanks for your reply! I have solved the issue. It's a bit of a long solve, but I'll outline the main steps.
For proper calibration IMU-Cam calibration, we cannot have a burst of measurements like we have when running the IMU in FIFO mode. Therefore, the best way to get around this is to use IMU in basic read mode. Also, I was able to achieve good VINS results running the IMU at 500Hz and 200Hz. I opted to remodel the IMU server to have it "fully" in ROS without the need for MPA, while maintaining the FIFO scheduler for "RT". This decision reduced the sampling jitter by a lot (about 50%). Still, jitter is present, while looking at the core load distribution, I noticed that sometimes all the processes get allocated to core 7 until it bounces back to another favorable core distribution, creating a tiny lag on the system and, consequently, causing the jitter -- of course, this is a hypothesis. I was wondering if you had faced the same situation and if you or your teammates have thought of the possibility of restricting specific cores for real-time operations, which would avoid this weird allocation.
Here are the results:

Best,
--ZBFT

zauberflote1

@Cliff-Wong
Image Description:
1)BsicRead Server Using MPA-ROS
2)Classic Server and MPA-ROS
3)IMU ROS ~500Hz
4)IMU ROS ~200Hz

zauberflote1

@zauberflote1
Forgot to mention that a way to reduce jitter is also by checking if the interrupt DTA Rdy is true before reading, avoiding reading past info from the registers.
Sorry for the many messages

tiralonghipol

@zauberflote1 hei! we are facing a similar issue, do you have a link for your implementation? thanks!

zauberflote1

@tiralonghipol and community
Here it is,
https://github.com/zauberflote1/voxlSyncVINS

tiralonghipol

@zauberflote1 amazing, thank you

coreyazion

@zauberflote1 said in voxl-open-vins-server How to Use, Overall Questions, ROS/ROS2 Findings:

decision reduced the sampling jitter b

Thank you for sharing your insights and great work!