Skip to content

Commit

Permalink
velocities are now transformed correctly
Browse files Browse the repository at this point in the history
  • Loading branch information
@pritzvac
pritzvac committed Apr 19, 2024
1 parent 67b8fa5 commit b42edd3
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 231 deletions.
5 changes: 3 additions & 2 deletions launch/vins_republisher_openvins_mavros.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,10 +29,11 @@
<!-- ================= Static transformations ================= -->
<!-- TF between the VINS world frame and MRS VINS world frame -->
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_to_vins_origin" args="0.0 0.0 0.0 1.5708 0.0 0.0 $(arg vins_world_frame) $(arg mrs_vins_world_frame)" /> -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_to_vins_origin" args="0.0 0.0 0.0 3.1415926535 0.0 0.0 $(arg vins_world_frame) $(arg mrs_vins_world_frame)" />
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_to_vins_origin" args="0.0 0.0 0.0 3.1415926535 0.0 0.0 $(arg vins_world_frame) $(arg mrs_vins_world_frame)" /> -->

<!-- TF between the FCU of the UAV and the VINS body (IMU) frame -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_fcu_to_mrs_fcu" args="0.085 0.0 0.13 0 0.0 0 $(arg fcu_frame) $(arg vins_fcu_front_frame)" />
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_fcu_to_mrs_fcu" args="0.085 0.0 0.13 0 0.0 0 $(arg fcu_frame) $(arg vins_fcu_front_frame)" /> -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_fcu_to_mrs_fcu" args="0.0 0.0 0.0 0 0.0 0 $(arg fcu_frame) $(arg vins_fcu_front_frame)" />
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="tf_vins_fcu_to_mrs_fcu" args="0.1 0.0 -0.15 -1.5708 0.0 0.0 $(arg fcu_frame) $(arg vins_fcu_front_frame)" /> -->

<!-- TF for variable pitch of the camera -->
Expand Down
234 changes: 5 additions & 229 deletions src/VinsRepublisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down Expand Up @@ -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) {
Expand All @@ -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());
Expand Down Expand Up @@ -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
Expand All @@ -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);

0 comments on commit b42edd3

Please sign in to comment.