Rewrite fcu_horiz publisher in C++ as a nodelet

clever/CMakeLists.txt

@@ -2,12 +2,15 @@ cmake_minimum_required(VERSION 2.8.3)
 project(clever)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+add_compile_options(-std=c++11)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  nodelet
+  pluginlib
+  roscpp
   rospy
   std_msgs
   message_generation
@@ -131,13 +134,13 @@ catkin_package(
 ## Your package locations should be listed before other locations
 include_directories(
 # include
-# ${catkin_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/clever.cpp
-# )
+add_library(fcu_horiz
+  src/fcu_horiz.cpp
+)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
@@ -160,9 +163,10 @@ include_directories(
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
+target_link_libraries(fcu_horiz
+  ${catkin_LIBRARIES}
+  "/opt/ros/kinetic/lib/libtf2_ros.so"
+)
 
 #############
 ## Install ##

clever/launch/clever.launch

@@ -28,6 +28,8 @@
 
     <include file="$(find clever)/launch/aruco.launch" if="$(arg aruco)"/>
 
+    <node pkg="nodelet" type="nodelet" name="fcu_horiz" args="standalone clever/fcu_horiz" output="screen" clear_params="true"/>
+
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clever" type="simple_offboard.py" output="screen"/>
 

clever/nodelet_plugins.xml

@@ -0,0 +1,5 @@
+<library path="lib/libfcu_horiz">
+   <class name="clever/fcu_horiz" type="FcuHoriz" base_class_type="nodelet::Nodelet">
+      <description/>
+   </class>
+</library>

clever/package.xml

@@ -41,9 +41,15 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>nodelet</build_depend>
+  <build_depend>roscpp</build_depend>
+
+  <run_depend>nodelet</run_depend>
+  <run_depend>roscpp</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
+      <nodelet plugin="${prefix}/nodelet_plugins.xml" />
     <!-- Other tools can request additional information be placed here -->
 
   </export>

clever/src/fcu_horiz.cpp

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

clever/src/simple_offboard.py

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