CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-05 09:09:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Move body frame publishing to simple_offboard.cpp

Browse Source
This commit is contained in:
Oleg Kalachev
2019-06-16 15:43:53 +03:00
parent 7610f02b38
commit 25f69596fc
4 changed files with 29 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
clever/src/frames.cpp
View File

@@ -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 <okalachev@gmail.com>
*
* 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 <string>
#include <memory>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
using std::string;
static std::shared_ptr<tf2_ros::TransformBroadcaster> 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<string>("body/frame_id", body.child_frame_id, "body");
br = std::make_shared<tf2_ros::TransformBroadcaster>();
ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &poseCallback);
ROS_INFO("frames: ready");
ros::spin();
}

32
clever/src/simple_offboard.cpp
View File

@@ -54,6 +54,7 @@ using mavros_msgs::Thrust;
// tf2
tf2_ros::Buffer tf_buffer;
std::shared_ptr<tf2_ros::TransformBroadcaster> 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<tf2_ros::TransformBroadcaster>();
// Params
nh.param<string>("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<string>("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<mavros_msgs::State, state>);
auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage<PoseStamped, local_position>);
auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);
// Setpoint publishers
position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);