diff --git a/launch/imu_transformer_simulation.launch b/launch/imu_transformer_simulation.launch
new file mode 100644
index 0000000..111a923
--- /dev/null
+++ b/launch/imu_transformer_simulation.launch
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/vins_republisher_openvins_bluefox.launch b/launch/vins_republisher_openvins_bluefox.launch
index 4b5c7a2..f38e0b6 100644
--- a/launch/vins_republisher_openvins_bluefox.launch
+++ b/launch/vins_republisher_openvins_bluefox.launch
@@ -29,7 +29,7 @@
-
+
diff --git a/src/VinsRepublisher.cpp b/src/VinsRepublisher.cpp
index ec24b39..aabada2 100644
--- a/src/VinsRepublisher.cpp
+++ b/src/VinsRepublisher.cpp
@@ -18,6 +18,7 @@
#include
#include
+#include
#include
#include
#include
@@ -68,6 +69,8 @@ class VinsRepublisher : public nodelet::Nodelet {
/* transformation handler */
mrs_lib::Transformer transformer_;
+ std::shared_ptr broadcaster_;
+
geometry_msgs::Point rotatePointByHdg(const geometry_msgs::Point &pt_in, const double hdg_in) const;
};
@@ -118,6 +121,8 @@ void VinsRepublisher::onInit() {
/* transformation handler */
transformer_ = mrs_lib::Transformer("VinsRepublisher");
+ broadcaster_ = std::make_shared();
+
// | ----------------------- subscribers ---------------------- |
subscriber_vins_ = nh_.subscribe("vins_odom_in", 10, &VinsRepublisher::odometryCallback, this, ros::TransportHints().tcpNoDelay());
@@ -251,171 +256,292 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
geometry_msgs::TransformStamped tf;
- /* get the transform from mrs_vins_world to vins_world //{ */
-
- /* Vins-mono uses different coordinate system. The y-axis is front, x-axis is right, z-axis is up */
- /* This transformation rotates in yaw of 90 deg */
-
- {
- auto res = transformer_.getTransform(odom->header.frame_id, _mrs_vins_world_frame_, odom->header.stamp);
+ nav_msgs::Odometry odom_transformed;
+ odom_transformed.header = odom->header;
+ odom_transformed.header.frame_id = _mrs_vins_world_frame_;
+
+ // transform pose
+ geometry_msgs::Pose pose_transformed;
+ // first transform it from IMU frame to FCU frame
+ // transform from the VINS body frame to the UAV FCU frame
+ auto res = transformer_.getTransform(_fcu_frame_, _vins_fcu_frame_, odom->header.stamp);
+ if (!res) {
+ ROS_WARN_THROTTLE(1.0, "[%s]: could not find transform from '%s' to '%s'", ros::this_node::getName().c_str(), _fcu_frame_.c_str(),
+ _vins_fcu_frame_.c_str());
+ return;
+ }
- if (!res) {
- ROS_WARN_THROTTLE(1.0, "[%s]: could not find transform from '%s' to '%s' at time '%f'", ros::this_node::getName().c_str(), _mrs_vins_world_frame_.c_str(),
- odom->header.frame_id.c_str(), odom->header.stamp.toSec());
- return;
+ Eigen::Matrix3d rotation = mrs_lib::AttitudeConverter(res.value().transform.rotation);
+ Eigen::Matrix3d original_orientation = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+ pose_transformed.orientation = mrs_lib::AttitudeConverter(original_orientation * rotation);
+ /* pose_transformed.orientation = mrs_lib::AttitudeConverter(rotation.transpose() * original_orientation.transpose()); */
+ Eigen::Vector3d t;
+ t << res.value().transform.translation.x, res.value().transform.translation.y, res.value().transform.translation.z;
+ Eigen::Vector3d translation = rotation.transpose() * t;
+ pose_transformed.position.x = odom->pose.pose.position.x + translation(0);
+ pose_transformed.position.y = odom->pose.pose.position.y + translation(1);
+ pose_transformed.position.z = odom->pose.pose.position.z + translation(2);
+
+ // then subtract initial pose (global 4 DOF)
+ // save initial pose to subtract it from all messages to compensate initialized orientation ambiguity
+ // TODO compensate position as well???
+ if (_init_in_zero_) {
+ if (!got_init_pose_) {
+ init_hdg_ = mrs_lib::AttitudeConverter(pose_transformed.orientation).getHeading();
+ ROS_INFO("[VinsRepublisher]: init hdg: %.2f", init_hdg_);
+ got_init_pose_ = true;
}
- tf = res.value();
+ // compensate initial heading ambiguity
+ const double hdg = mrs_lib::AttitudeConverter(pose_transformed.orientation).getHeading();
+ pose_transformed.orientation = mrs_lib::AttitudeConverter(pose_transformed.orientation).setHeading(hdg - init_hdg_);
+ pose_transformed.position = rotatePointByHdg(pose_transformed.position, -init_hdg_);
}
+ // TODO publish the initial offset to TF
+ geometry_msgs::TransformStamped tf_msg;
+ tf_msg.header.stamp = odom->header.stamp;
+ tf_msg.header.frame_id = odom->header.frame_id;
+ tf_msg.child_frame_id = odom_transformed.header.frame_id;
+ /* tf_msg.transform.translation = pointToVector3(pose_inv.position); */
+ tf_msg.transform.translation.x = 0;
+ tf_msg.transform.translation.x = 0;
+ tf_msg.transform.translation.x = 0;
+ tf_msg.transform.rotation = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(init_hdg_);
+
+ /* if (noNans(tf_msg)) { */
+ try {
+ ROS_INFO_THROTTLE(1.0, "[UavPoseEstimator]: Publishing TF.");
+ broadcaster_->sendTransform(tf_msg);
+ }
+ catch (...) {
+ ROS_ERROR("[UavPoseEstimator]: Exception caught during publishing TF: %s - %s.", tf_msg.child_frame_id.c_str(), tf_msg.header.frame_id.c_str());
+ }
+ /* } else { */
+ /* ROS_WARN_THROTTLE(1.0, "[UavPoseEstimator]: NaN detected in transform from %s to %s. Not publishing tf.", tf_msg.header.frame_id.c_str(), */
+ /* tf_msg.child_frame_id.c_str()); */
+ /* } */
- //}
+ odom_transformed.pose.pose = pose_transformed;
- /* transform the vins pose to mrs_world_frame //{ */
- geometry_msgs::PoseStamped vins_pose_mrs_world;
+ // transform velocity - linear and angular
+ // if in body frame - rotate from IMU frame to FCU frame
+ geometry_msgs::Vector3 linear_velocity;
+ linear_velocity = odom->twist.twist.linear;
+ Eigen::Vector3d v2;
+ v2 << vins_velocity.vector.x, vins_velocity.vector.y, vins_velocity.vector.z;
+ v2 = rotation.transpose() * v2;
+ linear_velocity.x = v2(0);
+ linear_velocity.y = v2(1);
+ linear_velocity.z = v2(2);
- {
- auto res = transformer_.transform(vins_pose, tf);
+ geometry_msgs::Vector3 angular_velocity;
+ angular_velocity = odom->twist.twist.angular;
+ Eigen::Vector3d v3;
+ v3 << vins_velocity.vector.x, vins_velocity.vector.y, vins_velocity.vector.z;
+ v3 = rotation.transpose() * v3;
+ angular_velocity.x = v3(0);
+ angular_velocity.y = v3(1);
+ angular_velocity.z = v3(2);
+ // TODO if in global frame, apply initial TF offset
- if (!res) {
- ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vins pose to '%s'", ros::this_node::getName().c_str(), _mrs_vins_world_frame_.c_str());
- return;
- }
+ odom_transformed.twist.twist.linear = linear_velocity;
+ odom_transformed.twist.twist.angular = angular_velocity;
- vins_pose_mrs_world = res.value();
- }
- Eigen::Matrix3d rotation;
+ // transform covariance - apparently translation is in world frame and rotation is in body frame with fixed axes???
+ // if so, apply initial TF offset to translation part and IMU to FCU TF to rotation part??? + twist covariance...
- {
- // transform from the VINS body frame to the UAV FCU frame
- auto res = transformer_.getTransform(_fcu_frame_, _vins_fcu_frame_, odom->header.stamp);
- if (!res) {
- ROS_WARN_THROTTLE(1.0, "[%s]: could not find transform from '%s' to '%s'", ros::this_node::getName().c_str(), _fcu_frame_.c_str(),
- _vins_fcu_frame_.c_str());
- return;
- }
- rotation = mrs_lib::AttitudeConverter(res.value().transform.rotation);
- Eigen::Matrix3d original_orientation = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation);
- vins_pose_mrs_world.pose.orientation = mrs_lib::AttitudeConverter(original_orientation * rotation);
- Eigen::Vector3d t;
- t << res.value().transform.translation.x, res.value().transform.translation.y, res.value().transform.translation.z;
- Eigen::Vector3d translation = rotation.transpose() * t;
- vins_pose_mrs_world.pose.position.x = vins_pose_mrs_world.pose.position.x + translation(0);
- vins_pose_mrs_world.pose.position.y = vins_pose_mrs_world.pose.position.y + translation(1);
- vins_pose_mrs_world.pose.position.z = vins_pose_mrs_world.pose.position.z + translation(2);
+ // publish
+ if (!validateOdometry(odom_transformed)) {
+ ROS_ERROR("[VinsRepublisher]: transformed odometry is not numerically valid");
+ return;
}
- //}
+ try {
+ publisher_odom_.publish(odom_transformed);
+ ROS_INFO_THROTTLE(1.0, "[%s]: Publishing", ros::this_node::getName().c_str());
+ publisher_odom_last_published_ = ros::Time::now();
+ }
+ catch (...) {
+ ROS_ERROR("exception caught during publishing topic '%s'", publisher_odom_.getTopic().c_str());
+ }
- geometry_msgs::Vector3Stamped vins_velocity_mrs_world;
- /* transform the vins velocity to mrs_world_frame //{ */
+ /* /1* get the transform from mrs_vins_world to vins_world //{ *1/ */
- {
- auto res = transformer_.transform(vins_velocity, tf);
+ /* /1* Vins-mono uses different coordinate system. The y-axis is front, x-axis is right, z-axis is up *1/ */
+ /* /1* This transformation rotates in yaw of 90 deg *1/ */
- if (!res) {
- ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vins velocity to '%s'", ros::this_node::getName().c_str(), _mrs_vins_world_frame_.c_str());
- return;
- }
+ /* { */
+ /* auto res = transformer_.getTransform(odom->header.frame_id, _mrs_vins_world_frame_, odom->header.stamp); */
- vins_velocity_mrs_world = res.value();
- if (_rotate_velocity_) {
- Eigen::Vector3d v2;
- v2 << vins_velocity.vector.x, vins_velocity.vector.y, vins_velocity.vector.z;
- v2 = rotation.transpose() * v2;
- vins_velocity_mrs_world.vector.x = v2(0);
- vins_velocity_mrs_world.vector.y = v2(1);
- vins_velocity_mrs_world.vector.z = v2(2);
- }
- }
+ /* if (!res) { */
+ /* ROS_WARN_THROTTLE(1.0, "[%s]: could not find transform from '%s' to '%s' at time '%f'", ros::this_node::getName().c_str(),
+ * _mrs_vins_world_frame_.c_str(), */
+ /* odom->header.frame_id.c_str(), odom->header.stamp.toSec()); */
+ /* return; */
+ /* } */
- //}
+ /* tf = res.value(); */
+ /* } */
- geometry_msgs::Vector3Stamped vins_ang_velocity_mrs_world;
+ /* //} */
- /* transform the vins angular velocity to mrs_world_frame //{ */
+ /* /1* transform the vins pose to mrs_world_frame //{ *1/ */
- {
- auto res = transformer_.transform(vins_ang_velocity, tf);
+ /* geometry_msgs::PoseStamped vins_pose_mrs_world; */
- if (!res) {
- ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vins angular velocity to '%s'", ros::this_node::getName().c_str(), _mrs_vins_world_frame_.c_str());
- return;
- }
+ /* { */
+ /* auto res = transformer_.transform(vins_pose, tf); */
- vins_ang_velocity_mrs_world = res.value();
- if (_rotate_velocity_) {
- Eigen::Vector3d v2;
- v2 << vins_ang_velocity.vector.x, vins_ang_velocity.vector.y, vins_ang_velocity.vector.z;
- v2 = rotation.transpose() * v2;
- vins_ang_velocity_mrs_world.vector.x = v2(0);
- vins_ang_velocity_mrs_world.vector.y = v2(1);
- vins_ang_velocity_mrs_world.vector.z = v2(2);
- }
- }
+ /* if (!res) { */
+ /* ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vins pose to '%s'", ros::this_node::getName().c_str(), _mrs_vins_world_frame_.c_str()); */
+ /* return; */
+ /* } */
- //}
+ /* vins_pose_mrs_world = res.value(); */
+ /* } */
- /* geometry_msgs::Vector3 camera_to_fcu_translation; */
+ /* Eigen::Matrix3d rotation; */
- /* /1* find transform from camera frame to fcu frame //{ *1/ */
+ /* { */
+ /* // transform from the VINS body frame to the UAV FCU frame */
+ /* auto res = transformer_.getTransform(_fcu_frame_, _vins_fcu_frame_, odom->header.stamp); */
+ /* if (!res) { */
+ /* ROS_WARN_THROTTLE(1.0, "[%s]: could not find transform from '%s' to '%s'", ros::this_node::getName().c_str(), _fcu_frame_.c_str(), */
+ /* _vins_fcu_frame_.c_str()); */
+ /* return; */
+ /* } */
+
+ /* rotation = mrs_lib::AttitudeConverter(res.value().transform.rotation); */
+ /* Eigen::Matrix3d original_orientation = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation); */
+ /* vins_pose_mrs_world.pose.orientation = mrs_lib::AttitudeConverter(original_orientation * rotation); */
+ /* Eigen::Vector3d t; */
+ /* t << res.value().transform.translation.x, res.value().transform.translation.y, res.value().transform.translation.z; */
+ /* Eigen::Vector3d translation = rotation.transpose() * t; */
+ /* vins_pose_mrs_world.pose.position.x = vins_pose_mrs_world.pose.position.x + translation(0); */
+ /* vins_pose_mrs_world.pose.position.y = vins_pose_mrs_world.pose.position.y + translation(1); */
+ /* vins_pose_mrs_world.pose.position.z = vins_pose_mrs_world.pose.position.z + translation(2); */
+ /* } */
+
+ /* //} */
+
+ /* geometry_msgs::Vector3Stamped vins_velocity_mrs_world; */
+
+ /* /1* transform the vins velocity to mrs_world_frame //{ *1/ */
/* { */
- /* auto res = transformer_.getTransform(_camera_frame_, _fcu_frame_, odom->header.stamp); */
+ /* /1* auto res = transformer_.transform(vins_velocity, tf); *1/ */
+
+ /* /1* if (!res) { *1/ */
+ /* /1* ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vins velocity to '%s'", ros::this_node::getName().c_str(), _mrs_vins_world_frame_.c_str()); *1/
+ */
+ /* /1* return; *1/ */
+ /* /1* } *1/ */
+
+ /* /1* vins_velocity_mrs_world = res.value(); *1/ */
+ /* if (_rotate_velocity_) { */
+ /* Eigen::Vector3d v2; */
+ /* v2 << vins_velocity.vector.x, vins_velocity.vector.y, vins_velocity.vector.z; */
+ /* v2 = rotation.transpose() * v2; */
+ /* vins_velocity_mrs_world.vector.x = v2(0); */
+ /* vins_velocity_mrs_world.vector.y = v2(1); */
+ /* vins_velocity_mrs_world.vector.z = v2(2); */
+ /* } */
+ /* } */
- /* if (!res) { */
+ /* //} */
+
+ /* geometry_msgs::Vector3Stamped vins_ang_velocity_mrs_world; */
+
+ /* /1* transform the vins angular velocity to mrs_world_frame //{ *1/ */
+
+ /* { */
+ /* auto res = transformer_.transform(vins_ang_velocity, tf); */
- /* ROS_WARN_THROTTLE(1.0, "[%s]: could not find transform from '%s' to '%s'", ros::this_node::getName().c_str(), _camera_frame_.c_str(), */
- /* _fcu_frame_.c_str()); */
+ /* if (!res) { */
+ /* ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vins angular velocity to '%s'", ros::this_node::getName().c_str(), _mrs_vins_world_frame_.c_str());
+ */
/* return; */
/* } */
- /* camera_to_fcu_translation = res.value().getTransform().transform.translation; */
+ /* vins_ang_velocity_mrs_world = res.value(); */
+ /* if (_rotate_velocity_) { */
+ /* Eigen::Vector3d v2; */
+ /* v2 << vins_ang_velocity.vector.x, vins_ang_velocity.vector.y, vins_ang_velocity.vector.z; */
+ /* v2 = rotation.transpose() * v2; */
+ /* vins_ang_velocity_mrs_world.vector.x = v2(0); */
+ /* vins_ang_velocity_mrs_world.vector.y = v2(1); */
+ /* vins_ang_velocity_mrs_world.vector.z = v2(2); */
+ /* } */
/* } */
/* //} */
- // save initial pose to subtract it from all messages to compensate initialized orientation ambiguity
- if (_init_in_zero_) {
- if (!got_init_pose_) {
- init_hdg_ = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).getHeading();
- ROS_INFO("[VinsRepublisher]: init hdg: %.2f", init_hdg_);
- got_init_pose_ = true;
- }
+ /* /1* geometry_msgs::Vector3 camera_to_fcu_translation; *1/ */
- // compensate initial heading ambiguity
- const double hdg = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).getHeading();
- vins_pose_mrs_world.pose.orientation = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).setHeading(hdg - init_hdg_);
- vins_pose_mrs_world.pose.position = rotatePointByHdg(vins_pose_mrs_world.pose.position, -init_hdg_);
- }
+ /* /1* /2* find transform from camera frame to fcu frame //{ *2/ *1/ */
+ /* /1* { *1/ */
+ /* /1* auto res = transformer_.getTransform(_camera_frame_, _fcu_frame_, odom->header.stamp); *1/ */
- // fill the new transformed odom message
- nav_msgs::Odometry odom_trans;
+ /* /1* if (!res) { *1/ */
- odom_trans.header.stamp = odom->header.stamp;
- odom_trans.header.frame_id = _mrs_vins_world_frame_;
+ /* /1* ROS_WARN_THROTTLE(1.0, "[%s]: could not find transform from '%s' to '%s'", ros::this_node::getName().c_str(), _camera_frame_.c_str(), *1/ */
+ /* /1* _fcu_frame_.c_str()); *1/ */
+ /* /1* return; *1/ */
+ /* /1* } *1/ */
- odom_trans.pose.pose = vins_pose_mrs_world.pose;
- odom_trans.twist.twist.linear = vins_velocity_mrs_world.vector;
- odom_trans.twist.twist.angular = vins_ang_velocity_mrs_world.vector;
+ /* /1* camera_to_fcu_translation = res.value().getTransform().transform.translation; *1/ */
+ /* /1* } *1/ */
- if (!validateOdometry(odom_trans)) {
- ROS_ERROR("[VinsRepublisher]: input odometry is not numerically valid");
- return;
- }
+ /* /1* //} *1/ */
- try {
- publisher_odom_.publish(odom_trans);
- ROS_INFO_THROTTLE(1.0, "[%s]: Publishing", ros::this_node::getName().c_str());
- publisher_odom_last_published_ = ros::Time::now();
- }
- catch (...) {
- ROS_ERROR("exception caught during publishing topic '%s'", publisher_odom_.getTopic().c_str());
- }
+ /* // save initial pose to subtract it from all messages to compensate initialized orientation ambiguity */
+ /* if (_init_in_zero_) { */
+ /* if (!got_init_pose_) { */
+ /* init_hdg_ = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).getHeading(); */
+ /* ROS_INFO("[VinsRepublisher]: init hdg: %.2f", init_hdg_); */
+ /* got_init_pose_ = true; */
+ /* } */
+
+ /* // compensate initial heading ambiguity */
+ /* const double hdg = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).getHeading(); */
+ /* vins_pose_mrs_world.pose.orientation = mrs_lib::AttitudeConverter(vins_pose_mrs_world.pose.orientation).setHeading(hdg - init_hdg_); */
+ /* vins_pose_mrs_world.pose.position = rotatePointByHdg(vins_pose_mrs_world.pose.position, -init_hdg_); */
+ /* } */
+
+
+ /* // TODO transform covariance */
+
+ /* // fill the new transformed odom message */
+ /* nav_msgs::Odometry odom_trans; */
+
+ /* odom_trans.header.stamp = odom->header.stamp; */
+ /* odom_trans.header.frame_id = _mrs_vins_world_frame_; */
+
+ /* odom_trans.pose.pose = vins_pose_mrs_world.pose; */
+ /* odom_trans.twist.twist.linear = vins_velocity_mrs_world.vector; */
+ /* odom_trans.twist.twist.angular = vins_ang_velocity_mrs_world.vector; */
+ /* odom_trans.pose.covariance = odom->pose.covariance; */
+ /* odom_trans.twist.covariance = odom->twist.covariance; */
+
+
+ /* if (!validateOdometry(odom_trans)) { */
+ /* ROS_ERROR("[VinsRepublisher]: input odometry is not numerically valid"); */
+ /* return; */
+ /* } */
+
+ /* try { */
+ /* publisher_odom_.publish(odom_trans); */
+ /* ROS_INFO_THROTTLE(1.0, "[%s]: Publishing", ros::this_node::getName().c_str()); */
+ /* publisher_odom_last_published_ = ros::Time::now(); */
+ /* } */
+ /* catch (...) { */
+ /* ROS_ERROR("exception caught during publishing topic '%s'", publisher_odom_.getTopic().c_str()); */
+ /* } */
}
//}