From 25f69596fce5ba24fe90fde1a9f71a65e73f2570 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 16 Jun 2019 15:43:53 +0300 Subject: [PATCH] Move body frame publishing to simple_offboard.cpp --- clever/CMakeLists.txt | 4 --- clever/launch/clever.launch | 5 --- clever/src/frames.cpp | 63 ---------------------------------- clever/src/simple_offboard.cpp | 32 +++++++++++++++-- 4 files changed, 29 insertions(+), 75 deletions(-) delete mode 100644 clever/src/frames.cpp diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt index 4cbb44d6..4f9e3e31 100644 --- a/clever/CMakeLists.txt +++ b/clever/CMakeLists.txt @@ -162,8 +162,6 @@ add_executable(rc src/rc.cpp) add_executable(camera_markers src/camera_markers.cpp) -add_executable(frames src/frames.cpp) - add_executable(vpe_publisher src/vpe_publisher.cpp) target_link_libraries(simple_offboard @@ -175,8 +173,6 @@ target_link_libraries(rc ${catkin_LIBRARIES}) target_link_libraries(camera_markers ${catkin_LIBRARIES}) -target_link_libraries(frames ${catkin_LIBRARIES}) - target_link_libraries(vpe_publisher ${catkin_LIBRARIES}) add_dependencies(simple_offboard clever_generate_messages_cpp) diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch index bb32da6b..acf2db3d 100644 --- a/clever/launch/clever.launch +++ b/clever/launch/clever.launch @@ -46,11 +46,6 @@ - - - - - diff --git a/clever/src/frames.cpp b/clever/src/frames.cpp deleted file mode 100644 index f0d5a8b0..00000000 --- a/clever/src/frames.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Auxiliary TF frames for CLEVER drone kit: - * - Body frame (drone body with zero pitch and roll). - * - TODO: REP-0105 `odom` frame emulation: continuous frame without discrete jumps. - * - TODO: Terrain frame (base on ALTITUDE message). - * - TODO: map_upside_down frame - * - TODO: home frame? - * - * Copyright (C) 2018 Copter Express Technologies - * - * Author: Oleg Kalachev - * - * Distributed under MIT License (available at https://opensource.org/licenses/MIT). - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - */ - -// TODO: consider implementing as a mavros plugin - -#include -#include -#include -#include -#include -#include -#include - -using std::string; - -static std::shared_ptr br; -static geometry_msgs::TransformStamped body; - -inline void publishBody(const geometry_msgs::PoseStamped& pose) -{ - // Get only yaw from pose - tf::Quaternion q; - q.setRPY(0, 0, tf::getYaw(pose.pose.orientation)); - tf::quaternionTFToMsg(q, body.transform.rotation); - - body.transform.translation.x = pose.pose.position.x; - body.transform.translation.y = pose.pose.position.y; - body.transform.translation.z = pose.pose.position.z; - body.header.frame_id = pose.header.frame_id; - body.header.stamp = pose.header.stamp; - br->sendTransform(body); -} - -void poseCallback(const geometry_msgs::PoseStamped& pose) -{ - publishBody(pose); -} - -int main(int argc, char **argv) { - ros::init(argc, argv, "frames"); - ros::NodeHandle nh, nh_priv("~"); - - nh_priv.param("body/frame_id", body.child_frame_id, "body"); - - br = std::make_shared(); - ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &poseCallback); - ROS_INFO("frames: ready"); - ros::spin(); -} diff --git a/clever/src/simple_offboard.cpp b/clever/src/simple_offboard.cpp index a47b8ba9..a0042a84 100644 --- a/clever/src/simple_offboard.cpp +++ b/clever/src/simple_offboard.cpp @@ -54,6 +54,7 @@ using mavros_msgs::Thrust; // tf2 tf2_ros::Buffer tf_buffer; +std::shared_ptr transform_broadcaster; // Parameters string local_frame; @@ -88,6 +89,7 @@ AttitudeTarget att_raw_msg; Thrust thrust_msg; TwistStamped rates_msg; TransformStamped target; +geometry_msgs::TransformStamped body; // State PoseStamped nav_start; @@ -128,6 +130,29 @@ void handleMessage(const T& msg) STORAGE = msg; } +inline void publishBodyFrame() +{ + if (body.child_frame_id.empty()) return; + + tf::Quaternion q; + q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation)); + tf::quaternionTFToMsg(q, body.transform.rotation); + + body.transform.translation.x = local_position.pose.position.x; + body.transform.translation.y = local_position.pose.position.y; + body.transform.translation.z = local_position.pose.position.z; + body.header.frame_id = local_position.header.frame_id; + body.header.stamp = local_position.header.stamp; + transform_broadcaster->sendTransform(body); +} + +void handleLocalPosition(const PoseStamped& pose) +{ + local_position = pose; + publishBodyFrame(); + // TODO: terrain?, home? +} + // wait for transform without interrupting publishing setpoints inline bool waitTransform(const string& target, const string& source, const ros::Time& stamp, const ros::Duration& timeout) @@ -364,13 +389,12 @@ void publish(const ros::Time stamp) if (!target.child_frame_id.empty()) { if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { - static tf2_ros::TransformBroadcaster tf_broadcaster; target.header = setpoint_position_transformed.header; target.transform.translation.x = setpoint_position_transformed.pose.position.x; target.transform.translation.y = setpoint_position_transformed.pose.position.y; target.transform.translation.z = setpoint_position_transformed.pose.position.z; target.transform.rotation = setpoint_position_transformed.pose.orientation; - tf_broadcaster.sendTransform(target); + transform_broadcaster->sendTransform(target); } } @@ -689,6 +713,7 @@ int main(int argc, char **argv) ros::NodeHandle nh, nh_priv("~"); tf2_ros::TransformListener tf_listener(tf_buffer); + transform_broadcaster = std::make_shared(); // Params nh.param("mavros/local_position/tf/frame_id", local_frame, "map"); @@ -697,6 +722,7 @@ int main(int argc, char **argv) nh_priv.param("auto_release", auto_release, true); nh_priv.param("land_only_in_offboard", land_only_in_offboard, true); nh_priv.param("default_speed", default_speed, 0.5f); + nh_priv.param("body_frame", body.child_frame_id, "body"); nh_priv.getParam("reference_frames", reference_frames); state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0)); @@ -717,11 +743,11 @@ int main(int argc, char **argv) // Telemetry subscribers auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage); - auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage); auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage); auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage); auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage); auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage); + auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition); // Setpoint publishers position_pub = nh.advertise("mavros/setpoint_position/local", 1);