diff --git a/launch/vins_republisher_openvins_mavros.launch b/launch/vins_republisher_openvins_mavros.launch
index 29ddafb..62b78ff 100644
--- a/launch/vins_republisher_openvins_mavros.launch
+++ b/launch/vins_republisher_openvins_mavros.launch
@@ -29,10 +29,11 @@
-
+
-
+
+
diff --git a/src/VinsRepublisher.cpp b/src/VinsRepublisher.cpp
index aabf71a..93ccc0b 100644
--- a/src/VinsRepublisher.cpp
+++ b/src/VinsRepublisher.cpp
@@ -47,7 +47,6 @@ class VinsRepublisher : public nodelet::Nodelet {
/* ros parameters */
std::string _uav_name_;
bool _rotate_velocity_ = false;
- /* std::string _camera_frame_; */
std::string _fcu_frame_;
std::string _mrs_vins_world_frame_;
std::string _vins_fcu_frame_;
@@ -241,26 +240,10 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
ROS_DEBUG("[%s]: publishing", ros::this_node::getName().c_str());
- geometry_msgs::PoseStamped vins_pose;
- vins_pose.header = odom->header;
-
- vins_pose.pose = mrs_lib::getPose(odom);
-
- geometry_msgs::Vector3Stamped vins_velocity;
- vins_velocity.header = odom->header;
- vins_velocity.vector = odom->twist.twist.linear;
-
- geometry_msgs::Vector3Stamped vins_ang_velocity;
- vins_ang_velocity.header = odom->header;
- vins_ang_velocity.vector = odom->twist.twist.angular;
-
- geometry_msgs::TransformStamped tf;
-
nav_msgs::Odometry odom_transformed;
odom_transformed.header = odom->header;
odom_transformed.header.frame_id = _mrs_vins_world_frame_;
-
// get transformation from the VINS body frame (i.e. IMU frame) to the UAV FCU frame
auto res = transformer_.getTransform(_vins_fcu_frame_, _fcu_frame_, odom->header.stamp);
if (!res) {
@@ -269,8 +252,6 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
return;
}
- Eigen::Matrix3d rotation = mrs_lib::AttitudeConverter(res.value().transform.rotation); // TODO remove
-
// transform the pose
geometry_msgs::Pose pose_transformed;
tf2::doTransform(odom->pose.pose, pose_transformed, res.value());
@@ -321,32 +302,20 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
// 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; */
- v2 = rotation * v2;
- linear_velocity.x = v2(0);
- linear_velocity.y = v2(1);
- linear_velocity.z = v2(2);
+ tf2::doTransform(odom->twist.twist.linear, linear_velocity, res.value());
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; */
- v3 = rotation * 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
+ tf2::doTransform(odom->twist.twist.angular, angular_velocity, res.value());
+ // TODO if in global frame, apply initial TF offset
+
odom_transformed.twist.twist.linear = linear_velocity;
odom_transformed.twist.twist.angular = angular_velocity;
// 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...
+ /* tf2::transformCovariance( */
// publish
@@ -363,203 +332,10 @@ void VinsRepublisher::odometryCallback(const nav_msgs::OdometryConstPtr &odom) {
catch (...) {
ROS_ERROR("exception caught during publishing topic '%s'", publisher_odom_.getTopic().c_str());
}
-
-
- /* /1* get the transform from mrs_vins_world to vins_world //{ *1/ */
-
- /* /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/ */
-
- /* { */
- /* auto res = transformer_.getTransform(odom->header.frame_id, _mrs_vins_world_frame_, odom->header.stamp); */
-
- /* 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(); */
- /* } */
-
- /* //} */
-
- /* /1* transform the vins pose to mrs_world_frame //{ *1/ */
-
- /* geometry_msgs::PoseStamped vins_pose_mrs_world; */
-
- /* { */
- /* auto res = transformer_.transform(vins_pose, tf); */
-
- /* 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(); */
- /* } */
-
- /* Eigen::Matrix3d rotation; */
-
- /* { */
- /* // 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/ */
-
- /* { */
- /* /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); */
- /* } */
- /* } */
-
- /* //} */
-
- /* 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); */
-
- /* 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; */
- /* } */
-
- /* 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); */
- /* } */
- /* } */
-
- /* //} */
-
- /* /1* geometry_msgs::Vector3 camera_to_fcu_translation; *1/ */
-
- /* /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/ */
-
- /* /1* if (!res) { *1/ */
-
- /* /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/ */
-
- /* /1* camera_to_fcu_translation = res.value().getTransform().transform.translation; *1/ */
- /* /1* } *1/ */
-
- /* /1* //} *1/ */
-
- /* // 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()); */
- /* } */
}
//}
-/* //{ rotatePointByHdg() */
-geometry_msgs::Point VinsRepublisher::rotatePointByHdg(const geometry_msgs::Point &pt_in, const double hdg_in) const {
-
- const tf2::Quaternion q_hdg = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(hdg_in);
-
- const tf2::Vector3 vec_tf2(pt_in.x, pt_in.y, pt_in.z);
-
- const tf2::Vector3 vec_rotated = tf2::quatRotate(q_hdg, vec_tf2);
-
- geometry_msgs::Point pt_out;
- pt_out.x = vec_rotated.x();
- pt_out.y = vec_rotated.y();
- pt_out.z = vec_rotated.z();
-
- return pt_out;
-}
-//}
-
} // namespace vins_republisher
/* every nodelet must export its class as nodelet plugin */
PLUGINLIB_EXPORT_CLASS(vins_republisher::VinsRepublisher, nodelet::Nodelet);