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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-31 15:43:07 +07:00
parent ae469e3271
commit b6733ae04c
14 changed files with 24 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/footprint.cpp
View File

@@ -249,25 +249,25 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandl
// nh.setParam("footprint", oss.str().c_str());
// }
double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
double getNumberFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& value, const std::string& full_param_name)
{
// Make sure that the value we're looking at is either a double or an int.
if (value.getType() != robot::XmlRpc::XmlRpcValue::TypeInt &&
value.getType() != robot::XmlRpc::XmlRpcValue::TypeDouble)
if (value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeInt &&
value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeDouble)
{
std::string& value_string = value;
printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
full_param_name.c_str(), value_string.c_str());
throw std::runtime_error("Values in the footprint specification must be numbers");
}
return value.getType() == robot::XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
return value.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
}
std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name)
{
// Make sure we have an array of at least 3 elements.
if (footprint_xmlrpc.getType() != robot::XmlRpc::XmlRpcValue::TypeArray ||
if (footprint_xmlrpc.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
footprint_xmlrpc.size() < 3)
{
printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
@@ -282,8 +282,8 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
for (int i = 0; i < footprint_xmlrpc.size(); ++i)
{
// Make sure each element of the list is an array of size 2. (x and y coordinates)
robot::XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
if (point.getType() != robot::XmlRpc::XmlRpcValue::TypeArray ||
robot_xmlrpcpp::XmlRpcValue point = footprint_xmlrpc[ i ];
if (point.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
point.size() != 2)
{
printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "

10
src/observation_buffer.cpp
View File

@@ -66,7 +66,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
std::string tf_error;
robot_geometry_msgs::TransformStamped transformStamped;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time(), &tf_error))
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
{
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
@@ -90,7 +90,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
new_global_frame, // frame đích
origin.header.frame_id, // frame nguồn
tf3::Time()
transform_time
);
tf3::doTransform(origin, origin, tfm_1);
obs.origin_ = origin.point;
@@ -100,7 +100,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
new_global_frame, // frame đích
obs.cloud_->header.frame_id, // frame nguồn
tf3::Time()
transform_time
);
tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
}
@@ -140,7 +140,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
local_origin.header.frame_id, // frame nguồn
tf3::Time()
data_convert::convertTime(local_origin.header.stamp)
);
tf3::doTransform(local_origin, global_origin, tfm_1);
@@ -161,7 +161,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
cloud.header.frame_id, // frame nguồn
tf3::Time()
data_convert::convertTime(cloud.header.stamp)
);
tf3::doTransform(cloud, global_frame_cloud, tfm_2);
global_frame_cloud.header.stamp = cloud.header.stamp;