Skip to content

Commit

Permalink
Add more rotation vector types (#449)
Browse files Browse the repository at this point in the history 
* separate magnetometer from rotation vector

* add rotation vector types
  • Loading branch information
@Serafadam
Serafadam authored Nov 9, 2023
1 parent 4795034 commit 1a9b13c
Show file tree
Hide file tree
Showing 9 changed files with 149 additions and 50 deletions.
7 changes: 7 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -231,6 +231,12 @@ Parameters:
* `COPY`
* `LINEAR_INTERPOLATE_GYRO`
* `LINEAR_INTERPOLATE_ACCEL`
* `i_rotation_vector_type` - type of rotation vector, for more information, refer to [this link](https://docs.luxonis.com/projects/api/en/latest/components/nodes/imu/). Available options:
* `ROTATION_VECTOR`
* `GAME_ROTATION_VECTOR`
* `GEOMAGNETIC_ROTATION_VECTOR`
* `ARVR_STABILIZED_ROTATION_VECTOR`
* `ARVR_STABILIZED_GAME_ROTATION_VECTOR`

#### Stopping/starting camera for power saving/reconfiguration
![](docs/start_stop.gif)
Expand Down Expand Up @@ -451,6 +457,7 @@ For easier development inside isolated workspace, one can use Visual Studio Code
i_max_batch_reports: 10
i_message_type: IMU
i_rot_cov: -1.0
i_rotation_vector_type: ROTATION_VECTOR
i_sync_method: LINEAR_INTERPOLATE_ACCEL
i_update_ros_base_time_on_ros_msg: false
left:
Expand Down
120 changes: 98 additions & 22 deletions depthai_bridge/include/depthai_bridge/ImuConverter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ class ImuConverter {
double rotation_cov = 0.0,
double magnetic_field_cov = 0.0,
bool enable_rotation = false,
bool enable_magn = false,
bool getBaseDeviceTimestamp = false);
~ImuConverter();

Expand Down Expand Up @@ -99,9 +100,9 @@ class ImuConverter {
rotationHist.resize(accelHist.size());
}

if(_enable_rotation && magnHist.size() == 0) {
if(_enable_magn && magnHist.size() == 0) {
magnHist.push_back(imuPackets[i].magneticField);
} else if(_enable_rotation && magnHist.back().sequence != imuPackets[i].magneticField.sequence) {
} else if(_enable_magn && magnHist.back().sequence != imuPackets[i].magneticField.sequence) {
magnHist.push_back(imuPackets[i].magneticField);
} else {
magnHist.resize(accelHist.size());
Expand All @@ -112,7 +113,11 @@ class ImuConverter {
continue;
} else {
if(_enable_rotation) {
interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs);
if(_enable_magn) {
interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs);
} else {
interpolate(accelHist, gyroHist, rotationHist, imuMsgs);
}
} else {
interpolate(accelHist, gyroHist, imuMsgs);
}
Expand All @@ -123,7 +128,11 @@ class ImuConverter {
continue;
} else {
if(_enable_rotation) {
interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs);
if(_enable_magn) {
interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs);
} else {
interpolate(gyroHist, accelHist, rotationHist, imuMsgs);
}
} else {
interpolate(gyroHist, accelHist, imuMsgs);
}
Expand All @@ -135,6 +144,7 @@ class ImuConverter {
uint32_t _sequenceNum;
double _linear_accel_cov, _angular_velocity_cov, _rotation_cov, _magnetic_field_cov;
bool _enable_rotation;
bool _enable_magn;
const std::string _frameName = "";
ImuSyncMethod _syncMode;
std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
Expand All @@ -145,32 +155,43 @@ class ImuConverter {
// Whether to update the ROS base time on each message conversion
bool _updateRosBaseTimeOnToRosMsg{false};

void fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg);
void fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg);
void fillImuMsg(dai::IMUReportRotationVectorWAcc report, ImuMsgs::Imu& msg);
void fillImuMsg(dai::IMUReportMagneticField report, ImuMsgs::Imu& msg);
void fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportAccelerometer report);
void fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportGyroscope report);
void fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportRotationVectorWAcc report);
void fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportMagneticField report);

void fillImuMsg(dai::IMUReportAccelerometer report, depthai_ros_msgs::msg::ImuWithMagneticField& msg);
void fillImuMsg(dai::IMUReportGyroscope report, depthai_ros_msgs::msg::ImuWithMagneticField& msg);
void fillImuMsg(dai::IMUReportRotationVectorWAcc report, depthai_ros_msgs::msg::ImuWithMagneticField& msg);
void fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_msgs::msg::ImuWithMagneticField& msg);
void fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportAccelerometer report);
void fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportGyroscope report);
void fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportRotationVectorWAcc report);
void fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportMagneticField report);

template <typename I, typename S, typename T, typename F, typename M>
void CreateUnitMessage(I first, S second, T third, F fourth, M& msg, std::chrono::_V2::steady_clock::time_point timestamp) {
fillImuMsg(first, msg);
fillImuMsg(second, msg);
fillImuMsg(third, msg);
fillImuMsg(fourth, msg);
void CreateUnitMessage(M& msg, std::chrono::_V2::steady_clock::time_point timestamp, I first, S second, T third, F fourth) {
fillImuMsg(msg, first);
fillImuMsg(msg, second);
fillImuMsg(msg, third);
fillImuMsg(msg, fourth);

msg.header.frame_id = _frameName;

msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp);
}

template <typename I, typename S, typename T, typename M>
void CreateUnitMessage(M& msg, std::chrono::_V2::steady_clock::time_point timestamp, I first, S second, T third) {
fillImuMsg(msg, first);
fillImuMsg(msg, second);
fillImuMsg(msg, third);

msg.header.frame_id = _frameName;

msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp);
}

template <typename I, typename S, typename M>
void CreateUnitMessage(I first, S second, M& msg, std::chrono::_V2::steady_clock::time_point timestamp) {
fillImuMsg(first, msg);
fillImuMsg(second, msg);
void CreateUnitMessage(M& msg, std::chrono::_V2::steady_clock::time_point timestamp, I first, S second) {
fillImuMsg(msg, first);
fillImuMsg(msg, second);

msg.header.frame_id = _frameName;

Expand Down Expand Up @@ -205,9 +226,63 @@ class ImuConverter {
tstamp = currSecond.getTimestampDevice();
else
tstamp = currSecond.getTimestamp();
CreateUnitMessage(interp, currSecond, msg, tstamp);
CreateUnitMessage(msg, tstamp, interp, currSecond);
imuMsgs.push_back(msg);
second.pop_front();
} else if(currSecond.timestamp.get() > interp1.timestamp.get()) {
interp0 = interp1;
if(interpolated.size()) {
interp1 = interpolated.front();
interpolated.pop_front();
duration_ms = interp1.timestamp.get() - interp0.timestamp.get();
dt = duration_ms.count();
} else {
break;
}
} else {
second.pop_front();
}
}
interp0 = interp1;
}
}
interpolated.push_back(interp0);
}

template <typename I, typename S, typename T, typename M>
void interpolate(std::deque<I>& interpolated, std::deque<S>& second, std::deque<T>& third, std::deque<M>& imuMsgs) {
I interp0, interp1;
S currSecond;
T currThird;
interp0.sequence = -1;
while(interpolated.size()) {
if(interp0.sequence == -1) {
interp0 = interpolated.front();
interpolated.pop_front();
} else {
interp1 = interpolated.front();
interpolated.pop_front();
// remove std::milli to get in seconds
std::chrono::duration<double, std::milli> duration_ms = interp1.timestamp.get() - interp0.timestamp.get();
double dt = duration_ms.count();
while(second.size()) {
currSecond = second.front();
currThird = third.front();
if(currSecond.timestamp.get() > interp0.timestamp.get() && currSecond.timestamp.get() <= interp1.timestamp.get()) {
// remove std::milli to get in seconds
std::chrono::duration<double, std::milli> diff = currSecond.timestamp.get() - interp0.timestamp.get();
const double alpha = diff.count() / dt;
I interp = lerpImu(interp0, interp1, alpha);
M msg;
std::chrono::_V2::steady_clock::time_point tstamp;
if(_getBaseDeviceTimestamp)
tstamp = currSecond.getTimestampDevice();
else
tstamp = currSecond.getTimestamp();
CreateUnitMessage(msg, tstamp, interp, currSecond, currThird);
imuMsgs.push_back(msg);
second.pop_front();
third.pop_front();
} else if(currSecond.timestamp.get() > interp1.timestamp.get()) {
interp0 = interp1;
if(interpolated.size()) {
Expand All @@ -220,6 +295,7 @@ class ImuConverter {
}
} else {
second.pop_front();
third.pop_front();
}
}
interp0 = interp1;
Expand Down Expand Up @@ -260,7 +336,7 @@ class ImuConverter {
tstamp = currSecond.getTimestampDevice();
else
tstamp = currSecond.getTimestamp();
CreateUnitMessage(interp, currSecond, currThird, currFourth, msg, tstamp);
CreateUnitMessage(msg, tstamp, interp, currSecond, currThird, currFourth);
imuMsgs.push_back(msg);
second.pop_front();
third.pop_front();
Expand Down
36 changes: 21 additions & 15 deletions depthai_bridge/src/ImuConverter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ ImuConverter::ImuConverter(const std::string& frameName,
double rotation_cov,
double magnetic_field_cov,
bool enable_rotation,
bool enable_magn,
bool getBaseDeviceTimestamp)
: _frameName(frameName),
_syncMode(syncMode),
Expand All @@ -22,6 +23,7 @@ ImuConverter::ImuConverter(const std::string& frameName,
_rotation_cov(rotation_cov),
_magnetic_field_cov(magnetic_field_cov),
_enable_rotation(enable_rotation),
_enable_magn(enable_magn),
_sequenceNum(0),
_steadyBaseTime(std::chrono::steady_clock::now()),
_getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
Expand All @@ -34,21 +36,21 @@ void ImuConverter::updateRosBaseTime() {
updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange);
}

void ImuConverter::fillImuMsg(dai::IMUReportAccelerometer report, ImuMsgs::Imu& msg) {
void ImuConverter::fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportAccelerometer report) {
msg.linear_acceleration.x = report.x;
msg.linear_acceleration.y = report.y;
msg.linear_acceleration.z = report.z;
msg.linear_acceleration_covariance = {_linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov};
}

void ImuConverter::fillImuMsg(dai::IMUReportGyroscope report, ImuMsgs::Imu& msg) {
void ImuConverter::fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportGyroscope report) {
msg.angular_velocity.x = report.x;
msg.angular_velocity.y = report.y;
msg.angular_velocity.z = report.z;
msg.angular_velocity_covariance = {_angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov};
}

void ImuConverter::fillImuMsg(dai::IMUReportRotationVectorWAcc report, ImuMsgs::Imu& msg) {
void ImuConverter::fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportRotationVectorWAcc report) {
if(_enable_rotation) {
msg.orientation.x = report.i;
msg.orientation.y = report.j;
Expand All @@ -64,23 +66,23 @@ void ImuConverter::fillImuMsg(dai::IMUReportRotationVectorWAcc report, ImuMsgs::
}
}

void ImuConverter::fillImuMsg(dai::IMUReportMagneticField report, ImuMsgs::Imu& msg) {
void ImuConverter::fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportMagneticField report) {
return;
}

void ImuConverter::fillImuMsg(dai::IMUReportAccelerometer report, depthai_ros_msgs::msg::ImuWithMagneticField& msg) {
fillImuMsg(report, msg.imu);
void ImuConverter::fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportAccelerometer report) {
fillImuMsg(msg.imu, report);
}

void ImuConverter::fillImuMsg(dai::IMUReportGyroscope report, depthai_ros_msgs::msg::ImuWithMagneticField& msg) {
fillImuMsg(report, msg.imu);
void ImuConverter::fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportGyroscope report) {
fillImuMsg(msg.imu, report);
}

void ImuConverter::fillImuMsg(dai::IMUReportRotationVectorWAcc report, depthai_ros_msgs::msg::ImuWithMagneticField& msg) {
fillImuMsg(report, msg.imu);
void ImuConverter::fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportRotationVectorWAcc report) {
fillImuMsg(msg.imu, report);
}

void ImuConverter::fillImuMsg(dai::IMUReportMagneticField report, depthai_ros_msgs::msg::ImuWithMagneticField& msg) {
void ImuConverter::fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportMagneticField report) {
msg.field.magnetic_field.x = report.x;
msg.field.magnetic_field.y = report.y;
msg.field.magnetic_field.z = report.z;
Expand All @@ -97,14 +99,19 @@ void ImuConverter::toRosMsg(std::shared_ptr<dai::IMUData> inData, std::deque<Imu
for(int i = 0; i < inData->packets.size(); ++i) {
auto accel = inData->packets[i].acceleroMeter;
auto gyro = inData->packets[i].gyroscope;

ImuMsgs::Imu msg;
std::chrono::_V2::steady_clock::time_point tstamp;
if(_getBaseDeviceTimestamp)
tstamp = accel.getTimestampDevice();
else
tstamp = accel.getTimestamp();

CreateUnitMessage(accel, gyro, msg, tstamp);
if(_enable_rotation) {
auto rot = inData->packets[i].rotationVector;
CreateUnitMessage(msg, tstamp, accel, gyro, rot);
} else {
CreateUnitMessage(msg, tstamp, accel, gyro);
}

outImuMsgs.push_back(msg);
}
Expand All @@ -129,8 +136,7 @@ void ImuConverter::toRosDaiMsg(std::shared_ptr<dai::IMUData> inData, std::deque<
tstamp = accel.getTimestampDevice();
else
tstamp = accel.getTimestamp();

CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp);
CreateUnitMessage(msg, tstamp, accel, gyro, rot, magn);
outImuMsgs.push_back(msg);
}
}
Expand Down
2 changes: 2 additions & 0 deletions depthai_ros_driver/include/depthai_ros_driver/param_handlers/imu_param_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <vector>

#include "depthai/pipeline/datatype/CameraControl.hpp"
#include "depthai-shared/properties/IMUProperties.hpp"
#include "depthai_bridge/ImuConverter.hpp"
#include "depthai_ros_driver/param_handlers/base_param_handler.hpp"

Expand Down Expand Up @@ -32,6 +33,7 @@ class ImuParamHandler : public BaseParamHandler {
dai::CameraControl setRuntimeParams(const std::vector<rclcpp::Parameter>& params) override;
std::unordered_map<std::string, dai::ros::ImuSyncMethod> imuSyncMethodMap;
std::unordered_map<std::string, imu::ImuMsgType> imuMessagetTypeMap;
std::unordered_map<std::string, dai::IMUSensor> rotationVectorTypeMap;
imu::ImuMsgType getMsgType();
dai::ros::ImuSyncMethod getSyncMethod();
};
Expand Down
8 changes: 2 additions & 6 deletions depthai_ros_driver/include/depthai_ros_driver/param_handlers/sensor_param_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,8 @@ class SensorParamHandler : public BaseParamHandler {
explicit SensorParamHandler(rclcpp::Node* node, const std::string& name, dai::CameraBoardSocket socket);
~SensorParamHandler();
void declareCommonParams(dai::CameraBoardSocket socket);
void declareParams(std::shared_ptr<dai::node::MonoCamera> monoCam,
dai_nodes::sensor_helpers::ImageSensor sensor,
bool publish);
void declareParams(std::shared_ptr<dai::node::ColorCamera> colorCam,
dai_nodes::sensor_helpers::ImageSensor sensor,
bool publish);
void declareParams(std::shared_ptr<dai::node::MonoCamera> monoCam, dai_nodes::sensor_helpers::ImageSensor sensor, bool publish);
void declareParams(std::shared_ptr<dai::node::ColorCamera> colorCam, dai_nodes::sensor_helpers::ImageSensor sensor, bool publish);
dai::CameraControl setRuntimeParams(const std::vector<rclcpp::Parameter>& params) override;

private:
Expand Down
2 changes: 1 addition & 1 deletion depthai_ros_driver/include/depthai_ros_driver/pipeline/pipeline_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ class PipelineGenerator {
*
* @return The validated pipeline type.
*/
std::string validatePipeline(rclcpp::Node* node,const std::string& typeStr, int sensorNum);
std::string validatePipeline(rclcpp::Node* node, const std::string& typeStr, int sensorNum);
/**
* @brief Creates the pipeline by using a plugin. Plugin types need to be of type depthai_ros_driver::pipeline_gen::BasePipeline.
*
Expand Down
4 changes: 3 additions & 1 deletion depthai_ros_driver/src/dai_nodes/sensors/imu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,16 +40,18 @@ void Imu::setupQueues(std::shared_ptr<dai::Device> device) {
auto imuMode = ph->getSyncMethod();
rclcpp::PublisherOptions options;
options.qos_overriding_options = rclcpp::QosOverridingOptions();
param_handlers::imu::ImuMsgType msgType = ph->getMsgType();
bool enableMagn = msgType == param_handlers::imu::ImuMsgType::IMU_WITH_MAG || msgType == param_handlers::imu::ImuMsgType::IMU_WITH_MAG_SPLIT;
imuConverter = std::make_unique<dai::ros::ImuConverter>(tfPrefix + "_frame",
imuMode,
ph->getParam<float>("i_acc_cov"),
ph->getParam<float>("i_gyro_cov"),
ph->getParam<float>("i_rot_cov"),
ph->getParam<float>("i_mag_cov"),
ph->getParam<bool>("i_enable_rotation"),
enableMagn,
ph->getParam<bool>("i_get_base_device_timestamp"));
imuConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));
param_handlers::imu::ImuMsgType msgType = ph->getMsgType();
switch(msgType) {
case param_handlers::imu::ImuMsgType::IMU: {
rosImuPub = getROSNode()->create_publisher<sensor_msgs::msg::Imu>("~/" + getName() + "/data", 10, options);
Expand Down
4 changes: 2 additions & 2 deletions depthai_ros_driver/src/dai_nodes/stereo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -337,10 +337,10 @@ void Stereo::closeQueues() {
if(ph->getParam<bool>("i_publish_topic")) {
stereoQ->close();
}
if(ph->getParam<bool>("i_publish_left_rect")){
if(ph->getParam<bool>("i_publish_left_rect")) {
leftRectQ->close();
}
if(ph->getParam<bool>("i_publish_right_rect")){
if(ph->getParam<bool>("i_publish_right_rect")) {
rightRectQ->close();
}
if(ph->getParam<bool>("i_publish_synced_rect_pair")) {
Expand Down
Loading

0 comments on commit 1a9b13c

Please sign in to comment.