From c118320e748cfcefd196d39894c2c69b4d0c9a53 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Wed, 28 Jun 2017 18:09:20 +0100 Subject: [PATCH] [WIP] Plugins: setpoint_attitude: add sync between thrust and attitude (#700) * plugins: setpoint_attitude: add sync between throttle and attitude topics to be sent together * plugins: typo correction: replace throttle with thrust * plugins: msgs: setpoint_attitude: replaces Float32Stamped for Thrust msg * plugins: setpoint_attitude: add sync between twist and thrust (RPY+Thrust) * setpoint_attitude: update the logic of thrust normalization verification * setpoint_attitude: implement sync between tf listener and thrust subscriber * TF sync listener: generalize topic type that can be syncronized with TF2 * TF2ListenerMixin: keep class template, use template for tf sync method only * TF2ListenerMixin: fix and improve sync tf2_start method * general update to yaml config files and parameters * setpoint_attitude: add note on Thrust sub name * setpoint_attitude: TF sync: pass subscriber pointer instead of binding it --- mavros/include/mavros/setpoint_mixin.h | 46 ++++- mavros/launch/apm_config.yaml | 20 +- mavros/launch/px4_config.yaml | 17 +- mavros/src/plugins/setpoint_accel.cpp | 12 +- mavros/src/plugins/setpoint_attitude.cpp | 189 +++++++++--------- mavros/src/plugins/setpoint_position.cpp | 9 +- mavros/src/plugins/setpoint_velocity.cpp | 2 +- .../src/plugins/vision_pose_estimate.cpp | 2 +- mavros_msgs/CMakeLists.txt | 1 + mavros_msgs/msg/Thrust.msg | 5 + 10 files changed, 184 insertions(+), 119 deletions(-) create mode 100644 mavros_msgs/msg/Thrust.msg diff --git a/mavros/include/mavros/setpoint_mixin.h b/mavros/include/mavros/setpoint_mixin.h index 8fee7b6c2..2a5f3d476 100644 --- a/mavros/include/mavros/setpoint_mixin.h +++ b/mavros/include/mavros/setpoint_mixin.h @@ -22,6 +22,10 @@ #include +#include "tf2_ros/message_filter.h" +#include + + namespace mavros { namespace plugin { /** @@ -122,7 +126,7 @@ class SetPositionTargetGlobalIntMixin { template class SetAttitudeTargetMixin { public: - //! Message sepecification: @p http://mavlink.org/messages/common#SET_ATTITIDE_TARGET + //! Message sepecification: @p http://mavlink.org/messages/common#SET_ATTITUDE_TARGET void set_attitude_target(uint32_t time_boot_ms, uint8_t type_mask, Eigen::Quaterniond orientation, @@ -200,6 +204,46 @@ class TF2ListenerMixin { } }); } + + /** + * @brief start tf listener syncronized with another topic + * + * @param _thd_name listener thread name + * @param cbp plugin callback function + */ + template + void tf2_start(const char *_thd_name, message_filters::Subscriber &topic_sub, void (D::*cbp)(const geometry_msgs::TransformStamped &, const typename T::ConstPtr &)) + { + tf_thd_name = _thd_name; + + tf_thread = std::thread([this, cbp, &topic_sub]() { + mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str()); + + mavros::UAS *m_uas_ = static_cast(this)->m_uas; + ros::NodeHandle &_sp_nh = static_cast(this)->sp_nh; + std::string &_frame_id = static_cast(this)->tf_frame_id; + std::string &_child_frame_id = static_cast(this)->tf_child_frame_id; + + tf2_ros::MessageFilter tf2_filter(topic_sub, m_uas_->tf2_buffer, _frame_id, 10, _sp_nh); + + ros::Rate rate(static_cast(this)->tf_rate); + while (ros::ok()) { + // Wait up to 3s for transform + if (m_uas_->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) { + try { + auto transform = m_uas_->tf2_buffer.lookupTransform( + _frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0)); + + tf2_filter.registerCallback(boost::bind(cbp, static_cast(this), transform, _1)); + } + catch (tf2::LookupException &ex) { + ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what()); + } + } + rate.sleep(); + } + }); + } }; } // namespace plugin } // namespace mavros diff --git a/mavros/launch/apm_config.yaml b/mavros/launch/apm_config.yaml index a77ca58b7..a5ec70598 100644 --- a/mavros/launch/apm_config.yaml +++ b/mavros/launch/apm_config.yaml @@ -86,19 +86,20 @@ setpoint_accel: # setpoint_attitude setpoint_attitude: - reverse_throttle: false # allow reversed throttle + reverse_thrust: false # allow reversed thrust + use_quaternion: false # enable PoseStamped topic subscriber tf: listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "attitude" - rate_limit: 10.0 + frame_id: "map" + child_frame_id: "target_attitude" + rate_limit: 50.0 # setpoint_position setpoint_position: tf: listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "setpoint" + frame_id: "map" + child_frame_id: "target_position" rate_limit: 50.0 # setpoint_velocity @@ -174,8 +175,8 @@ px4flow: vision_pose: tf: listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "vision" + frame_id: "map" + child_frame_id: "vision_estimate" rate_limit: 10.0 # vision_speed_estimate @@ -186,4 +187,7 @@ vision_speed: vibration: frame_id: "vibration" +# adsb +# None + # vim:set ts=2 sw=2 et: diff --git a/mavros/launch/px4_config.yaml b/mavros/launch/px4_config.yaml index ff159f924..299fe132d 100644 --- a/mavros/launch/px4_config.yaml +++ b/mavros/launch/px4_config.yaml @@ -88,19 +88,20 @@ setpoint_accel: # setpoint_attitude setpoint_attitude: - reverse_throttle: false # allow reversed throttle + reverse_thrust: false # allow reversed thrust + use_quaternion: false # enable PoseStamped topic subscriber tf: listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "attitude" - rate_limit: 10.0 + frame_id: "map" + child_frame_id: "target_attitude" + rate_limit: 50.0 # setpoint_position setpoint_position: tf: listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "setpoint" + frame_id: "map" + child_frame_id: "target_position" rate_limit: 50.0 # setpoint_velocity @@ -188,8 +189,8 @@ px4flow: vision_pose: tf: listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "vision" + frame_id: "map" + child_frame_id: "vision_estimate" rate_limit: 10.0 # vision_speed_estimate diff --git a/mavros/src/plugins/setpoint_accel.cpp b/mavros/src/plugins/setpoint_accel.cpp index d2e173992..be88bcd14 100644 --- a/mavros/src/plugins/setpoint_accel.cpp +++ b/mavros/src/plugins/setpoint_accel.cpp @@ -81,12 +81,12 @@ class SetpointAccelerationPlugin : public plugin::PluginBase, auto accel = ftf::transform_frame_enu_ned(accel_enu); set_position_target_local_ned(stamp.toNSec() / 1000000, - utils::enum_value(MAV_FRAME::LOCAL_NED), - ignore_all_except_a_xyz, - Eigen::Vector3d::Zero(), - Eigen::Vector3d::Zero(), - accel, - 0.0, 0.0); + utils::enum_value(MAV_FRAME::LOCAL_NED), + ignore_all_except_a_xyz, + Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), + accel, + 0.0, 0.0); } /* -*- callbacks -*- */ diff --git a/mavros/src/plugins/setpoint_attitude.cpp b/mavros/src/plugins/setpoint_attitude.cpp index 0d95a021a..de1160ed6 100644 --- a/mavros/src/plugins/setpoint_attitude.cpp +++ b/mavros/src/plugins/setpoint_attitude.cpp @@ -19,12 +19,20 @@ #include #include +#include +#include +#include + #include #include -#include +#include namespace mavros { namespace std_plugins { + +using SyncPoseThrust = message_filters::sync_policies::ApproximateTime; +using SyncTwistThrust = message_filters::sync_policies::ApproximateTime; + /** * @brief Setpoint attitude plugin * @@ -36,36 +44,53 @@ class SetpointAttitudePlugin : public plugin::PluginBase, public: SetpointAttitudePlugin() : PluginBase(), sp_nh("~setpoint_attitude"), - tf_rate(10.0), - reverse_throttle(false) + tf_rate(50.0), + use_quaternion(false), + reverse_thrust(false), + tf_listen(false) { } void initialize(UAS &uas_) { PluginBase::initialize(uas_); - bool tf_listen; - // main params - sp_nh.param("reverse_throttle", reverse_throttle, false); + sp_nh.param("use_quaternion", use_quaternion, false); + sp_nh.param("reverse_thrust", reverse_thrust, false); // tf params sp_nh.param("tf/listen", tf_listen, false); sp_nh.param("tf/frame_id", tf_frame_id, "map"); - sp_nh.param("tf/child_frame_id", tf_child_frame_id, "aircraft"); - sp_nh.param("tf/rate_limit", tf_rate, 10.0); + sp_nh.param("tf/child_frame_id", tf_child_frame_id, "target_attitude"); + sp_nh.param("tf/rate_limit", tf_rate, 50.0); + + // thrust msg subscriber to sync + message_filters::Subscriber th_sub(sp_nh, "thrust", 10); if (tf_listen) { ROS_INFO_STREAM_NAMED("attitude", - "Listen to desired attitude transform " - << tf_frame_id << " -> " << tf_child_frame_id); - tf2_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb); + "Listen to desired attitude transform " + << tf_frame_id << " -> " << tf_child_frame_id); + + tf2_start("AttitudeSpTFSync", th_sub, &SetpointAttitudePlugin::transform_cb); + } + else if (use_quaternion) { + /** + * @brief Use message_filters to sync attitude and thrust msg coming from different topics + */ + message_filters::Subscriber pose_sub(sp_nh, "target_attitude", 1); + + /** + * @brief Matches messages, even if they have different time stamps, + * by using an adaptative algorithm + */ + message_filters::Synchronizer sync(SyncPoseThrust(10), pose_sub, th_sub); + sync.registerCallback(boost::bind(&SetpointAttitudePlugin::attitude_pose_cb, this, _1, _2)); } else { - twist_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this); - pose_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this); + message_filters::Subscriber twist_sub(sp_nh, "cmd_vel", 1); + message_filters::Synchronizer sync(SyncTwistThrust(10), twist_sub, th_sub); + sync.registerCallback(boost::bind(&SetpointAttitudePlugin::attitude_twist_cb, this, _1, _2)); } - - throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this); } Subscriptions get_subscriptions() @@ -78,125 +103,109 @@ class SetpointAttitudePlugin : public plugin::PluginBase, friend class TF2ListenerMixin; ros::NodeHandle sp_nh; - ros::Subscriber twist_sub; - ros::Subscriber pose_sub; - ros::Subscriber throttle_sub; - std::string tf_frame_id; std::string tf_child_frame_id; + + bool tf_listen; double tf_rate; - bool reverse_throttle; + + bool use_quaternion; + + bool reverse_thrust; + float normalized_thrust; + + /** + * @brief Function to verify if the thrust values are normalized; + * considers also the reversed trust values + */ + inline bool is_normalized(float thrust){ + if (reverse_thrust) { + if (thrust < -1.0) { + ROS_WARN_NAMED("attitude", "Not normalized reversed thrust! Thd(%f) < Min(%f)", thrust, -1.0); + return false; + } + } + else { + if (thrust < 0.0) { + ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) < Min(%f)", thrust, 0.0); + return false; + } + } + + if (thrust > 1.0) { + ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) > Max(%f)", thrust, 1.0); + return false; + } + return true; + } /* -*- mid-level helpers -*- */ /** - * @brief Send attitude setpoint to FCU attitude controller - * - * @note ENU frame. + * @brief Send attitude setpoint and thrust to FCU attitude controller */ - void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) + void send_attitude_quaternion(const ros::Time &stamp, const Eigen::Affine3d &tr, const float thrust) { - /* Thrust + RPY, also bits numbering started from 1 in docs + /** + * @note RPY, also bits numbering started from 1 in docs */ - const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0); + const uint8_t ignore_all_except_q_and_thrust = (7 << 0); auto q = ftf::transform_orientation_enu_ned( ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())) ); set_attitude_target(stamp.toNSec() / 1000000, - ignore_all_except_q, - q, - Eigen::Vector3d::Zero(), - 0.0); + ignore_all_except_q_and_thrust, + q, + Eigen::Vector3d::Zero(), + thrust); } /** - * @brief Send angular velocity setpoint to FCU attitude controller - * - * @note ENU frame. + * @brief Send angular velocity setpoint and thrust to FCU attitude controller */ - void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel) + void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel, const float thrust) { - /* Q + Thrust, also bits noumbering started from 1 in docs + /** + * @note Q, also bits noumbering started from 1 in docs */ - const uint8_t ignore_all_except_rpy = (1 << 7) | (1 << 6); + const uint8_t ignore_all_except_rpy = (1 << 7); auto av = ftf::transform_frame_baselink_aircraft(ang_vel); set_attitude_target(stamp.toNSec() / 1000000, - ignore_all_except_rpy, - Eigen::Quaterniond::Identity(), - av, - 0.0); - } - - /** - * @brief Send throttle to FCU attitude controller - */ - void send_attitude_throttle(const float throttle) - { - // Q + RPY - const uint8_t ignore_all_except_throttle = (1 << 7) | (7 << 0); - - set_attitude_target(ros::Time::now().toNSec() / 1000000, - ignore_all_except_throttle, - Eigen::Quaterniond::Identity(), - Eigen::Vector3d::Zero(), - throttle); + ignore_all_except_rpy, + Eigen::Quaterniond::Identity(), + av, + thrust); } /* -*- callbacks -*- */ - void transform_cb(const geometry_msgs::TransformStamped &transform) { + void transform_cb(const geometry_msgs::TransformStamped &transform, const mavros_msgs::Thrust::ConstPtr &thrust_msg) { Eigen::Affine3d tr; tf::transformMsgToEigen(transform.transform, tr); - send_attitude_target(transform.header.stamp, tr); + send_attitude_quaternion(transform.header.stamp, tr, thrust_msg->thrust); } - void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req) { + void attitude_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose_msg, const mavros_msgs::Thrust::ConstPtr &thrust_msg) { Eigen::Affine3d tr; - tf::poseMsgToEigen(req->pose, tr); + tf::poseMsgToEigen(pose_msg->pose, tr); - send_attitude_target(req->header.stamp, tr); + if (is_normalized(thrust_msg->thrust)) + send_attitude_quaternion(pose_msg->header.stamp, tr, thrust_msg->thrust); } - void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) { + void attitude_twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req, const mavros_msgs::Thrust::ConstPtr &thrust_msg) { Eigen::Vector3d ang_vel; tf::vectorMsgToEigen(req->twist.angular, ang_vel); - send_attitude_ang_velocity(req->header.stamp, ang_vel); - } - - inline bool is_normalized(float throttle, const float min, const float max) - { - if (throttle < min) { - ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) < Min(%f)", throttle, min); - return false; - } - else if (throttle > max) { - ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) > Max(%f)", throttle, max); - return false; - } - - return true; + if (is_normalized(thrust_msg->thrust)) + send_attitude_ang_velocity(req->header.stamp, ang_vel, thrust_msg->thrust); } - void throttle_cb(const std_msgs::Float64::ConstPtr &req) - { - float throttle_normalized = req->data; - - /** - * && are lazy, is_normalized() should be called only if reverse_throttle are true. - */ - if (reverse_throttle && !is_normalized(throttle_normalized, -1.0, 1.0)) - return; - else if (!is_normalized(throttle_normalized, 0.0, 1.0)) - return; - - send_attitude_throttle(throttle_normalized); - } }; } // namespace std_plugins } // namespace mavros diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index 83e4020c0..35d76c870 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -33,19 +33,18 @@ class SetpointPositionPlugin : public plugin::PluginBase, public: SetpointPositionPlugin() : PluginBase(), sp_nh("~setpoint_position"), - tf_rate(10.0) + tf_rate(50.0), + tf_listen(false) { } void initialize(UAS &uas_) { PluginBase::initialize(uas_); - bool tf_listen; - // tf params sp_nh.param("tf/listen", tf_listen, false); sp_nh.param("tf/frame_id", tf_frame_id, "map"); - sp_nh.param("tf/child_frame_id", tf_child_frame_id, "aircraft"); + sp_nh.param("tf/child_frame_id", tf_child_frame_id, "target_position"); sp_nh.param("tf/rate_limit", tf_rate, 50.0); if (tf_listen) { @@ -72,6 +71,8 @@ class SetpointPositionPlugin : public plugin::PluginBase, std::string tf_frame_id; std::string tf_child_frame_id; + + bool tf_listen; double tf_rate; /* -*- mid-level helpers -*- */ diff --git a/mavros/src/plugins/setpoint_velocity.cpp b/mavros/src/plugins/setpoint_velocity.cpp index fc16a22d6..a9eeade62 100644 --- a/mavros/src/plugins/setpoint_velocity.cpp +++ b/mavros/src/plugins/setpoint_velocity.cpp @@ -95,7 +95,7 @@ class SetpointVelocityPlugin : public plugin::PluginBase, send_setpoint_velocity(req->header.stamp, vel_enu, req->twist.angular.z); } - + void vel_unstamped_cb(const geometry_msgs::Twist::ConstPtr &req) { Eigen::Vector3d vel_enu; diff --git a/mavros_extras/src/plugins/vision_pose_estimate.cpp b/mavros_extras/src/plugins/vision_pose_estimate.cpp index 8d392f23e..3a0d93c16 100644 --- a/mavros_extras/src/plugins/vision_pose_estimate.cpp +++ b/mavros_extras/src/plugins/vision_pose_estimate.cpp @@ -48,7 +48,7 @@ class VisionPoseEstimatePlugin : public plugin::PluginBase, // tf params sp_nh.param("tf/listen", tf_listen, false); sp_nh.param("tf/frame_id", tf_frame_id, "map"); - sp_nh.param("tf/child_frame_id", tf_child_frame_id, "vision"); + sp_nh.param("tf/child_frame_id", tf_child_frame_id, "vision_estimate"); sp_nh.param("tf/rate_limit", tf_rate, 50.0); if (tf_listen) { diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index 79713e026..006f92046 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -31,6 +31,7 @@ add_message_files( RCOut.msg RadioStatus.msg State.msg + Thrust.msg VFR_HUD.msg Vibration.msg Waypoint.msg diff --git a/mavros_msgs/msg/Thrust.msg b/mavros_msgs/msg/Thrust.msg new file mode 100644 index 000000000..fa2fcd678 --- /dev/null +++ b/mavros_msgs/msg/Thrust.msg @@ -0,0 +1,5 @@ +# Thrust to send to the FCU + +std_msgs/Header header + +float32 thrust