HiepLM update

CMakeLists.txt

@@ -5,6 +5,10 @@ set(CMAKE_CXX_STANDARD 17)
 
 add_library(data_convert INTERFACE)
 
+target_link_libraries(data_convert INTERFACE 
+    robot_geometry_msgs
+)
+
 target_include_directories(data_convert
     INTERFACE
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

include/data_convert/data_convert.h

@@ -1,10 +1,10 @@
 #ifndef COSTMAP_2D_DATA_CONVERT_H
 #define COSTMAP_2D_DATA_CONVERT_H
 
-#include <geometry_msgs/TransformStamped.h>
+#include <robot_geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/Quaternion.h>
+#include <robot_geometry_msgs/Quaternion.h>
-#include <geometry_msgs/Pose.h>
+#include <robot_geometry_msgs/Pose.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <tf3/utils.h>
 #include <tf3/compat.h>
 #include <robot/time.h>
@@ -12,16 +12,16 @@
 
 namespace data_convert
 {
-    inline double getYaw(const geometry_msgs::Quaternion& q)
+    inline double getYaw(const robot_geometry_msgs::Quaternion& q)
     {
         double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
         double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
         return std::atan2(siny_cosp, cosy_cosp);
     }
 
-    inline geometry_msgs::Quaternion getQuaternion(const double& yaw)
+    inline robot_geometry_msgs::Quaternion getQuaternion(const double& yaw)
     {
-        geometry_msgs::Quaternion q;
+        robot_geometry_msgs::Quaternion q;
         q.x = 0.0;
         q.y = 0.0;
         q.z = std::sin(yaw / 2.0);
@@ -45,15 +45,15 @@ namespace data_convert
         return time_tmp;
     }
 
-    inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q)
+    inline tf3::Quaternion convertQuaternion(const robot_geometry_msgs::Quaternion& q)
     {
         tf3::Quaternion out(q.x,q.y,q.z,q.w);
         return out;
     }
     
-    inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
+    inline robot_geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
     {
-        geometry_msgs::Quaternion out;
+        robot_geometry_msgs::Quaternion out;
         out.x = q.x();
         out.y = q.y();
         out.z = q.z();
@@ -61,12 +61,12 @@ namespace data_convert
         return out;
     }
 
-    inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(const double& yaw)
+    inline robot_geometry_msgs::Quaternion createQuaternionMsgFromYaw(const double& yaw)
     {
         tf3::Quaternion q;
         q.setRPY(0.0, 0.0, yaw);
 
-        geometry_msgs::Quaternion q_msg;
+        robot_geometry_msgs::Quaternion q_msg;
         q_msg.x = q.x();
         q_msg.y = q.y();
         q_msg.z = q.z();
@@ -75,7 +75,7 @@ namespace data_convert
         return q_msg;
     }
     
-    inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
+    inline tf3::Transform convertTotf3Transform(const robot_geometry_msgs::Transform& msg)
     {
         tf3::Quaternion q(
             msg.rotation.x,
@@ -134,7 +134,7 @@ namespace data_convert
         return out;
     }
 
-    inline tf3::Transform convertTotf3Transform(const geometry_msgs::TransformStamped& msg)
+    inline tf3::Transform convertTotf3Transform(const robot_geometry_msgs::TransformStamped& msg)
     {
         tf3::Transform out;
 
@@ -155,7 +155,7 @@ namespace data_convert
         return out;
     }
 
-    inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
+    inline tf3::TransformStampedMsg convertToTransformStampedMsg(const robot_geometry_msgs::TransformStamped& msg)
     {
         tf3::TransformStampedMsg out;
         out.header.seq = msg.header.seq;
@@ -172,9 +172,9 @@ namespace data_convert
         return out;
     }
     
-    inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
+    inline robot_geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
     {
-        geometry_msgs::TransformStamped out;
+        robot_geometry_msgs::TransformStamped out;
         out.header.seq = msg.header.seq;
         out.header.stamp = convertTime(msg.header.stamp);
         out.header.frame_id = msg.header.frame_id;