Best posts made by zauberflote1

zauberflote1

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

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

zauberflote1

Hello ModalAI community,
Hardware: VOXL2 (Sentinel)

Recently, I was trying to run a few experiments that required a conservative obstacle avoidance capability. My objective was to send relative position setpoints (drone-centered frame) to voxl-mapper; however, I noticed that the only available package that allows to pipeline setpoints to voxl-mapper is voxl-portal, and the user has to manually on click each individual setpoint on the portal site.

Note that I am not talking about sending a full trajectory, just a goal setpoint and allow voxl-mapper to find the optimal path to it

Hence, I developed what is called the mapper-commander package. Of course, implementation details will vary from application to application, but for this base case example, the package allows the user to send a square maneuver -- you can easily modify this as you wish, e.g., figure 8 maneuver.

How it works?

Grab VIO estimates insert them in the ring buffer
Add desired relative setpoints to current position in the ring buffer
Send control pipe commands to plan, follow and stop following accordingly
A separate thread controls the cmdHandler callbacks and sleeps based on the realtime clock

/**
 * @ Author: zauberflote1
 * @ Create Time: 2024-10-06 23:20:17
 * @ Modified by: zauberflote1
 * @ Modified time: 2024-10-11 19:31:20
 * @ Description:
 * PIPELINE TO VOXL-MAPPER VIA COMMANDS
 */


#include "mapper_cmd.hpp"

MapperCmd::MapperCmd() {
    

    ch_mapper = pipe_client_get_next_available_channel();
    ch_vio = pipe_client_get_next_available_channel();

    pose_buffer = RC_TF_RINGBUF_INITIALIZER;
    rc_tf_ringbuf_alloc(&pose_buffer, 2500);

    pipe_client_set_simple_helper_cb(ch_vio, [](int ch, char* data, int bytes, void* context) {
        static_cast<MapperCmd*>(context)->_vio_helper_cb(ch, data, bytes, context);
    }, this);

    pipe_client_set_connect_cb(ch_vio, [](int ch, void* context) {
        static_cast<MapperCmd*>(context)->_vio_connect_cb(ch, context);
    }, this);

    pipe_client_set_disconnect_cb(ch_vio, [](int ch, void* context) {
        static_cast<MapperCmd*>(context)->_vio_disconnect_cb(ch, context);
    }, this);


//SQUARE MISSION ONLY
//1M SQUARE
    square_mission << 1.0, 0.0, 0.0,
                      0.0, 1.0, 0.0,
                      -1.0, 0.0, 0.0,
                      0.0, -1.0, 0.0;

    pts_togo = square_mission.rows();
}

MapperCmd::~MapperCmd() {
    //CLOSE PIPES --  AND FREE BUFFERS?
    _cmdHandler_terminate_cb();
    pipe_client_close_all();
}



int MapperCmd::getRobotPose(rc_tf_t &tf_body_wrt_fixed, int64_t ts){
    static int error_ctr = 0;

    int ret = rc_tf_ringbuf_get_tf_at_time(&pose_buffer, ts, &tf_body_wrt_fixed);
    if (ret < 0)
    {
        error_ctr++;
        if(error_ctr>5){
            //fprintf(stderr, "ERROR fetching tf from tf ringbuffer\n");
            if (ret == -2){
                printf("INFO waiting for VIO to start\n");
                error_ctr = 0;
            }
            if (ret == -3){
                printf("WARNING the requested timestamp was too new, VIO may have stopped\n");
                error_ctr = 0;
            }
            if (ret == -4){
                printf("WARNING the requested timestamp was too old\n");
            }
        }
        return -1;
    }

    return 0;
}

int MapperCmd::sendPlanCommand(Eigen::Vector3d goal_vtf) {


    char command[100];
    //DO NOT STOP FOLLOWING IF THIS IS THE FIRST ITERATION -- MAY CHANGE THIS IN THE FUTURE...
    if (goal_plan){ //CHECK POTENTIAL RACE CONDITION
        snprintf(command, sizeof(command), STOP_FOLLOWING); // STOP_FOLLOWING = "stop_following"
        if (pipe_client_send_control_cmd(ch_mapper, command) == 0) {
            std::cout << "Sent STOP_FOLLOWING command: " << command << std::endl;
        } else {
            std::cerr << "Failed to send STOP_FOLLOWING command" << std::endl;
            return -1;
        }
    }
    usleep(500000); // SLEEP FOR 0.5 SECOND TO GIVE THE SERVER TIME TO PROCESS STOP_FOLLOWING COMMAND
    snprintf(command, sizeof(command), "plan_to:%.2f,%.2f,%.2f", goal_vtf.x(), goal_vtf.y(), goal_vtf.z());

    // Send the command to the server using pipe_client_send_control_cmd
    if (pipe_client_send_control_cmd(ch_mapper, command) == 0) {
        std::cout << "Sent PLAN_TO command: " << command << std::endl;
    } else {
        std::cerr << "Failed to send PLAN_TO command" << std::endl;
        return -1;
    }
    usleep(500000); // SLEEP FOR 0.5 SECOND TO GIVE THE SERVER TIME TO PROCESS PLAN_TO COMMAND
    snprintf(command, sizeof(command), FOLLOW_PATH); // FOLLOW_PATH = "follow_path"
    if (pipe_client_send_control_cmd(ch_mapper, command) == 0) { 
        std::cout << "Sent FOLLOW_TO command: " << command << std::endl;
    } else {
        std::cerr << "Failed to send FOLLOW_TO command" << std::endl;
        return -1;
    }
    return 0;

}
//////////////////////////////////////////////////////////////////////////////////////////////////////////
//VIO HELPER FUNCTIONS (FROM VOXL-MAPPER)
void MapperCmd::_vio_helper_cb(__attribute__((unused)) int ch, char *data, int bytes, __attribute__((unused)) void *context){
    // validate data
    int n_packets;
    pose_vel_6dof_t *d = pipe_validate_pose_vel_6dof_t(data, bytes, &n_packets);

    // if there was an error OR no packets received, just return;
    if (d == NULL){
        std::cerr << "[VIO] ERROR: Failed to validate pose_vel_6dof_t data" << std::endl;
        return;
    }
    if (n_packets <= 0){
        std::cerr << "[VIO] ERROR: No packets received" << std::endl;
        return;
    }

    for (int i = 0; i < n_packets; i++)
    {
        rc_tf_ringbuf_insert_pose(&pose_buffer, d[i]);
    }
    // std::cout << "[VIO] GOT NEW" << std::endl;
    return;
}

void MapperCmd::_vio_connect_cb(__attribute__((unused)) int ch, __attribute__((unused)) void *context){
    printf("Connected to VIO server\n");
}

void MapperCmd::_vio_disconnect_cb(__attribute__((unused)) int ch, __attribute__((unused)) void *context){
    printf("Disconnected from VIO server\n");
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////

//MAPPERCMD HELPER FUNCTIONS
void MapperCmd::cmdHandler(){
    //GET CURRENT POSITION
    rc_tf_t tf_body_wrt_fixed = RC_TF_INITIALIZER;
    int ret_cmd = -2;
    if (getRobotPose(tf_body_wrt_fixed, my_time_monotonic_ns()) != 0) {
        return; //EARLY EXIT IF WE FAIL TO GET THE CURRENT POSITION
    }
    Eigen::Vector3d current_pos = Eigen::Vector3d(tf_body_wrt_fixed.d[0][3], tf_body_wrt_fixed.d[1][3], tf_body_wrt_fixed.d[2][3]);
    // std::cout << tf_body_wrt_fixed.d[0][3] << std::endl;
    // std::cout << tf_body_wrt_fixed.d[1][3] << std::endl;
    // std::cout << tf_body_wrt_fixed.d[2][3] << std::endl;
    if (pts_togo == 0){

        std::cout << "MISSION COMPLETE" << std::endl;

        return;
    }
    Eigen::Vector3d goal_pos = square_mission.row(pts_togo - 1).transpose() + current_pos;
    //START GOAL PLAN, IF WE HAVEN'T ALREADY
    if (!goal_plan){
        //SEND INITIAL GOAL
        ret_cmd = sendPlanCommand(goal_pos);
        if (ret_cmd != 0){
            std::cerr << "[MAPPER-CMD] Failed to send initial goal" << std::endl;
            return;
        }
        goal_plan = true; //CHECK POTENTIAL RACE CONDITION IF USING MULTIPLE DRONES
    }
    else{
        if((current_pos - (square_mission.row(pts_togo)).transpose()).norm() < 0.1){
            //SEND NEW GOAL
            ret_cmd = sendPlanCommand(goal_pos);
            if (ret_cmd != 0){
                std::cerr << "[MAPPER-CMD] Failed to send new goal" << std::endl;
                return;
            }   
        } else{
            // std::cout << "WAITING FOR GOAL" << std::endl;
            return;
        }
        
    }
    if (pts_togo > 0) { //EXTRA SAFETY CHECK
        pts_togo--;
    }


}

void MapperCmd::_cmdHandler_cb() {
    int64_t next_time = 0;
    while (!stop_thread) {
        {
            std::lock_guard<std::mutex> lock(cmd_mutex);
            // std::cout << "[CMD HANDLER] Iteration start" << std::endl;
            cmdHandler();
        }
        // std::cout << "[CMD HANDLER] Sleeping for 20Hz cycle." << std::endl; 
        my_loop_sleep(20.0, &next_time); //20HZ, MIGHT BE TOO FAST OR TOO SLOW...
    }
        // std::cout << "[CMD HANDLER] Thread stopping." << std::endl; 
}
void MapperCmd::_cmdHandler_terminate_cb() {
    stop_thread = true;
    if (cmd_thread.joinable()) {
        cmd_thread.join();
    }
}
void MapperCmd::initMPA(){
//MAPPER
    ret_mapper = pipe_client_open(ch_mapper, PLAN_LOCATION, "zbft-mapper-cmd", EN_PIPE_CLIENT_AUTO_RECONNECT, 1024 * 1024 * 64); //NOT SURE ABOUT THE BUFFER SIZE HERE...JUST COPYING FROM VOXL-MAPPER
    if (ret_mapper != 0) {
        std::cerr << "[MAPPER] Failed to open the pipe client." << std::endl;
        return;
    }
    std::cout << "[MAPPER] Pipe client opened successfully." << std::endl;
//VIO
   ret_vio = pipe_client_open(ch_vio, BODY_WRT_FIXED_POSE_PATH, "zbft-mapper-cmd",
                         EN_PIPE_CLIENT_SIMPLE_HELPER | EN_PIPE_CLIENT_AUTO_RECONNECT,
                         POSE_6DOF_RECOMMENDED_READ_BUF_SIZE);
    if (ret_vio != 0){
        std::cerr << "[VIO] Failed to open the pipe client." << std::endl;
        return;
    }
    std::cout << "[VIO] Pipe client opened successfully." << std::endl;
//CMD THREAD
    cmd_thread = std::thread(&MapperCmd::_cmdHandler_cb, this);
}

Observation 1:
My goal was to not modify the voxl-mapper package, but if you decide that integrating mapper-commander to voxl-mapper is better for you, you can make the ring buffer variable a shared pointer -- this should save some memory allocation/usage cost.

Observation 2:
I am not using librc-math library, so I copied the source files needed for ring buffer operations -- similarly to what is done in voxl-mapper, but different than what is done in voxl-vision-hub.

Observation 3:
You may not need a ring buffer variable, but for more complex routines it might be interested to store past VIO estimates. If you decide to not use the ring buffer variable, make sure to add the proper mutex to avoid race conditions from the VIO pipe client thread and the command handler thread when dealing with the current pose variable

Future TODOs (it would be great to hear your insights):

ADD CHECK IF DESIRED SETPOINT WAS ILLEGAL -- maybe elapsed time based?
POTENTIALLY SEND STOP_FOLLOWING BEFORE INITIAL POINT -- in case you want to repeat the maneuver without resetting voxl-mapper

DISCLAIMER:
Fly offboard at your own discretion and responsibility!

Full Github repo:
https://github.com/zauberflote1/zbft-mapper-commander/tree/main
Please let me know your thoughts!

As always happy to contribute,
--ZBFT