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
Show Stats 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;

6
plugins/directional_layer.cpp
View File

@ -19,7 +19,7 @@ namespace costmap_2d
DirectionalLayer::~DirectionalLayer() {}
bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)
bool DirectionalLayer::laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan)
{
if (directional_areas_.empty())
throw "Has no map data";
@ -119,13 +119,13 @@ namespace costmap_2d
std::vector<std::pair<unsigned int, double>> X_List, Y_List;
std::vector<double> Yaw_list;
const geometry_msgs::PoseStamped &e = path.poses.back();
const robot_geometry_msgs::PoseStamped &e = path.poses.back();
bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_);
if(!get_success) return false;
for (auto it = path.poses.begin(); it != path.poses.end(); ++it)
{
const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
const robot_geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
unsigned int mx, my;
if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
{

2
plugins/static_layer.cpp
View File

@ -343,7 +343,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
// geometry_msgs::TransformStamped transform;
// robot_geometry_msgs::TransformStamped transform;
tf3::TransformStampedMsg transformMsg;
try
{

2
plugins/voxel_layer.cpp
View File

@ -406,7 +406,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
// if (publish_clearing_points)
// {
geometry_msgs::Point32 point;
robot_geometry_msgs::Point32 point;
point.x = wpx;
point.y = wpy;
point.z = wpz;

2
src/costmap_2d.cpp
View File

@ -312,7 +312,7 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
delete[] local_map;
}
bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
bool Costmap2D::setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value)
{
// we assume the polygon is given in the global_frame... we need to transform it to map coordinates
std::vector<MapLocation> map_polygon;

26
src/costmap_2d_robot.cpp
View File

@ -227,7 +227,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
}
}
std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
std::vector<robot_geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
new_footprint = loadFootprint(layer["footprint"], new_footprint);
transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
@ -306,7 +306,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
}
}
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint)
{
setUnpaddedRobotFootprint(toPointVector(footprint));
}
@ -324,8 +324,8 @@ Costmap2DROBOT::~Costmap2DROBOT()
}
void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
const std::vector<geometry_msgs::Point> &old_footprint,
void Costmap2DROBOT::readFootprintFromConfig(const std::vector<robot_geometry_msgs::Point> &new_footprint,
const std::vector<robot_geometry_msgs::Point> &old_footprint,
const double &robot_radius)
{
// Only change the footprint if footprint or robot_radius has
@ -354,7 +354,7 @@ void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Po
}
}
void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<robot_geometry_msgs::Point>& points)
{
unpadded_footprint_ = points;
padded_footprint_ = points;
@ -365,7 +365,7 @@ void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::
void Costmap2DROBOT::checkMovement()
{
geometry_msgs::PoseStamped new_pose;
robot_geometry_msgs::PoseStamped new_pose;
if (!getRobotPose(new_pose))
std::cout << "Cannot get robot pose\n";
}
@ -390,14 +390,14 @@ void Costmap2DROBOT::updateMap()
if (!stop_updates_)
{
// get global pose
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
if (getRobotPose (pose))
{
double x = pose.pose.position.x,
y = pose.pose.position.y,
yaw = data_convert::getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw);
geometry_msgs::PolygonStamped footprint;
robot_geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
footprint.header.stamp = robot::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint);
@ -495,10 +495,10 @@ void Costmap2DROBOT::resetLayers()
}
}
bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) const
{
geometry_msgs::PoseStamped robot_pose;
geometry_msgs::Pose pose_default;
robot_geometry_msgs::PoseStamped robot_pose;
robot_geometry_msgs::Pose pose_default;
global_pose.pose = pose_default;
robot_pose.pose = pose_default;
@ -555,10 +555,10 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
return true;
}
void Costmap2DROBOT::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
void Costmap2DROBOT::getOrientedFootprint(std::vector<robot_geometry_msgs::Point>& oriented_footprint) const
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getOrientedFootprint");
geometry_msgs::PoseStamped global_pose;
robot_geometry_msgs::PoseStamped global_pose;
if (!getRobotPose(global_pose))
return;

6
src/costmap_math.cpp
View File

@ -61,7 +61,7 @@ double distanceToLine(double pX, double pY, double x0, double y0, double x1, dou
return distance(pX, pY, xx, yy);
}
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 c = false;
int i, j, nvert = polygon.size();
@ -75,7 +75,7 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float t
return c;
}
bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
bool intersects_helper(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2)
{
for (unsigned int i = 0; i < polygon1.size(); i++)
if (intersects(polygon2, polygon1[i].x, polygon1[i].y))
@ -83,7 +83,7 @@ bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<
return false;
}
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)
{
return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
}

62
src/footprint.cpp
View File

@ -33,12 +33,12 @@
#include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h>
#include <geometry_msgs/Point32.h>
#include <robot_geometry_msgs/Point32.h>
namespace costmap_2d
{
void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
{
min_dist = std::numeric_limits<double>::max();
max_dist = 0.0;
@ -66,36 +66,36 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
}
geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
robot_geometry_msgs::Point32 toPoint32(robot_geometry_msgs::Point pt)
{
geometry_msgs::Point32 point32;
robot_geometry_msgs::Point32 point32;
point32.x = pt.x;
point32.y = pt.y;
point32.z = pt.z;
return point32;
}
geometry_msgs::Point toPoint(geometry_msgs::Point32 pt)
robot_geometry_msgs::Point toPoint(robot_geometry_msgs::Point32 pt)
{
geometry_msgs::Point point;
robot_geometry_msgs::Point point;
point.x = pt.x;
point.y = pt.y;
point.z = pt.z;
return point;
}
geometry_msgs::Polygon toPolygon(std::vector<geometry_msgs::Point> pts)
robot_geometry_msgs::Polygon toPolygon(std::vector<robot_geometry_msgs::Point> pts)
{
geometry_msgs::Polygon polygon;
robot_geometry_msgs::Polygon polygon;
for (int i = 0; i < pts.size(); i++){
polygon.points.push_back(toPoint32(pts[i]));
}
return polygon;
}
std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
std::vector<robot_geometry_msgs::Point> toPointVector(robot_geometry_msgs::Polygon polygon)
{
std::vector<geometry_msgs::Point> pts;
std::vector<robot_geometry_msgs::Point> pts;
for (int i = 0; i < polygon.points.size(); i++)
{
pts.push_back(toPoint(polygon.points[i]));
@ -103,8 +103,8 @@ std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
return pts;
}
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)
{
// build the oriented footprint at a given location
oriented_footprint.clear();
@ -112,15 +112,15 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
double sin_th = sin(theta);
for (unsigned int i = 0; i < footprint_spec.size(); ++i)
{
geometry_msgs::Point new_pt;
robot_geometry_msgs::Point new_pt;
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.push_back(new_pt);
}
}
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)
{
// build the oriented footprint at a given location
oriented_footprint.polygon.points.clear();
@ -128,32 +128,32 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
double sin_th = sin(theta);
for (unsigned int i = 0; i < footprint_spec.size(); ++i)
{
geometry_msgs::Point32 new_pt;
robot_geometry_msgs::Point32 new_pt;
new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.polygon.points.push_back(new_pt);
}
}
void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
void padFootprint(std::vector<robot_geometry_msgs::Point>& footprint, double padding)
{
// pad footprint in place
for (unsigned int i = 0; i < footprint.size(); i++)
{
geometry_msgs::Point& pt = footprint[ i ];
robot_geometry_msgs::Point& pt = footprint[ i ];
pt.x += sign0(pt.x) * padding;
pt.y += sign0(pt.y) * padding;
}
}
std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
std::vector<robot_geometry_msgs::Point> makeFootprintFromRadius(double radius)
{
std::vector<geometry_msgs::Point> points;
std::vector<robot_geometry_msgs::Point> points;
// Loop over 16 angles around a circle making a point each time
int N = 16;
geometry_msgs::Point pt;
robot_geometry_msgs::Point pt;
for (int i = 0; i < N; ++i)
{
double angle = i * 2 * M_PI / N;
@ -167,7 +167,7 @@ std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
}
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)
{
std::string error;
std::vector<std::vector<float> > vvf = parseVVF(footprint_string, error);
@ -190,7 +190,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
{
if (vvf[ i ].size() == 2)
{
geometry_msgs::Point point;
robot_geometry_msgs::Point point;
point.x = vvf[ i ][ 0 ];
point.y = vvf[ i ][ 1 ];
point.z = 0;
@ -207,17 +207,17 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
return true;
}
std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
{
YAML::Node ft = nh.getParamValue("footprint");
std::vector<geometry_msgs::Point> footprint;
std::vector<robot_geometry_msgs::Point> footprint;
if (ft && ft.IsSequence())
{
for (size_t i = 0; i < ft.size(); ++i)
{
auto pt = ft[i];
geometry_msgs::Point p;
robot_geometry_msgs::Point p;
p.x = pt[0].as<double>();
p.y = pt[1].as<double>();
p.z = 0.0;
@ -228,13 +228,13 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
return footprint;
}
// 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)
// {
// std::ostringstream oss;
// bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++)
// {
// geometry_msgs::Point p = footprint[ i ];
// robot_geometry_msgs::Point p = footprint[ i ];
// if (first)
// {
// oss << "[[" << p.x << "," << p.y << "]";
@ -263,7 +263,7 @@ double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value, const std::string&
return value.getType() == robot::XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
}
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)
{
// Make sure we have an array of at least 3 elements.
@ -276,8 +276,8 @@ std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcV
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
std::vector<robot_geometry_msgs::Point> footprint;
robot_geometry_msgs::Point pt;
for (int i = 0; i < footprint_xmlrpc.size(); ++i)
{

2
src/layer.cpp
View File

@ -48,7 +48,7 @@ void Layer::initialize(LayeredCostmap* parent, std::string name, tf3::BufferCore
onInitialize();
}
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const
const std::vector<robot_geometry_msgs::Point>& Layer::getFootprint() const
{
return layered_costmap_->getFootprint();
}

2
src/layered_costmap.cpp
View File

@ -175,7 +175,7 @@ namespace costmap_2d
return current_;
}
void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point> &footprint_spec)
void LayeredCostmap::setFootprint(const std::vector<robot_geometry_msgs::Point> &footprint_spec)
{
footprint_ = footprint_spec;
costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);

8
src/observation_buffer.cpp
View File

@ -65,7 +65,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
tf3::Time transform_time = tf3::Time::now();
std::string tf_error;
geometry_msgs::TransformStamped transformStamped;
robot_geometry_msgs::TransformStamped transformStamped;
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(),
@ -80,7 +80,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
Observation& obs = *obs_it;
geometry_msgs::PointStamped origin;
robot_geometry_msgs::PointStamped origin;
origin.header.frame_id = global_frame_;
origin.header.stamp = data_convert::convertTime(transform_time);
origin.point = obs.origin_;
@ -119,7 +119,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
{
geometry_msgs::PointStamped global_origin;
robot_geometry_msgs::PointStamped global_origin;
// create a new observation on the list to be populated
observation_list_.push_front(Observation());
@ -130,7 +130,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
try
{
// given these observations come from sensors... we'll need to store the origin pt of the sensor
geometry_msgs::PointStamped local_origin;
robot_geometry_msgs::PointStamped local_origin;
local_origin.header.stamp = cloud.header.stamp;
local_origin.header.frame_id = origin_frame;
local_origin.point.x = 0;

2
test/static_layer_test.cpp
View File

@ -2,7 +2,7 @@
#include <costmap_2d/cost_values.h>
#include <gtest/gtest.h>
#include <tf3/buffer_core.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <robot_geometry_msgs/PoseWithCovariance.h>
namespace costmap_2d {