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

HiepLM update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:08:14 +07:00
parent 71adf1390f
commit 2c3d7d586d
23 changed files with 117 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/costmap_2d/costmap_2d.h
View File

@@ -40,7 +40,7 @@
#include <vector>
#include <queue>
#include <geometry_msgs/Point.h>
#include <robot_geometry_msgs/Point.h>
#include <boost/thread.hpp>
namespace costmap_2d
@@ -249,7 +249,7 @@ public:
* @param cost_value The value to set costs to
* @return True if the polygon was filled... false if it could not be filled
*/
bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
bool setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value);
/**
* @brief Get the map cells that make up the outline of a polygon

30
include/costmap_2d/costmap_2d_robot.h
View File

@@ -42,9 +42,9 @@
#include <costmap_2d/layer.h>
#include <costmap_2d/footprint.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <tf3/LinearMath/Transform.h>
#include <robot/rate.h>
@@ -135,7 +135,7 @@ public:
* @param global_pose Will be set to the pose of the robot in the global frame of the costmap
* @return True if the pose was set successfully, false otherwise
*/
bool getRobotPose(geometry_msgs::PoseStamped& global_pose) const;
bool getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) const;
/** @brief Returns costmap name */
inline const std::string& getName() const noexcept
@@ -179,8 +179,8 @@ public:
return layered_costmap_;
}
/** @brief Returns the current padded footprint as a geometry_msgs::Polygon. */
geometry_msgs::Polygon getRobotFootprintPolygon() const
/** @brief Returns the current padded footprint as a robot_geometry_msgs::Polygon. */
robot_geometry_msgs::Polygon getRobotFootprintPolygon() const
{
return costmap_2d::toPolygon(padded_footprint_);
}
@@ -193,7 +193,7 @@ public:
* The footprint initially comes from the rosparam "footprint" but
* can be overwritten by dynamic reconfigure or by messages received
* on the "footprint" topic. */
inline const std::vector<geometry_msgs::Point>& getRobotFootprint() const noexcept
inline const std::vector<robot_geometry_msgs::Point>& getRobotFootprint() const noexcept
{
return padded_footprint_;
}
@@ -205,7 +205,7 @@ public:
* The footprint initially comes from the rosparam "footprint" but
* can be overwritten by dynamic reconfigure or by messages received
* on the "footprint" topic. */
inline const std::vector<geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
inline const std::vector<robot_geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
{
return unpadded_footprint_;
}
@@ -214,7 +214,7 @@ public:
* @brief Build the oriented footprint of the robot at the robot's current pose
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const;
void getOrientedFootprint(std::vector<robot_geometry_msgs::Point>& oriented_footprint) const;
/** @brief Set the footprint of the robot to be the given set of
* points, padded by footprint_padding.
@@ -226,7 +226,7 @@ public:
* layered_costmap_->setFootprint(). Also saves the unpadded
* footprint, which is available from
* getUnpaddedRobotFootprint(). */
void setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points);
void setUnpaddedRobotFootprint(const std::vector<robot_geometry_msgs::Point>& points);
/** @brief Set the footprint of the robot to be the given polygon,
* padded by footprint_padding.
@@ -238,7 +238,7 @@ public:
* layered_costmap_->setFootprint(). Also saves the unpadded
* footprint, which is available from
* getUnpaddedRobotFootprint(). */
void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint);
void setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint);
protected:
LayeredCostmap* layered_costmap_;
@@ -253,8 +253,8 @@ private:
*
* If the values of footprint and robot_radius are the same in
* new_config and old_config, nothing is changed. */
void readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
const std::vector<geometry_msgs::Point> &old_footprint,
void readFootprintFromConfig(const std::vector<robot_geometry_msgs::Point> &new_footprint,
const std::vector<robot_geometry_msgs::Point> &old_footprint,
const double &robot_radius);
std::vector<boost::function<PluginLayerPtr()>> creators_;
void checkMovement();
@@ -267,8 +267,8 @@ private:
boost::recursive_mutex configuration_mutex_;
std::vector<geometry_msgs::Point> unpadded_footprint_;
std::vector<geometry_msgs::Point> padded_footprint_;
std::vector<robot_geometry_msgs::Point> unpadded_footprint_;
std::vector<robot_geometry_msgs::Point> padded_footprint_;
float footprint_padding_;
private:

6
include/costmap_2d/costmap_math.h
View File

@@ -41,7 +41,7 @@
#include <math.h>
#include <algorithm>
#include <vector>
#include <geometry_msgs/Point.h>
#include <robot_geometry_msgs/Point.h>
/** @brief Return -1 if x < 0, +1 otherwise. */
inline double sign(double x)
@@ -62,8 +62,8 @@ inline double distance(double x0, double y0, double x1, double y1)
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1);
bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy);
bool intersects(std::vector<robot_geometry_msgs::Point>& polygon, float testx, float testy);
bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);
bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2);
#endif // COSTMAP_2D_COSTMAP_MATH_H_

2
include/costmap_2d/directional_layer.h
View File

@@ -11,7 +11,7 @@ namespace costmap_2d
public:
DirectionalLayer();
virtual ~DirectionalLayer();
bool laneFilter(const std::vector<geometry_msgs::PoseStamped> plan);
bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
void resetMap();
private:

38
include/costmap_2d/footprint.h
View File

@@ -38,10 +38,10 @@
#ifndef COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Polygon.h>
#include <robot_geometry_msgs/PolygonStamped.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Point32.h>
#include <robot/node_handle.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
@@ -56,28 +56,28 @@ namespace costmap_2d
* @param min_dist Output parameter of the minimum distance
* @param max_dist Output parameter of the maximum distance
*/
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint,
void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint,
double& min_dist, double& max_dist);
/**
* @brief Convert Point32 to Point
*/
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt);
robot_geometry_msgs::Point toPoint(robot_geometry_msgs::Point32 pt);
/**
* @brief Convert Point to Point32
*/
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt);
robot_geometry_msgs::Point32 toPoint32(robot_geometry_msgs::Point pt);
/**
* @brief Convert vector of Points to Polygon msg
*/
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts);
robot_geometry_msgs::Polygon toPolygon(std::vector<robot_geometry_msgs::Point> pts);
/**
* @brief Convert Polygon msg to vector of Points.
*/
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon);
std::vector<robot_geometry_msgs::Point> toPointVector(robot_geometry_msgs::Polygon polygon);
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (list of Points)
@@ -87,8 +87,8 @@ std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon);
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
std::vector<geometry_msgs::Point>& oriented_footprint);
void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
std::vector<robot_geometry_msgs::Point>& oriented_footprint);
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped)
@@ -98,18 +98,18 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
geometry_msgs::PolygonStamped & oriented_footprint);
void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
robot_geometry_msgs::PolygonStamped & oriented_footprint);
/**
* @brief Adds the specified amount of padding to the footprint (in place)
*/
void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding);
void padFootprint(std::vector<robot_geometry_msgs::Point>& footprint, double padding);
/**
* @brief Create a circular footprint from a given radius
*/
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
std::vector<robot_geometry_msgs::Point> makeFootprintFromRadius(double radius);
/**
* @brief Make the footprint from the given string.
@@ -117,13 +117,13 @@ std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
* Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
*
*/
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint);
bool makeFootprintFromString(const std::string& footprint_string, std::vector<robot_geometry_msgs::Point>& footprint);
/**
* @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree.
*/
std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh);
std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh);
/**
* @brief Create the footprint from the given XmlRpcValue.
@@ -136,13 +136,13 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
* @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<geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name);
// /** @brief Write the current unpadded_footprint_ to the "footprint"
// * parameter of the given NodeHandle so that dynamic_reconfigure
// * will see the new value. */
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint);
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint);
} // end namespace costmap_2d

4
include/costmap_2d/layer.h
View File

@@ -123,7 +123,7 @@ public:
}
/** @brief Convenience function for layered_costmap_->getFootprint(). */
const std::vector<geometry_msgs::Point>& getFootprint() const;
const std::vector<robot_geometry_msgs::Point>& getFootprint() const;
/** @brief LayeredCostmap calls this whenever the footprint there
* changes (via LayeredCostmap::setFootprint()). Override to be
@@ -159,7 +159,7 @@ protected:
tf3::BufferCore *tf_;
private:
std::vector<geometry_msgs::Point> footprint_spec_;
std::vector<robot_geometry_msgs::Point> footprint_spec_;
};
using PluginLayerPtr = boost::shared_ptr<Layer>;

6
include/costmap_2d/layered_costmap.h
View File

@@ -135,10 +135,10 @@ public:
/** @brief Updates the stored footprint, updates the circumscribed
* and inscribed radii, and calls onFootprintChanged() in all
* layers. */
void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);
void setFootprint(const std::vector<robot_geometry_msgs::Point>& footprint_spec);
/** @brief Returns the latest footprint stored with setFootprint(). */
const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }
const std::vector<robot_geometry_msgs::Point>& getFootprint() { return footprint_; }
/** @brief The radius of a circle centered at the origin of the
* robot which just surrounds all points on the robot's
@@ -169,7 +169,7 @@ private:
bool initialized_;
bool size_locked_;
double circumscribed_radius_, inscribed_radius_;
std::vector<geometry_msgs::Point> footprint_;
std::vector<robot_geometry_msgs::Point> footprint_;
};
} // namespace costmap_2d

6
include/costmap_2d/observation.h
View File

@@ -32,7 +32,7 @@
#ifndef COSTMAP_2D_OBSERVATION_H_
#define COSTMAP_2D_OBSERVATION_H_
#include <geometry_msgs/Point.h>
#include <robot_geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
namespace costmap_2d
@@ -66,7 +66,7 @@ public:
* @param obstacle_range The range out to which an observation should be able to insert obstacles
* @param raytrace_range The range out to which an observation should be able to clear via raytracing
*/
Observation(geometry_msgs::Point& origin, const sensor_msgs::PointCloud2 &cloud,
Observation(robot_geometry_msgs::Point& origin, const sensor_msgs::PointCloud2 &cloud,
double obstacle_range, double raytrace_range) :
origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
@@ -93,7 +93,7 @@ public:
{
}
geometry_msgs::Point origin_;
robot_geometry_msgs::Point origin_;
sensor_msgs::PointCloud2* cloud_;
double obstacle_range_, raytrace_range_;
};

2
include/costmap_2d/obstacle_layer.h
View File

@@ -141,7 +141,7 @@ protected:
void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
double* max_x, double* max_y);
std::vector<geometry_msgs::Point> transformed_footprint_;
std::vector<robot_geometry_msgs::Point> transformed_footprint_;
bool footprint_clearing_enabled_;
void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y);

2
include/costmap_2d/testing_helper.h
View File

@@ -77,7 +77,7 @@ void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, doubl
*iter_y = y;
*iter_z = z;
geometry_msgs::Point p;
robot_geometry_msgs::Point p;
p.x = ox;
p.y = oy;
p.z = oz;

6
include/costmap_2d/utils.h
View File

@@ -15,17 +15,17 @@ namespace costmap_2d
return default_value;
}
inline std::vector<geometry_msgs::Point> loadFootprint(const YAML::Node& node, const std::vector<geometry_msgs::Point>& default_value)
inline std::vector<robot_geometry_msgs::Point> loadFootprint(const YAML::Node& node, const std::vector<robot_geometry_msgs::Point>& default_value)
{
if( !node || !node.IsDefined())
return default_value;
std::vector<geometry_msgs::Point> fp;
std::vector<robot_geometry_msgs::Point> fp;
for (const auto& p : node) {
if (p.size() != 2)
throw std::runtime_error("Footprint point must be [x, y]");
geometry_msgs::Point point;
robot_geometry_msgs::Point point;
point.x = p[0].as<double>();
point.y = p[1].as<double>();
point.z = 0.0;

8
include/costmap_2d/voxel_grid.h
View File

@@ -11,8 +11,8 @@
#include <vector>
#include <std_msgs/Header.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Vector3.h>
#include <robot_geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Vector3.h>
namespace costmap_2d
{
@@ -20,8 +20,8 @@ namespace costmap_2d
{
std_msgs::Header header;
std::vector<uint32_t> data;
geometry_msgs::Point32 origin;
geometry_msgs::Vector3 resolutions;
robot_geometry_msgs::Point32 origin;
robot_geometry_msgs::Vector3 resolutions;
uint32_t size_x;
uint32_t size_y;
uint32_t size_z;