Hello,
I am curious about the math behind the "fusion" of the fixed pose coming from the apriltags. From what I understood reading pose_filter.c and geometry.c, the filter uses a ring buffer and finds the average of the values buffered as the new correct values of x, y, z, and yaw. Could anyone confirm this?
zauberflote1
@zauberflote1
I like drones, VINS, C++, and Lie Theory. The rest is history...
Latest posts made by zauberflote1
-
Math Behind Fusion of FIXED POSE (AprilTag Pose)
-
RE: voxl-open-vins-server How to Use, Overall Questions, ROS/ROS2 Findings
@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 -
RE: voxl-open-vins-server How to Use, Overall Questions, ROS/ROS2 Findings
@Cliff-Wong
Image Description:
1)BsicRead Server Using MPA-ROS
2)Classic Server and MPA-ROS
3)IMU ROS ~500Hz
4)IMU ROS ~200Hz -
RE: voxl-open-vins-server How to Use, Overall Questions, ROS/ROS2 Findings
@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 -
RE: voxl-open-vins-server How to Use, Overall Questions, ROS/ROS2 Findings
@Moderator Thanks,
a Gitlab repo link would suffice in case documentation is not available. -
voxl-open-vins-server How to Use, Overall Questions, ROS/ROS2 Findings
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
-
RE: ROS2 topic data not visible on remote computer (VOXL2, SDK 1.1)
FYI, you won't be able to see some of the VOXL2 ROS2 "native" raw topics if the xrce-dds server is active... (voxl-microdds-agent) -- of course, if you don't plan on fiddling with the internal pipeline.
Another interesting thing is that even if you "Stop" voxl-microdds-agent (i.e., systemctl stop voxl-microdds-agent) and try to use a common ROS ID on a "ground station" and VOXL2 to process raw camera and imu topics, the overhead is significant. From my experience, this happens with fast rate and/or large messages. In practicality, you can overcome this using the streamer. Still, if you precisely need "real-time" data in ROS2 format and cannot deploy a docker or another ROS2 node inside the VOXL2, it's easier to set your VOXL2 configuration to a minimal CPU usage and/or throttle down the topic messages. For instance, a simple solution for images is to reduce the resolution.
Finally, the way the mpa-to-ros2 logic works, the topic messages are only published when there's a subscriber (subscriber_count >0),
EDIT: You can change the voxl-microdds-agent source files or carefully open another xrce-dds bridge, so you have one server that communicates with your ground station (or companion computer) and another that communicates with the PX4 inside the VOXL2 -- a bit tricky but possible...
--I hope this helps
ZBFT