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);