Merge pull request #75 from Jaeyoung-Lim/pr-heading

Add yaw subscriber

Author: Jaeyoung-Lim

geometric_controller/include/geometric_controller/geometric_controller.h

@@ -13,6 +13,7 @@
 #include <sstream>
 
 #include <Eigen/Dense>
+#include <std_msgs/Float32.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/TwistStamped.h>
@@ -45,6 +46,7 @@ class geometricCtrl
     ros::Subscriber mavstateSub_;
     ros::Subscriber mavposeSub_, gzmavposeSub_;
     ros::Subscriber mavtwistSub_;
+    ros::Subscriber yawreferenceSub_;
     ros::Publisher rotorVelPub_, angularVelPub_, target_pose_pub_;
     ros::Publisher referencePosePub_;
     ros::ServiceClient arming_client_;
@@ -91,6 +93,7 @@ class geometricCtrl
     void odomCallback(const nav_msgs::OdometryConstPtr& odomMsg);
     void targetCallback(const geometry_msgs::TwistStamped& msg);
     void flattargetCallback(const controller_msgs::FlatTarget& msg);
+    void yawtargetCallback(const std_msgs::Float32& msg);
     void multiDOFJointCallback(const trajectory_msgs::MultiDOFJointTrajectory& msg);
     void keyboardCallback(const geometry_msgs::Twist& msg);
     void cmdloopCallback(const ros::TimerEvent& event);

geometric_controller/src/geometric_controller.cpp

@@ -16,6 +16,7 @@ geometricCtrl::geometricCtrl(const ros::NodeHandle& nh, const ros::NodeHandle& n
 
   referenceSub_=nh_.subscribe("reference/setpoint",1, &geometricCtrl::targetCallback,this,ros::TransportHints().tcpNoDelay());
   flatreferenceSub_ = nh_.subscribe("reference/flatsetpoint", 1, &geometricCtrl::flattargetCallback, this, ros::TransportHints().tcpNoDelay());
+  yawreferenceSub_ = nh_.subscribe("reference/yaw", 1, &geometricCtrl::yawtargetCallback, this, ros::TransportHints().tcpNoDelay());
   multiDOFJointSub_ = nh_.subscribe("/command/trajectory", 1, &geometricCtrl::multiDOFJointCallback, this, ros::TransportHints().tcpNoDelay());
   mavstateSub_ = nh_.subscribe("/mavros/state", 1, &geometricCtrl::mavstateCallback, this,ros::TransportHints().tcpNoDelay());
   mavposeSub_ = nh_.subscribe("/mavros/local_position/pose", 1, &geometricCtrl::mavposeCallback, this,ros::TransportHints().tcpNoDelay());
@@ -120,6 +121,10 @@ void geometricCtrl::flattargetCallback(const controller_msgs::FlatTarget& msg) {
   }
 }
 
+void geometricCtrl::yawtargetCallback(const std_msgs::Float32& msg) {
+  if(!velocity_yaw_) mavYaw_ = double(msg.data);
+}
+
 void geometricCtrl::multiDOFJointCallback(const trajectory_msgs::MultiDOFJointTrajectory& msg) {
 
   trajectory_msgs::MultiDOFJointTrajectoryPoint pt = msg.points[0];