remove file data_convert

CMakeLists.txt

@@ -71,6 +71,8 @@ target_link_libraries(costmap_2d
     voxel_grid
     tf3
     tf3_geometry_msgs
+    tf3_sensor_msgs
+    data_convert
     xmlrpcpp                  # XMLRPC
 )
 

include/costmap_2d/costmap_2d_robot.h

@@ -48,7 +48,7 @@
 
 #include <tf3/LinearMath/Transform.h>
 #include <robot/rate.h>
-#include <costmap_2d/data_convert.h>
+#include <data_convert/data_convert.h>
 #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <xmlrpcpp/XmlRpcValue.h>

include/costmap_2d/data_convert.h

@@ -1,159 +0,0 @@
-#ifndef COSTMAP_2D_DATA_CONVERT_H
-#define COSTMAP_2D_DATA_CONVERT_H
-
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/Quaternion.h>
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <tf3/utils.h>
-#include <tf3/compat.h>
-#include <robot/time.h>
-#include <cmath>
-
-namespace costmap_2d
-{
-    template <typename T>
-    bool isEqual(const T& a, const T& b, double eps = 1e-5)
-    {
-        return std::fabs(a - b) < eps;
-    }
-
-    inline bool operator==(const geometry_msgs::Quaternion& a, const geometry_msgs::Quaternion& b)
-    {
-        return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z) && isEqual(a.w, b.w);
-    }
-
-    inline bool operator==(const geometry_msgs::Point& a, const geometry_msgs::Point& b)
-    {
-        return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z);
-    }
-
-    inline bool operator==(const geometry_msgs::Pose& a, const geometry_msgs::Pose& b)
-    {
-        return (a.position == b.position) && (a.orientation == b.orientation);
-    }
-
-    inline bool operator==(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b)
-    {
-        return (a.header.frame_id == b.header.frame_id) &&
-            (a.pose == b.pose);
-    }
-
-    inline double getYaw(const 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)
-    {
-        geometry_msgs::Quaternion q;
-        q.x = 0.0;
-        q.y = 0.0;
-        q.z = std::sin(yaw / 2.0);
-        q.w = std::cos(yaw / 2.0);
-        return q;
-    }
-
-    inline robot::Time convertTime(tf3::Time time)
-    {
-        robot::Time time_tmp;
-        time_tmp.sec = time.sec;
-        time_tmp.nsec = time.nsec;
-        return time_tmp;
-    }
-
-    inline tf3::Time convertTime(robot::Time time)
-    {
-        tf3::Time time_tmp;
-        time_tmp.sec = time.sec;
-        time_tmp.nsec = time.nsec;
-        return time_tmp;
-    }
-
-    inline tf3::Quaternion convertQuaternion(const 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)
-    {
-        return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
-    }
-    
-    inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
-    {
-        tf3::Quaternion q(
-            msg.rotation.x,
-            msg.rotation.y,
-            msg.rotation.z,
-            msg.rotation.w
-        );
-
-        tf3::Vector3 t(
-            msg.translation.x,
-            msg.translation.y,
-            msg.translation.z
-        );
-
-        tf3::Transform tf(q, t);
-        return tf;
-    }
-
-    inline tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
-    {
-        tf3::Quaternion q(
-            msg.rotation.x,
-            msg.rotation.y,
-            msg.rotation.z,
-            msg.rotation.w
-        );
-
-        tf3::Vector3 t(
-            msg.translation.x,
-            msg.translation.y,
-            msg.translation.z
-        );
-
-        tf3::Transform tf(q, t);
-        return tf;
-    }
-
-    inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
-    {
-        tf3::TransformStampedMsg out;
-        out.header.seq = msg.header.seq;
-        out.header.stamp = convertTime(msg.header.stamp);
-        out.header.frame_id = msg.header.frame_id;
-        out.child_frame_id = msg.child_frame_id;
-        out.transform.translation.x = msg.transform.translation.x;
-        out.transform.translation.y = msg.transform.translation.y;
-        out.transform.translation.z = msg.transform.translation.z;
-        out.transform.rotation.x = msg.transform.rotation.x; 
-        out.transform.rotation.y = msg.transform.rotation.y; 
-        out.transform.rotation.z = msg.transform.rotation.z; 
-        out.transform.rotation.w = msg.transform.rotation.w; 
-        return out;
-    }
-    
-    inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
-    {
-        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;
-        out.child_frame_id = msg.child_frame_id;
-        out.transform.translation.x = msg.transform.translation.x;
-        out.transform.translation.y = msg.transform.translation.y;
-        out.transform.translation.z = msg.transform.translation.z;
-        out.transform.rotation.x = msg.transform.rotation.x; 
-        out.transform.rotation.y = msg.transform.rotation.y; 
-        out.transform.rotation.z = msg.transform.rotation.z; 
-        out.transform.rotation.w = msg.transform.rotation.w; 
-        return out;
-    }
-}
-
-#endif // DATA_CONVERT_H

plugins/directional_layer.cpp

@@ -1,7 +1,8 @@
 #include <costmap_2d/directional_layer.h>
-#include <costmap_2d/data_convert.h>
+#include <data_convert/data_convert.h>
 #include <tf3/convert.h>
 #include <tf3/utils.h>
+#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <boost/dll/alias.hpp>
 
@@ -134,7 +135,7 @@ namespace costmap_2d
         return false;
       }
       // Convert to yaw
-      tf3::Quaternion quaternion = convertQuaternion(p.pose.orientation);
+      tf3::Quaternion quaternion = data_convert::convertQuaternion(p.pose.orientation);
       double theta = tf3::getYaw(quaternion);
       unsigned char value = laneFilter(mx, my, theta);
       if (get_success)
@@ -413,7 +414,7 @@ namespace costmap_2d
     }
     // Copy map data given proper transformations
     tf3::Transform tf3_transform;
-    tf3_transform = convertTotf3Transform(transformMsg.transform);
+    tf3_transform = data_convert::convertTotf3Transform(transformMsg.transform);
     x = tf3_transform.getOrigin().x();
     y = tf3_transform.getOrigin().y();
     // Extract the rotation as a quaternion from the transform

plugins/static_layer.cpp

@@ -1,7 +1,7 @@
 
 #include <costmap_2d/static_layer.h>
 #include <costmap_2d/costmap_math.h>
-#include <costmap_2d/data_convert.h>
+#include <data_convert/data_convert.h>
 #include <costmap_2d/utils.h>
 #include <tf3/convert.h>
 #include <tf3/utils.h>
@@ -9,6 +9,7 @@
 #include <filesystem>
 #include <string>
 #include <iostream>
+#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 using costmap_2d::NO_INFORMATION;
 using costmap_2d::LETHAL_OBSTACLE;
@@ -315,7 +316,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
     }
     // Copy map data given proper transformations
     tf3::Transform tf3_transform;
-    tf3_transform = convertTotf3Transform(transformMsg.transform);
+    tf3_transform = data_convert::convertTotf3Transform(transformMsg.transform);
     // tf3::convert(transformMsg.transform, tf3_transform);
     for (unsigned int i = min_i; i < max_i; ++i)
     {

src/costmap_2d_robot.cpp

@@ -84,7 +84,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
     }
     else
     {
-      std::cout << "/cfg folder not found!" << std::endl;
+      std::cout<< config_file_name << " file not found!" << std::endl;
     }
 
     
@@ -102,8 +102,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
     {
       if (last_error + robot::Duration(5.0) < robot::Time::now())
       {
-        printf("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
+        printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
-                robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
+                robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
         last_error = robot::Time::now();
       }
       // The error string will accumulate and errors will typically be the same, so the last
@@ -293,7 +293,7 @@ void Costmap2DROBOT::updateMap()
     {
       double x = pose.pose.position.x,
              y = pose.pose.position.y,
-             yaw = getYaw(pose.pose.orientation);
+             yaw = data_convert::getYaw(pose.pose.orientation);
       layered_costmap_->updateMap(x, y, yaw);
       geometry_msgs::PolygonStamped footprint;
       footprint.header.frame_id = global_frame_;
@@ -408,9 +408,9 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
   try
   {
     // use current time if possible (makes sure it's not in the future)
-    if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time)))
+    if (tf_.canTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time)))
     {
-      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time));
+      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time));
       tf3::doTransform(robot_pose, global_pose, transform);
     }
     // use the latest otherwise
@@ -420,7 +420,7 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
       tf3::TransformStampedMsg transform = tf_.lookupTransform(
                     global_frame_,                      // frame đích
                     robot_base_frame_,                      // frame nguồn
-                    tf3::convertTime(robot_pose.header.stamp)
+                    data_convert::convertTime(robot_pose.header.stamp)
                 );
       tf3::doTransform(robot_pose, global_pose, transform);
     }
@@ -460,7 +460,7 @@ void Costmap2DROBOT::getOrientedFootprint(std::vector<geometry_msgs::Point>& ori
   if (!getRobotPose(global_pose))
     return;
 
-  double yaw = getYaw(global_pose.pose.orientation);
+  double yaw = data_convert::getYaw(global_pose.pose.orientation);
   transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
                      padded_footprint_, oriented_footprint);
 }

src/observation_buffer.cpp

@@ -1,7 +1,7 @@
 #include <costmap_2d/observation_buffer.h>
-#include <costmap_2d/data_convert.h>
+#include <data_convert/data_convert.h>
-// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
-// #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
+#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
 #include<tf3/convert.h>
 #include <sensor_msgs/point_cloud2_iterator.h>
 #include <cstdint>
@@ -59,14 +59,14 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
       tf3::doTransform(origin, origin, 
         tf3_buffer_.lookupTransform(new_global_frame, 
                         origin.header.frame_id,
-                        convertTime(origin.header.stamp)));
+                        data_convert::convertTime(origin.header.stamp)));
       obs.origin_ = origin.point;
 
       // we also need to transform the cloud of the observation to the new global frame
       tf3::doTransform(*(obs.cloud_), *(obs.cloud_), 
         tf3_buffer_.lookupTransform(new_global_frame, 
                         obs.cloud_->header.frame_id,
-                        convertTime(obs.cloud_->header.stamp)));
+                        data_convert::convertTime(obs.cloud_->header.stamp)));
     }
     catch (TransformException& ex)
     {
@@ -103,7 +103,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
     tf3::doTransform(local_origin, global_origin, 
         tf3_buffer_.lookupTransform(global_frame_, 
                         local_origin.header.frame_id,
-                        convertTime(local_origin.header.stamp)));
+                        data_convert::convertTime(local_origin.header.stamp)));
     tf3::convert(global_origin.point, observation_list_.front().origin_);
 
     // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
@@ -116,7 +116,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
     tf3::doTransform(cloud, global_frame_cloud, 
         tf3_buffer_.lookupTransform(global_frame_, 
                         (cloud.header.frame_id),
-                        convertTime(cloud.header.stamp)));
+                        data_convert::convertTime(cloud.header.stamp)));
     global_frame_cloud.header.stamp = cloud.header.stamp;
 
     // now we need to remove observations from the cloud that are below or above our height thresholds