Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add more rotation vector types #449

Merged
merged 2 commits into from
Nov 9, 2023
Merged

Add more rotation vector types #449

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
939ed24
separate magnetometer from rotation vector
Serafadam Nov 6, 2023
9d3b39c
add rotation vector types
Serafadam Nov 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions README.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading