CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-07 18:14:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Rewrite fcu_horiz publisher in C++ as a nodelet

Browse Source
This commit is contained in:
Oleg Kalachev
2017-12-19 03:36:26 +03:00
parent e4fdf13094
commit 23b9bb8bea
6 changed files with 56 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
clever/src/fcu_horiz.cpp Normal file
View File

@@ -0,0 +1,31 @@
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
class FcuHoriz : public nodelet::Nodelet
{
geometry_msgs::TransformStamped t_;
void handlePose(const geometry_msgs::PoseStampedConstPtr& msg)
{
static tf2_ros::TransformBroadcaster br;
t_.header.stamp = msg->header.stamp;
t_.header.frame_id = msg->header.frame_id;
t_.transform.translation.x = msg->pose.position.x;
t_.transform.translation.y = msg->pose.position.y;
t_.transform.translation.z = msg->pose.position.z;
br.sendTransform(t_);
}
void onInit()
{
t_.child_frame_id = "fcu_horiz";
t_.transform.rotation.w = 1;
static ros::Subscriber pose_sub = getNodeHandle().subscribe("mavros/local_position/pose", 1, &FcuHoriz::handlePose, this);
ROS_INFO("fcu_horiz initialized");
}
};
PLUGINLIB_EXPORT_CLASS(FcuHoriz, nodelet::Nodelet)

20
clever/src/simple_offboard.py
View File

@@ -28,26 +28,6 @@ tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
def init_fcu_horiz():
# `fcu_horiz` frame publishing
tr = TransformStamped()
tr.header.frame_id = 'local_origin'
tr.child_frame_id = 'fcu_horiz'
def update_pose(data):
tr.header.stamp = data.header.stamp
tr.transform.translation = vector3_from_point(data.pose.position)
yaw = euler_from_orientation(data.pose.orientation)[2]
tr.transform.rotation = orientation_from_euler(0, 0, yaw)
tf_broadcaster.sendTransform(tr)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, update_pose)
init_fcu_horiz()
position_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=1)
attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
target_pub = rospy.Publisher('~target', PoseStamped, queue_size=1)