ModalAI Forum
    • Categories
    • Recent
    • Tags
    • Popular
    • Users
    • Groups
    • Register
    • Login

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

    Ask your questions right here!
    5
    11
    1061
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • zauberflote1Z
      zauberflote1 ModalAI Team
      last edited by

      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

      ModeratorM 1 Reply Last reply Reply Quote 1
      • ModeratorM
        Moderator ModalAI Team @zauberflote1
        last edited by 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

        zauberflote1Z 1 Reply Last reply Reply Quote 0
        • zauberflote1Z
          zauberflote1 ModalAI Team @Moderator
          last edited by

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

          1 Reply Last reply Reply Quote 0
          • Cliff WongC
            Cliff Wong ModalAI Team
            last edited by

            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.

            zauberflote1Z 2 Replies Last reply Reply Quote 0
            • zauberflote1Z
              zauberflote1 ModalAI Team @Cliff Wong
              last edited by

              @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:
              BasicReadServerMPA.png FIFOSERVERMPA.png 500IMUROS.png 200IMUROS.png

              Best,
              --ZBFT

              C 1 Reply Last reply Reply Quote 0
              • zauberflote1Z
                zauberflote1 ModalAI Team @Cliff Wong
                last edited by

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

                zauberflote1Z 1 Reply Last reply Reply Quote 0
                • zauberflote1Z
                  zauberflote1 ModalAI Team @zauberflote1
                  last edited by

                  @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

                  tiralonghipolT 1 Reply Last reply Reply Quote 0
                  • tiralonghipolT
                    tiralonghipol @zauberflote1
                    last edited by

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

                    zauberflote1Z 1 Reply Last reply Reply Quote 0
                    • zauberflote1Z
                      zauberflote1 ModalAI Team @tiralonghipol
                      last edited by

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

                      tiralonghipolT 1 Reply Last reply Reply Quote 2
                      • tiralonghipolT
                        tiralonghipol @zauberflote1
                        last edited by

                        @zauberflote1 amazing, thank you

                        1 Reply Last reply Reply Quote 0
                        • C
                          coreyazion @zauberflote1
                          last edited by

                          @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!

                          1 Reply Last reply Reply Quote 0
                          • First post
                            Last post
                          Powered by NodeBB | Contributors