DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

remove file data_convert

Browse Source
This commit is contained in:
duongtd 2025-11-26 16:27:41 +07:00
parent 2e0a4348dd
commit 5ad8e83761
7 changed files with 25 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

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

2
include/costmap_2d/costmap_2d_robot.h
View File

@ -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>

159
include/costmap_2d/data_convert.h
View File

@ -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

7
plugins/directional_layer.cpp
View File

@ -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

5
plugins/static_layer.cpp
View File

@ -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)
{

16
src/costmap_2d_robot.cpp
View File

@ -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",
robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
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);
}

14
src/observation_buffer.cpp
View File

@ -1,7 +1,7 @@
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/data_convert.h>
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
// #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <data_convert/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_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