CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-06 01:29: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)