From 2c3d7d586d1a664573b99f45d8521950d46db30a Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 30 Dec 2025 09:08:14 +0700 Subject: [PATCH] HiepLM update --- include/costmap_2d/costmap_2d.h | 4 +- include/costmap_2d/costmap_2d_robot.h | 30 ++++++------- include/costmap_2d/costmap_math.h | 6 +-- include/costmap_2d/directional_layer.h | 2 +- include/costmap_2d/footprint.h | 38 ++++++++-------- include/costmap_2d/layer.h | 4 +- include/costmap_2d/layered_costmap.h | 6 +-- include/costmap_2d/observation.h | 6 +-- include/costmap_2d/obstacle_layer.h | 2 +- include/costmap_2d/testing_helper.h | 2 +- include/costmap_2d/utils.h | 6 +-- include/costmap_2d/voxel_grid.h | 8 ++-- plugins/directional_layer.cpp | 6 +-- plugins/static_layer.cpp | 2 +- plugins/voxel_layer.cpp | 2 +- src/costmap_2d.cpp | 2 +- src/costmap_2d_robot.cpp | 26 +++++------ src/costmap_math.cpp | 6 +-- src/footprint.cpp | 62 +++++++++++++------------- src/layer.cpp | 2 +- src/layered_costmap.cpp | 2 +- src/observation_buffer.cpp | 8 ++-- test/static_layer_test.cpp | 2 +- 23 files changed, 117 insertions(+), 117 deletions(-) diff --git a/include/costmap_2d/costmap_2d.h b/include/costmap_2d/costmap_2d.h index 38658e8..b94cc92 100755 --- a/include/costmap_2d/costmap_2d.h +++ b/include/costmap_2d/costmap_2d.h @@ -40,7 +40,7 @@ #include #include -#include +#include #include 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& polygon, unsigned char cost_value); + bool setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value); /** * @brief Get the map cells that make up the outline of a polygon diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h index bbdff46..f3b7066 100755 --- a/include/costmap_2d/costmap_2d_robot.h +++ b/include/costmap_2d/costmap_2d_robot.h @@ -42,9 +42,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -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& getRobotFootprint() const noexcept + inline const std::vector& 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& getUnpaddedRobotFootprint() const noexcept + inline const std::vector& 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& oriented_footprint) const; + void getOrientedFootprint(std::vector& 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& points); + void setUnpaddedRobotFootprint(const std::vector& 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 &new_footprint, - const std::vector &old_footprint, + void readFootprintFromConfig(const std::vector &new_footprint, + const std::vector &old_footprint, const double &robot_radius); std::vector> creators_; void checkMovement(); @@ -267,8 +267,8 @@ private: boost::recursive_mutex configuration_mutex_; - std::vector unpadded_footprint_; - std::vector padded_footprint_; + std::vector unpadded_footprint_; + std::vector padded_footprint_; float footprint_padding_; private: diff --git a/include/costmap_2d/costmap_math.h b/include/costmap_2d/costmap_math.h index 71fe96c..a3c1864 100755 --- a/include/costmap_2d/costmap_math.h +++ b/include/costmap_2d/costmap_math.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include /** @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& polygon, float testx, float testy); +bool intersects(std::vector& polygon, float testx, float testy); -bool intersects(std::vector& polygon1, std::vector& polygon2); +bool intersects(std::vector& polygon1, std::vector& polygon2); #endif // COSTMAP_2D_COSTMAP_MATH_H_ diff --git a/include/costmap_2d/directional_layer.h b/include/costmap_2d/directional_layer.h index 452c946..5414d3d 100644 --- a/include/costmap_2d/directional_layer.h +++ b/include/costmap_2d/directional_layer.h @@ -11,7 +11,7 @@ namespace costmap_2d public: DirectionalLayer(); virtual ~DirectionalLayer(); - bool laneFilter(const std::vector plan); + bool laneFilter(const std::vector plan); void resetMap(); private: diff --git a/include/costmap_2d/footprint.h b/include/costmap_2d/footprint.h index be3d6a8..3bc0475 100755 --- a/include/costmap_2d/footprint.h +++ b/include/costmap_2d/footprint.h @@ -38,10 +38,10 @@ #ifndef COSTMAP_2D_FOOTPRINT_H #define COSTMAP_2D_FOOTPRINT_H -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -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& footprint, +void calculateMinAndMaxDistances(const std::vector& 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 pts); +robot_geometry_msgs::Polygon toPolygon(std::vector pts); /** * @brief Convert Polygon msg to vector of Points. */ -std::vector toPointVector(geometry_msgs::Polygon polygon); +std::vector 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 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& footprint_spec, - std::vector& oriented_footprint); +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + std::vector& 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& footprint_spec, - geometry_msgs::PolygonStamped & oriented_footprint); +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + robot_geometry_msgs::PolygonStamped & oriented_footprint); /** * @brief Adds the specified amount of padding to the footprint (in place) */ -void padFootprint(std::vector& footprint, double padding); +void padFootprint(std::vector& footprint, double padding); /** * @brief Create a circular footprint from a given radius */ -std::vector makeFootprintFromRadius(double radius); +std::vector makeFootprintFromRadius(double radius); /** * @brief Make the footprint from the given string. @@ -117,13 +117,13 @@ std::vector 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& footprint); +bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint); /** * @brief Read the ros-params "footprint" and/or "robot_radius" from * the given NodeHandle using searchParam() to go up the tree. */ -std::vector makeFootprintFromParams(robot::NodeHandle& nh); +std::vector makeFootprintFromParams(robot::NodeHandle& nh); /** * @brief Create the footprint from the given XmlRpcValue. @@ -136,13 +136,13 @@ std::vector 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 makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc, +std::vector 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& footprint); +// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector& footprint); } // end namespace costmap_2d diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h index ea8f3aa..95e9cc5 100755 --- a/include/costmap_2d/layer.h +++ b/include/costmap_2d/layer.h @@ -123,7 +123,7 @@ public: } /** @brief Convenience function for layered_costmap_->getFootprint(). */ - const std::vector& getFootprint() const; + const std::vector& 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 footprint_spec_; + std::vector footprint_spec_; }; using PluginLayerPtr = boost::shared_ptr; diff --git a/include/costmap_2d/layered_costmap.h b/include/costmap_2d/layered_costmap.h index 72a8bdd..8155a2a 100755 --- a/include/costmap_2d/layered_costmap.h +++ b/include/costmap_2d/layered_costmap.h @@ -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& footprint_spec); + void setFootprint(const std::vector& footprint_spec); /** @brief Returns the latest footprint stored with setFootprint(). */ - const std::vector& getFootprint() { return footprint_; } + const std::vector& 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 footprint_; + std::vector footprint_; }; } // namespace costmap_2d diff --git a/include/costmap_2d/observation.h b/include/costmap_2d/observation.h index 695892b..3fdefbd 100755 --- a/include/costmap_2d/observation.h +++ b/include/costmap_2d/observation.h @@ -32,7 +32,7 @@ #ifndef COSTMAP_2D_OBSERVATION_H_ #define COSTMAP_2D_OBSERVATION_H_ -#include +#include #include 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_; }; diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h index 087412a..7a4793c 100755 --- a/include/costmap_2d/obstacle_layer.h +++ b/include/costmap_2d/obstacle_layer.h @@ -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 transformed_footprint_; + std::vector 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); diff --git a/include/costmap_2d/testing_helper.h b/include/costmap_2d/testing_helper.h index 81f9c3e..08b25e6 100644 --- a/include/costmap_2d/testing_helper.h +++ b/include/costmap_2d/testing_helper.h @@ -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; diff --git a/include/costmap_2d/utils.h b/include/costmap_2d/utils.h index a61a703..cb098b8 100644 --- a/include/costmap_2d/utils.h +++ b/include/costmap_2d/utils.h @@ -15,17 +15,17 @@ namespace costmap_2d return default_value; } - inline std::vector loadFootprint(const YAML::Node& node, const std::vector& default_value) + inline std::vector loadFootprint(const YAML::Node& node, const std::vector& default_value) { if( !node || !node.IsDefined()) return default_value; - std::vector fp; + std::vector 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(); point.y = p[1].as(); point.z = 0.0; diff --git a/include/costmap_2d/voxel_grid.h b/include/costmap_2d/voxel_grid.h index 6363e6c..aa3226d 100644 --- a/include/costmap_2d/voxel_grid.h +++ b/include/costmap_2d/voxel_grid.h @@ -11,8 +11,8 @@ #include #include -#include -#include +#include +#include namespace costmap_2d { @@ -20,8 +20,8 @@ namespace costmap_2d { std_msgs::Header header; std::vector 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; diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index 2c65eb8..2b32c88 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -19,7 +19,7 @@ namespace costmap_2d DirectionalLayer::~DirectionalLayer() {} - bool DirectionalLayer::laneFilter(const std::vector plan) + bool DirectionalLayer::laneFilter(const std::vector plan) { if (directional_areas_.empty()) throw "Has no map data"; @@ -119,13 +119,13 @@ namespace costmap_2d std::vector> X_List, Y_List; std::vector 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)) { diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 79bbf36..046e1c5 100755 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -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 { diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index b7c2177..fe9d8da 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -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; diff --git a/src/costmap_2d.cpp b/src/costmap_2d.cpp index 3d50327..34b0ebb 100755 --- a/src/costmap_2d.cpp +++ b/src/costmap_2d.cpp @@ -312,7 +312,7 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) delete[] local_map; } -bool Costmap2D::setConvexPolygonCost(const std::vector& polygon, unsigned char cost_value) +bool Costmap2D::setConvexPolygonCost(const std::vector& 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 map_polygon; diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 6086daa..dbf13ff 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -227,7 +227,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH } } - std::vector new_footprint;// = loadParam(layer, "footprint", 0.0); + std::vector 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 &new_footprint, - const std::vector &old_footprint, +void Costmap2DROBOT::readFootprintFromConfig(const std::vector &new_footprint, + const std::vector &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& points) +void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector& points) { unpadded_footprint_ = points; padded_footprint_ = points; @@ -365,7 +365,7 @@ void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vectorupdateMap(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& oriented_footprint) const +void Costmap2DROBOT::getOrientedFootprint(std::vector& 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; diff --git a/src/costmap_math.cpp b/src/costmap_math.cpp index 97f7f50..2bfe1b4 100755 --- a/src/costmap_math.cpp +++ b/src/costmap_math.cpp @@ -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& polygon, float testx, float testy) +bool intersects(std::vector& polygon, float testx, float testy) { bool c = false; int i, j, nvert = polygon.size(); @@ -75,7 +75,7 @@ bool intersects(std::vector& polygon, float testx, float t return c; } -bool intersects_helper(std::vector& polygon1, std::vector& polygon2) +bool intersects_helper(std::vector& polygon1, std::vector& 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& polygon1, std::vector< return false; } -bool intersects(std::vector& polygon1, std::vector& polygon2) +bool intersects(std::vector& polygon1, std::vector& polygon2) { return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1); } diff --git a/src/footprint.cpp b/src/footprint.cpp index 81a4a9b..47dc31f 100755 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -33,12 +33,12 @@ #include #include #include -#include +#include namespace costmap_2d { -void calculateMinAndMaxDistances(const std::vector& footprint, double& min_dist, double& max_dist) +void calculateMinAndMaxDistances(const std::vector& footprint, double& min_dist, double& max_dist) { min_dist = std::numeric_limits::max(); max_dist = 0.0; @@ -66,36 +66,36 @@ void calculateMinAndMaxDistances(const std::vector& 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 pts) +robot_geometry_msgs::Polygon toPolygon(std::vector 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 toPointVector(geometry_msgs::Polygon polygon) +std::vector toPointVector(robot_geometry_msgs::Polygon polygon) { - std::vector pts; + std::vector pts; for (int i = 0; i < polygon.points.size(); i++) { pts.push_back(toPoint(polygon.points[i])); @@ -103,8 +103,8 @@ std::vector toPointVector(geometry_msgs::Polygon polygon) return pts; } -void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, - std::vector& oriented_footprint) +void transformFootprint(double x, double y, double theta, const std::vector& footprint_spec, + std::vector& 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& footprint_spec, - geometry_msgs::PolygonStamped& oriented_footprint) +void transformFootprint(double x, double y, double theta, const std::vector& 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& footprint, double padding) +void padFootprint(std::vector& 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 makeFootprintFromRadius(double radius) +std::vector makeFootprintFromRadius(double radius) { - std::vector points; + std::vector 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 makeFootprintFromRadius(double radius) } -bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint) +bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint) { std::string error; std::vector > vvf = parseVVF(footprint_string, error); @@ -190,7 +190,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector makeFootprintFromParams(robot::NodeHandle& nh) +std::vector makeFootprintFromParams(robot::NodeHandle& nh) { YAML::Node ft = nh.getParamValue("footprint"); - std::vector footprint; + std::vector 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(); p.y = pt[1].as(); p.z = 0.0; @@ -228,13 +228,13 @@ std::vector makeFootprintFromParams(robot::NodeHandle& nh) return footprint; } -// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector& footprint) +// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector& 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 makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc, +std::vector 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 makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcV "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); } - std::vector footprint; - geometry_msgs::Point pt; + std::vector footprint; + robot_geometry_msgs::Point pt; for (int i = 0; i < footprint_xmlrpc.size(); ++i) { diff --git a/src/layer.cpp b/src/layer.cpp index a320e6d..9267f84 100755 --- a/src/layer.cpp +++ b/src/layer.cpp @@ -48,7 +48,7 @@ void Layer::initialize(LayeredCostmap* parent, std::string name, tf3::BufferCore onInitialize(); } -const std::vector& Layer::getFootprint() const +const std::vector& Layer::getFootprint() const { return layered_costmap_->getFootprint(); } diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp index 6c590cc..7bef187 100755 --- a/src/layered_costmap.cpp +++ b/src/layered_costmap.cpp @@ -175,7 +175,7 @@ namespace costmap_2d return current_; } - void LayeredCostmap::setFootprint(const std::vector &footprint_spec) + void LayeredCostmap::setFootprint(const std::vector &footprint_spec) { footprint_ = footprint_spec; costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_); diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 7452fff..acc5e6b 100755 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -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; diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index a27156f..6ed8283 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace costmap_2d {