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
