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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/robot_costmap_2d/costmap_2d_robot.h
View File

@ -55,18 +55,18 @@
#include <robot/node_handle.h>
#include <robot/plugin_loader_helper.h>
class SuperValue : public robot::XmlRpc::XmlRpcValue
class SuperValue : public robot_xmlrpcpp::XmlRpcValue
{
public:
void setStruct(robot::XmlRpc::XmlRpcValue::ValueStruct* a)
void setStruct(robot_xmlrpcpp::XmlRpcValue::ValueStruct* a)
{
_type = TypeStruct;
_value.asStruct = new robot::XmlRpc::XmlRpcValue::ValueStruct(*a);
_value.asStruct = new robot_xmlrpcpp::XmlRpcValue::ValueStruct(*a);
}
void setArray(robot::XmlRpc::XmlRpcValue::ValueArray* a)
void setArray(robot_xmlrpcpp::XmlRpcValue::ValueArray* a)
{
_type = TypeArray;
_value.asArray = new std::vector<robot::XmlRpc::XmlRpcValue>(*a);
_value.asArray = new std::vector<robot_xmlrpcpp::XmlRpcValue>(*a);
}
};

4
include/robot_costmap_2d/critical_layer.h
View File

@ -10,10 +10,6 @@ class CriticalLayer : public StaticLayer
public:
CriticalLayer();
virtual ~CriticalLayer();
LayerType getType() const override
{
return LayerType::CRITICAL_LAYER;
}
private:
unsigned char interpretValue(unsigned char value) override;
void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;

5
include/robot_costmap_2d/directional_layer.h
View File

@ -13,10 +13,7 @@ namespace robot_costmap_2d
virtual ~DirectionalLayer();
bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
void resetMap();
LayerType getType() const override
{
return LayerType::DIRECTIONAL_LAYER;
}
private:
void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);

2
include/robot_costmap_2d/footprint.h
View File

@ -136,7 +136,7 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandl
* @param full_param_name this is the full name of the rosparam from
* which the footprint_xmlrpc value came. It is used only for
* reporting errors. */
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);
// /** @brief Write the current unpadded_footprint_ to the "footprint"

5
include/robot_costmap_2d/inflation_layer.h
View File

@ -67,7 +67,6 @@ public:
unsigned int index_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};
class InflationLayer : public Layer
@ -121,10 +120,6 @@ public:
*/
void setInflationParameters(double inflation_radius, double cost_scaling_factor);
LayerType getType() const override
{
return LayerType::INFLATION_LAYER;
}
protected:
virtual void onFootprintChanged();
boost::recursive_mutex* inflation_access_;

19
include/robot_costmap_2d/layer.h
View File

@ -45,21 +45,6 @@
#include <robot/node_handle.h>
namespace robot_costmap_2d
{
enum class LayerType
{
UNKNOWN,
STATIC_LAYER,
OBSTACLE_LAYER,
INFLATION_LAYER,
CRITICAL_LAYER,
DIRECTIONAL_LAYER,
PREFERRED_LAYER,
UNPREFERRED_LAYER,
VOXEL_LAYER
};
class LayeredCostmap;
class Layer
@ -94,7 +79,7 @@ public:
virtual void reset() {}
virtual ~Layer() = default;
virtual ~Layer() {}
/**
* @brief Check to make sure all the data in the layer is up to date.
@ -145,8 +130,6 @@ public:
* notified of changes to the robot's footprint. */
virtual void onFootprintChanged() {}
virtual LayerType getType() const { return LayerType::UNKNOWN; }
template<typename T>
void dataCallBack(const T& value, const std::string& topic) {
handle(value, topic);

5
include/robot_costmap_2d/obstacle_layer.h
View File

@ -76,11 +76,6 @@ public:
void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
void clearStaticObservations(bool marking, bool clearing);
LayerType getType() const override
{
return LayerType::OBSTACLE_LAYER;
}
protected:
void handleImpl(const void* data,
const std::type_info&,

4
include/robot_costmap_2d/preferred_layer.h
View File

@ -10,10 +10,6 @@ class PreferredLayer : public StaticLayer
public:
PreferredLayer();
virtual ~PreferredLayer();
LayerType getType() const override
{
return LayerType::PREFERRED_LAYER;
}
private:
unsigned char interpretValue(unsigned char value);
};

6
include/robot_costmap_2d/static_layer.h
View File

@ -63,12 +63,6 @@ public:
virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
virtual void matchSize();
LayerType getType() const override
{
return LayerType::STATIC_LAYER;
}
protected:
void handleImpl(const void* data,
const std::type_info& type,

5
include/robot_costmap_2d/unpreferred_layer.h
View File

@ -10,10 +10,7 @@ class UnPreferredLayer : public StaticLayer
public:
UnPreferredLayer();
virtual ~UnPreferredLayer();
LayerType getType() const override
{
return LayerType::UNPREFERRED_LAYER;
}
private:
unsigned char interpretValue(unsigned char value);

5
include/robot_costmap_2d/voxel_layer.h
View File

@ -76,10 +76,7 @@ public:
}
virtual void matchSize();
virtual void reset();
LayerType getType() const override
{
return LayerType::VOXEL_LAYER;
}
protected:

2
plugins/static_layer.cpp
View File

@ -347,7 +347,7 @@ void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_
tf3::TransformStampedMsg transformMsg;
try
{
transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time());
transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
}
catch (tf3::TransformException ex)
{

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;