From 0fddf44dbaec002f03dddbaa03e19b0108b582fa Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 23 Jan 2019 15:25:57 +0000 Subject: [PATCH] local_position: add an aditional topic for velocity on the local frame --- mavros/src/plugins/local_position.cpp | 57 +++++++++++++++++---------- 1 file changed, 36 insertions(+), 21 deletions(-) diff --git a/mavros/src/plugins/local_position.cpp b/mavros/src/plugins/local_position.cpp index 1630ecc37..aae997e14 100644 --- a/mavros/src/plugins/local_position.cpp +++ b/mavros/src/plugins/local_position.cpp @@ -57,16 +57,17 @@ class LocalPositionPlugin : public plugin::PluginBase { local_position = lp_nh.advertise("pose", 10); local_position_cov = lp_nh.advertise("pose_cov", 10); - local_velocity = lp_nh.advertise("velocity", 10); - local_velocity_cov = lp_nh.advertise("velocity_cov", 10); + local_velocity_local = lp_nh.advertise("velocity_local", 10); + local_velocity_body = lp_nh.advertise("velocity_body", 10); + local_velocity_cov = lp_nh.advertise("velocity_body_cov", 10); local_accel = lp_nh.advertise("accel", 10); local_odom = lp_nh.advertise("odom",10); } Subscriptions get_subscriptions() { return { - make_handler(&LocalPositionPlugin::handle_local_position_ned), - make_handler(&LocalPositionPlugin::handle_local_position_ned_cov) + make_handler(&LocalPositionPlugin::handle_local_position_ned), + make_handler(&LocalPositionPlugin::handle_local_position_ned_cov) }; } @@ -75,7 +76,8 @@ class LocalPositionPlugin : public plugin::PluginBase { ros::Publisher local_position; ros::Publisher local_position_cov; - ros::Publisher local_velocity; + ros::Publisher local_velocity_local; + ros::Publisher local_velocity_body; ros::Publisher local_velocity_cov; ros::Publisher local_accel; ros::Publisher local_odom; @@ -139,12 +141,24 @@ class LocalPositionPlugin : public plugin::PluginBase { local_position.publish(pose); // publish velocity always - auto twist = boost::make_shared(); - twist->header.stamp = odom->header.stamp; - twist->header.frame_id = odom->child_frame_id; - twist->twist = odom->twist.twist; - local_velocity.publish(twist); - + // velocity in the body frame + auto twist_body = boost::make_shared(); + twist_body->header.stamp = odom->header.stamp; + twist_body->header.frame_id = tf_child_frame_id; + twist_body->twist.linear = odom->twist.twist.linear; + twist_body->twist.angular = baselink_angular_msg; + local_velocity_body.publish(twist_body); + + // velocity in the local frame + auto twist_local = boost::make_shared(); + twist_local->header.stamp = twist_body->header.stamp; + twist_local->header.frame_id = tf_child_frame_id; + tf::vectorEigenToMsg(enu_velocity, twist_local->twist.linear); + tf::vectorEigenToMsg(ftf::transform_frame_baselink_enu(ftf::to_eigen(baselink_angular_msg), enu_orientation), + twist_body->twist.angular); + local_velocity_local.publish(twist_local); + + // publish tf publish_tf(odom); } @@ -170,13 +184,13 @@ class LocalPositionPlugin : public plugin::PluginBase { tf::vectorEigenToMsg(baselink_linear, odom->twist.twist.linear); odom->twist.twist.angular = baselink_angular_msg; - odom->pose.covariance[0] = pos_ned.covariance[0]; // x - odom->pose.covariance[7] = pos_ned.covariance[9]; // y - odom->pose.covariance[14] = pos_ned.covariance[17]; // z + odom->pose.covariance[0] = pos_ned.covariance[0]; // x + odom->pose.covariance[7] = pos_ned.covariance[9]; // y + odom->pose.covariance[14] = pos_ned.covariance[17]; // z - odom->twist.covariance[0] = pos_ned.covariance[24]; // vx - odom->twist.covariance[7] = pos_ned.covariance[30]; // vy - odom->twist.covariance[14] = pos_ned.covariance[35]; // vz + odom->twist.covariance[0] = pos_ned.covariance[24]; // vx + odom->twist.covariance[7] = pos_ned.covariance[30]; // vy + odom->twist.covariance[14] = pos_ned.covariance[35]; // vz // TODO: orientation + angular velocity covariances from ATTITUDE_QUATERION_COV // publish odom always @@ -206,8 +220,9 @@ class LocalPositionPlugin : public plugin::PluginBase { twist->header.stamp = odom->header.stamp; twist->header.frame_id = odom->child_frame_id; twist->twist = odom->twist.twist; - local_velocity.publish(twist); + local_velocity_body.publish(twist); + // publish tf publish_tf(odom); } @@ -218,9 +233,9 @@ class LocalPositionPlugin : public plugin::PluginBase { auto enu_accel = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.ax, pos_ned.ay, pos_ned.az)); tf::vectorEigenToMsg(enu_accel, accel->accel.accel.linear); - accel->accel.covariance[0] = pos_ned.covariance[39]; // ax - accel->accel.covariance[7] = pos_ned.covariance[42]; // ay - accel->accel.covariance[14] = pos_ned.covariance[44]; // az + accel->accel.covariance[0] = pos_ned.covariance[39]; // ax + accel->accel.covariance[7] = pos_ned.covariance[42]; // ay + accel->accel.covariance[14] = pos_ned.covariance[44]; // az local_accel.publish(accel); }