Changed desire coordinate message to quaternion

aerial_robot_control/include/aerial_robot_control/control/under_actuated_tilted_lqi_controller.h

@@ -36,7 +36,7 @@
 #pragma once
 
 #include <aerial_robot_control/control/under_actuated_lqi_controller.h>
-#include <spinal/DesireCoord.h>
+#include <geometry_msgs/Quaternion.h>
 
 namespace aerial_robot_control
 {

aerial_robot_control/src/control/under_actuated_tilted_lqi_controller.cpp

@@ -46,7 +46,7 @@ void UnderActuatedTiltedLQIController::initialize(ros::NodeHandle nh,
 {
   UnderActuatedLQIController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate);
 
-  desired_baselink_rot_pub_ = nh_.advertise<spinal::DesireCoord>("desire_coordinate", 1);
+  desired_baselink_rot_pub_ = nh_.advertise<geometry_msgs::Quaternion>("desire_coordinate", 1);
 
   pid_msg_.z.p_term.resize(1);
   pid_msg_.z.i_term.resize(1);
@@ -154,12 +154,14 @@ void UnderActuatedTiltedLQIController::publishGain()
 {
   UnderActuatedLQIController::publishGain();
 
-  double roll,pitch, yaw;
-  robot_model_->getCogDesireOrientation<KDL::Rotation>().GetRPY(roll, pitch, yaw);
+  double qx, qy, qz, qw;
+  robot_model_->getCogDesireOrientation<KDL::Rotation>().GetQuaternion(qx, qy, qz, qw);
 
-  spinal::DesireCoord coord_msg;
-  coord_msg.roll = roll;
-  coord_msg.pitch = pitch;
+  geometry_msgs::Quaternion coord_msg;
+  coord_msg.x = qx;
+  coord_msg.y = qy;
+  coord_msg.z = qz;
+  coord_msg.w = qw;
   desired_baselink_rot_pub_.publish(coord_msg);
 }
 

aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model_ros.h

@@ -37,8 +37,8 @@
 
 #include <aerial_robot_model/model/aerial_robot_model.h>
 #include <aerial_robot_model/AddExtraModule.h>
+#include <geometry_msgs/Quaternion.h>
 #include <pluginlib/class_loader.h>
-#include <spinal/DesireCoord.h>
 #include <tf/tf.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/static_transform_broadcaster.h>
@@ -74,6 +74,6 @@ namespace aerial_robot_model {
     //private functions
     void jointStateCallback(const sensor_msgs::JointStateConstPtr& state);
     bool addExtraModuleCallback(aerial_robot_model::AddExtraModule::Request& req, aerial_robot_model::AddExtraModule::Response& res);
-    void desireCoordinateCallback(const spinal::DesireCoordConstPtr& msg);
+    void desireCoordinateCallback(const geometry_msgs::QuaternionConstPtr& msg);
   };
 } //namespace aerial_robot_model

aerial_robot_model/src/model/base_model/robot_model_ros.cpp

@@ -91,8 +91,9 @@ namespace aerial_robot_model {
     return false;
   }
 
-  void RobotModelRos::desireCoordinateCallback(const spinal::DesireCoordConstPtr& msg)
+  void RobotModelRos::desireCoordinateCallback(const geometry_msgs::QuaternionConstPtr& msg)
   {
-    robot_model_->setCogDesireOrientation(msg->roll, msg->pitch, msg->yaw);
+    KDL::Rotation rot = KDL::Rotation::Quaternion(msg->x, msg->y, msg->z, msg->w);
+    robot_model_->setCogDesireOrientation(rot);
   }
 } //namespace aerial_robot_model

aerial_robot_model/test/aerial_robot_model/numerical_jacobians.cpp

@@ -28,9 +28,10 @@ namespace aerial_robot_model {
     nhp_.param("feasible_control_torque_diff_thre", feasible_control_torque_diff_thre_, 0.001);
   }
 
-  void NumericalJacobian::desireCoordinateCallback(const spinal::DesireCoordConstPtr& msg)
+  void NumericalJacobian::desireCoordinateCallback(const geometry_msgs::QuaternionConstPtr& msg)
   {
-    getRobotModel().setCogDesireOrientation(msg->roll, msg->pitch, msg->yaw);
+    KDL::Rotation rot = KDL::Rotation::Quaternion(msg->x, msg->y, msg->z, msg->w);
+    getRobotModel().setCogDesireOrientation(rot);
   }
 
   void NumericalJacobian::jointStateCallback(const sensor_msgs::JointStateConstPtr& state)

aerial_robot_model/test/aerial_robot_model/numerical_jacobians.h

@@ -36,7 +36,7 @@
 #pragma once
 
 #include <aerial_robot_model/model/transformable_aerial_robot_model.h>
-#include <spinal/DesireCoord.h>
+#include <geometry_msgs/Quaternion.h>
 
 namespace aerial_robot_model  {
 
@@ -82,7 +82,7 @@ namespace aerial_robot_model  {
     aerial_robot_model::transformable::RobotModel& getRobotModel() const { return *robot_model_; }
 
     void jointStateCallback(const sensor_msgs::JointStateConstPtr& state);
-    void desireCoordinateCallback(const spinal::DesireCoordConstPtr& msg);
+    void desireCoordinateCallback(const geometry_msgs::QuaternionConstPtr& msg);
 
     //  numerical solution
     virtual const Eigen::MatrixXd thrustForceNumericalJacobian(std::vector<int> joint_indices);

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/attitude/attitude_estimate.h

@@ -25,10 +25,12 @@
 #include <spinal/Imu.h>
 #include <spinal/DesireCoord.h>
 #include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/Quaternion.h>
 
 /* sensors */
 #ifdef SIMULATION
 #include <tf/LinearMath/Matrix3x3.h>
+#include <tf/LinearMath/Quaternion.h>
 #else
 #include "sensors/imu/imu_mpu9250.h"
 #include "sensors/gps/gps_ublox.h"
@@ -251,7 +253,7 @@ class AttitudeEstimate
 #ifdef SIMULATION
   ros::Subscriber desire_coord_sub_;
 #else
-  ros::Subscriber<spinal::DesireCoord, AttitudeEstimate> desire_coord_sub_;
+  ros::Subscriber<geometry_msgs::Quaternion, AttitudeEstimate> desire_coord_sub_;
 #endif
 
   EstimatorAlgorithm* estimator_;
@@ -295,12 +297,32 @@ class AttitudeEstimate
 
   uint32_t last_imu_pub_time_, last_attitude_pub_time_;
 
-  void desireCoordCallback(const spinal::DesireCoord& coord_msg)
+  void desireCoordCallback(const geometry_msgs::Quaternion& msg)
   {
-    estimator_->coordinateUpdate(coord_msg.roll, coord_msg.pitch, coord_msg.yaw);
+    ap::Quaternion quat = ap::Quaternion(msg.w, msg.x, msg.y, msg.z);
+    estimator_->setCoordinateFromQuaternion(quat);
+
+    ap::Matrix3f desire_coord_r = estimator_->getDesiredCoord();
+
+    ap::Vector3f colx = desire_coord_r.colx();
+    ap::Vector3f coly = desire_coord_r.coly();
+    ap::Vector3f colz = desire_coord_r.colz();
+
+    // estimator_->coordinateUpdate(coord_msg.roll, coord_msg.pitch, coord_msg.yaw);
 #ifdef SIMULATION
     /* bypass to tf::Matrix3x3 */
-    tf_desired_coord_.setRPY(coord_msg.roll, coord_msg.pitch, coord_msg.yaw);
+    // tf_desired_coord_.setRPY(coord_msg.roll, coord_msg.pitch, coord_msg.yaw);
+
+    tf_desired_coord_[0][0] = colx[0];
+    tf_desired_coord_[1][0] = colx[1];
+    tf_desired_coord_[2][0] = colx[2];
+    tf_desired_coord_[0][1] = coly[0];
+    tf_desired_coord_[1][1] = coly[1];
+    tf_desired_coord_[2][1] = coly[2];
+    tf_desired_coord_[0][2] = colz[0];
+    tf_desired_coord_[1][2] = colz[1];
+    tf_desired_coord_[2][2] = colz[2];
+
 #endif
   }
 

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/state_estimate/attitude/estimator.h

@@ -69,6 +69,10 @@ class EstimatorAlgorithm
     r_.from_euler(desire_attitude_roll, desire_attitude_pitch, desire_attitude_yaw);
   }
 
+  void setCoordinateFromQuaternion(ap::Quaternion quat)
+  {
+    quat.rotation_matrix(r_);
+  }
 
   void update(const ap::Vector3f& gyro, const ap::Vector3f& acc, const ap::Vector3f& mag)
   {

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_;

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();