From 9384e005e7ac7613cdfa446bc4995c3a8f51e4c0 Mon Sep 17 00:00:00 2001
From: sugikazu75 <sugihara@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 14 Apr 2024 01:46:04 +0900
Subject: [PATCH] [dragon] change desire coord interface for robot model to
 quaternion

---
 robots/dragon/include/dragon/dragon_navigation.h | 2 +-
 robots/dragon/src/dragon_navigation.cpp          | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/robots/dragon/include/dragon/dragon_navigation.h b/robots/dragon/include/dragon/dragon_navigation.h
index 92ed86255..fee02609b 100644
--- a/robots/dragon/include/dragon/dragon_navigation.h
+++ b/robots/dragon/include/dragon/dragon_navigation.h
@@ -38,6 +38,7 @@
 #include <aerial_robot_control/flight_navigation.h>
 #include <sensor_msgs/JointState.h>
 #include <spinal/DesireCoord.h>
+#include <geometry_msgs/Quaternion.h>
 
 namespace aerial_robot_navigation
 {
@@ -72,7 +73,6 @@ namespace aerial_robot_navigation
     void rosParamInit() override;
 
     void setFinalTargetBaselinkRotCallback(const spinal::DesireCoordConstPtr & msg);
-    void targetBaselinkRotCallback(const spinal::DesireCoordConstPtr& msg);
 
     /* target baselink rotation */
     double prev_rotation_stamp_;
diff --git a/robots/dragon/src/dragon_navigation.cpp b/robots/dragon/src/dragon_navigation.cpp
index d75a9538a..f04311883 100644
--- a/robots/dragon/src/dragon_navigation.cpp
+++ b/robots/dragon/src/dragon_navigation.cpp
@@ -21,7 +21,7 @@ void DragonNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
   /* initialize the flight control */
   BaseNavigator::initialize(nh, nhp, robot_model, estimator, loop_du);
 
-  curr_target_baselink_rot_pub_ = nh_.advertise<spinal::DesireCoord>("desire_coordinate", 1);
+  curr_target_baselink_rot_pub_ = nh_.advertise<geometry_msgs::Quaternion>("desire_coordinate", 1);
   joint_control_pub_ = nh_.advertise<sensor_msgs::JointState>("joints_ctrl", 1);
   final_target_baselink_rot_sub_ = nh_.subscribe("final_target_baselink_rot", 1, &DragonNavigator::setFinalTargetBaselinkRotCallback, this);
 
@@ -55,9 +55,9 @@ void DragonNavigator::baselinkRotationProcess()
       else
         curr_target_baselink_rot_ = final_target_baselink_rot_;
 
-      spinal::DesireCoord target_baselink_rot_msg;
-      target_baselink_rot_msg.roll = curr_target_baselink_rot_.x();
-      target_baselink_rot_msg.pitch = curr_target_baselink_rot_.y();
+      KDL::Rotation rot = KDL::Rotation::RPY(curr_target_baselink_rot_.x(), curr_target_baselink_rot_.y(), 0);
+      geometry_msgs::Quaternion target_baselink_rot_msg;
+      rot.GetQuaternion(target_baselink_rot_msg.x, target_baselink_rot_msg.y, target_baselink_rot_msg.z,  target_baselink_rot_msg.w);
       curr_target_baselink_rot_pub_.publish(target_baselink_rot_msg);
 
       prev_rotation_stamp_ = ros::Time::now().toSec();