From 07d0203290c0bdd97a21389db2bdf6d3b56077ef Mon Sep 17 00:00:00 2001
From: JaeyoungLim <jalim@ethz.ch>
Date: Sat, 27 Feb 2021 16:15:56 +0100
Subject: [PATCH] Include attitude reference in setpoints (#172)

Add support for publishing attitude setpoints
---
 .../include/geometric_controller/geometric_controller.h   | 2 +-
 geometric_controller/src/geometric_controller.cpp         | 8 ++++++--
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/geometric_controller/include/geometric_controller/geometric_controller.h b/geometric_controller/include/geometric_controller/geometric_controller.h
index 32c187c..64c57ff 100644
--- a/geometric_controller/include/geometric_controller/geometric_controller.h
+++ b/geometric_controller/include/geometric_controller/geometric_controller.h
@@ -145,7 +145,7 @@ class geometricCtrl {
   int posehistory_window_;
 
   void pubMotorCommands();
-  void pubRateCommands(const Eigen::Vector4d &cmd);
+  void pubRateCommands(const Eigen::Vector4d &cmd, const Eigen::Vector4d &target_attitude);
   void pubReferencePose(const Eigen::Vector3d &target_position, const Eigen::Vector4d &target_attitude);
   void pubPoseHistory();
   void pubSystemStatus();
diff --git a/geometric_controller/src/geometric_controller.cpp b/geometric_controller/src/geometric_controller.cpp
index 6f7047d..81dc754 100644
--- a/geometric_controller/src/geometric_controller.cpp
+++ b/geometric_controller/src/geometric_controller.cpp
@@ -233,7 +233,7 @@ void geometricCtrl::cmdloopCallback(const ros::TimerEvent &event) {
     case MISSION_EXECUTION:
       if (!feedthrough_enable_) computeBodyRateCmd(cmdBodyRate_, targetPos_, targetVel_, targetAcc_);
       pubReferencePose(targetPos_, q_des);
-      pubRateCommands(cmdBodyRate_);
+      pubRateCommands(cmdBodyRate_, q_des);
       appendPoseHistory();
       pubPoseHistory();
       break;
@@ -295,7 +295,7 @@ void geometricCtrl::pubReferencePose(const Eigen::Vector3d &target_position, con
   referencePosePub_.publish(msg);
 }
 
-void geometricCtrl::pubRateCommands(const Eigen::Vector4d &cmd) {
+void geometricCtrl::pubRateCommands(const Eigen::Vector4d &cmd, const Eigen::Vector4d &target_attitude) {
   mavros_msgs::AttitudeTarget msg;
 
   msg.header.stamp = ros::Time::now();
@@ -304,6 +304,10 @@ void geometricCtrl::pubRateCommands(const Eigen::Vector4d &cmd) {
   msg.body_rate.y = cmd(1);
   msg.body_rate.z = cmd(2);
   msg.type_mask = 128;  // Ignore orientation messages
+  msg.orientation.w = target_attitude(0);
+  msg.orientation.x = target_attitude(1);
+  msg.orientation.y = target_attitude(2);
+  msg.orientation.z = target_attitude(3);
   msg.thrust = cmd(3);
 
   angularVelPub_.publish(msg);