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