[aerial_robot_model] change desire coordinate message to quaternion i…

…n modeling
jsk-ros-pkg · Apr 13, 2024 · 019813c · 019813c
1 parent a35443d
commit 019813c
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 8 deletions.
diff --git a/aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model_ros.h b/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
diff --git a/aerial_robot_model/src/model/base_model/robot_model_ros.cpp b/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
diff --git a/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.cpp b/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)

diff --git a/aerial_robot_model/test/aerial_robot_model/numerical_jacobians.h b/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);