Best posts made by zauberflote1

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

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

Hello,
If you are developing your own custom pipelines, applications, and modifying existing source code from ModalAI, this may be applicable to you.

Motivation: Control which CPU core is responsible for a certain thread in your custom application.

Background: It all started when I was observing the htop behavior. Sometimes the dynamic core allocation, overloads a particular core instead of keeping the effort evenly distributed, potentially causing issues if you have a "real-time" sensitive application. -- Assuming you have already set the priority and scheduler properly...

Tackling the issue: If you look in the qvio-server source files, you will find that a particular computer vision thread is locked to CPU core 7 -- known to be the fastest VOXL2 core. Then, what if we use the same idea in a custom application (potentially for a different core), would it work? YES.

How to do it:
PS1: I rather use ROS over modalpipes, so the full example linked is a for a modified IMU server that controls the driver entirely via ROS, but you can do the same for a modalpipe implementation WLOG.

Using the heavenly chosen language, C++, and ROS syntax

void _set_affinity_to_core_7() {
    cpu_set_t cpuset;
    pthread_t thread = pthread_self();

    //SET IT UP TO CORE 7
    CPU_ZERO(&cpuset);
//change this to the core you want
    CPU_SET(7, &cpuset); 
    if (pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset)) {
        perror("pthread_setaffinity_np");
    }

    //CHECK IF IT WAS SET UP
    if (pthread_getaffinity_np(thread, sizeof(cpu_set_t), &cpuset)) {
        perror("pthread_getaffinity_np");
    }

    ROS_INFO("IMU read thread is now locked to core: ");
    for (int j = 0; j < CPU_SETSIZE; j++) {
        if (CPU_ISSET(j, &cpuset)) {
            ROS_INFO(" %d", j);
        }
    }
    ROS_INFO("\n");
}

In the main() and using boost, define the lambda as follows:

    std::vector<boost::thread> threads;
    for (int i = 0; i < 1; ++i) {
        if (imu_enable[i]) {
            //IF YOU DONT WANT TO LOCK A SPECIFIC CORE, THEN 
            // threads.push_back(boost::thread(boost::bind(&readThreadFunc, i)));

            threads.push_back(boost::thread([i]() {
            _set_affinity_to_core_7();  
            readThreadFunc(i);
            }));

        }
    }

PS2: Full Example can be found here:
https://github.com/zauberflote1/voxlSyncVINS/blob/91d74b57cacd78db87c6bd48ac03f14ca36874ab/imu_ros/src/voxl_imu_ros/server/src/node.cpp#L43C32-L43C33

Final Remarks:
Only do this if you know what you are doing as you may overload a core by accident.

All the best,
ZBFT