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

6
include/costmap_2d/costmap_math.h
View File

@ -41,7 +41,7 @@
#include <math.h> #include <math.h>
#include <algorithm> #include <algorithm>
#include <vector> #include <vector>
#include <geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
/** @brief Return -1 if x < 0, +1 otherwise. */ /** @brief Return -1 if x < 0, +1 otherwise. */
inline double sign(double x) 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); 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_ #endif // COSTMAP_2D_COSTMAP_MATH_H_

2
include/costmap_2d/directional_layer.h
View File

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

38
include/costmap_2d/footprint.h
View File

@ -38,10 +38,10 @@
#ifndef COSTMAP_2D_FOOTPRINT_H #ifndef COSTMAP_2D_FOOTPRINT_H
#define COSTMAP_2D_FOOTPRINT_H #define COSTMAP_2D_FOOTPRINT_H
#include <geometry_msgs/Polygon.h> #include <robot_geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h> #include <robot_geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h> #include <robot_geometry_msgs/Point32.h>
#include <robot/node_handle.h> #include <robot/node_handle.h>
#include <robot_xmlrpcpp/XmlRpcValue.h> #include <robot_xmlrpcpp/XmlRpcValue.h>
@ -56,28 +56,28 @@ namespace costmap_2d
* @param min_dist Output parameter of the minimum distance * @param min_dist Output parameter of the minimum distance
* @param max_dist Output parameter of the maximum 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); double& min_dist, double& max_dist);
/** /**
* @brief Convert Point32 to Point * @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 * @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 * @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. * @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) * @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 footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot * @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, void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
std::vector<geometry_msgs::Point>& oriented_footprint); std::vector<robot_geometry_msgs::Point>& oriented_footprint);
/** /**
* @brief Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped) * @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 footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot * @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, void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
geometry_msgs::PolygonStamped & oriented_footprint); robot_geometry_msgs::PolygonStamped & oriented_footprint);
/** /**
* @brief Adds the specified amount of padding to the footprint (in place) * @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 * @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. * @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], ...] * 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 * @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree. * 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. * @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 * @param full_param_name this is the full name of the rosparam from
* which the footprint_xmlrpc value came. It is used only for * which the footprint_xmlrpc value came. It is used only for
* reporting errors. */ * 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); const std::string& full_param_name);
// /** @brief Write the current unpadded_footprint_ to the "footprint" // /** @brief Write the current unpadded_footprint_ to the "footprint"
// * parameter of the given NodeHandle so that dynamic_reconfigure // * parameter of the given NodeHandle so that dynamic_reconfigure
// * will see the new value. */ // * 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 } // end namespace costmap_2d

4
include/costmap_2d/layer.h
View File

@ -123,7 +123,7 @@ public:
} }
/** @brief Convenience function for layered_costmap_->getFootprint(). */ /** @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 /** @brief LayeredCostmap calls this whenever the footprint there
* changes (via LayeredCostmap::setFootprint()). Override to be * changes (via LayeredCostmap::setFootprint()). Override to be
@ -159,7 +159,7 @@ protected:
tf3::BufferCore *tf_; tf3::BufferCore *tf_;
private: private:
std::vector<geometry_msgs::Point> footprint_spec_; std::vector<robot_geometry_msgs::Point> footprint_spec_;
}; };
using PluginLayerPtr = boost::shared_ptr<Layer>; 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 /** @brief Updates the stored footprint, updates the circumscribed
* and inscribed radii, and calls onFootprintChanged() in all * and inscribed radii, and calls onFootprintChanged() in all
* layers. */ * 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(). */ /** @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 /** @brief The radius of a circle centered at the origin of the
* robot which just surrounds all points on the robot's * robot which just surrounds all points on the robot's
@ -169,7 +169,7 @@ private:
bool initialized_; bool initialized_;
bool size_locked_; bool size_locked_;
double circumscribed_radius_, inscribed_radius_; double circumscribed_radius_, inscribed_radius_;
std::vector<geometry_msgs::Point> footprint_; std::vector<robot_geometry_msgs::Point> footprint_;
}; };
} // namespace costmap_2d } // namespace costmap_2d

6
include/costmap_2d/observation.h
View File

@ -32,7 +32,7 @@
#ifndef COSTMAP_2D_OBSERVATION_H_ #ifndef COSTMAP_2D_OBSERVATION_H_
#define 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> #include <sensor_msgs/PointCloud2.h>
namespace costmap_2d 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 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 * @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) : double obstacle_range, double raytrace_range) :
origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)), origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range) 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_; sensor_msgs::PointCloud2* cloud_;
double obstacle_range_, raytrace_range_; 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, void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
double* max_x, double* max_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_; bool footprint_clearing_enabled_;
void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_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_y = y;
*iter_z = z; *iter_z = z;
geometry_msgs::Point p; robot_geometry_msgs::Point p;
p.x = ox; p.x = ox;
p.y = oy; p.y = oy;
p.z = oz; p.z = oz;

6
include/costmap_2d/utils.h
View File

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

8
include/costmap_2d/voxel_grid.h
View File

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

6
plugins/directional_layer.cpp
View File

@ -19,7 +19,7 @@ namespace costmap_2d
DirectionalLayer::~DirectionalLayer() {} 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()) if (directional_areas_.empty())
throw "Has no map data"; 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<std::pair<unsigned int, double>> X_List, Y_List;
std::vector<double> Yaw_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_); bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_);
if(!get_success) return false; if(!get_success) return false;
for (auto it = path.poses.begin(); it != path.poses.end(); ++it) 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; unsigned int mx, my;
if (!worldToMap(p.pose.position.x, p.pose.position.y, 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; unsigned int mx, my;
double wx, wy; double wx, wy;
// Might even be in a different frame // Might even be in a different frame
// geometry_msgs::TransformStamped transform; // robot_geometry_msgs::TransformStamped transform;
tf3::TransformStampedMsg transformMsg; tf3::TransformStampedMsg transformMsg;
try try
{ {

2
plugins/voxel_layer.cpp
View File

@ -406,7 +406,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
// if (publish_clearing_points) // if (publish_clearing_points)
// { // {
geometry_msgs::Point32 point; robot_geometry_msgs::Point32 point;
point.x = wpx; point.x = wpx;
point.y = wpy; point.y = wpy;
point.z = wpz; 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; 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 // we assume the polygon is given in the global_frame... we need to transform it to map coordinates
std::vector<MapLocation> map_polygon; 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); new_footprint = loadFootprint(layer["footprint"], new_footprint);
transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0); 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)); setUnpaddedRobotFootprint(toPointVector(footprint));
} }
@ -324,8 +324,8 @@ Costmap2DROBOT::~Costmap2DROBOT()
} }
void Costmap2DROBOT::readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint, void Costmap2DROBOT::readFootprintFromConfig(const std::vector<robot_geometry_msgs::Point> &new_footprint,
const std::vector<geometry_msgs::Point> &old_footprint, const std::vector<robot_geometry_msgs::Point> &old_footprint,
const double &robot_radius) const double &robot_radius)
{ {
// Only change the footprint if footprint or robot_radius has // 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; unpadded_footprint_ = points;
padded_footprint_ = points; padded_footprint_ = points;
@ -365,7 +365,7 @@ void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::
void Costmap2DROBOT::checkMovement() void Costmap2DROBOT::checkMovement()
{ {
geometry_msgs::PoseStamped new_pose; robot_geometry_msgs::PoseStamped new_pose;
if (!getRobotPose(new_pose)) if (!getRobotPose(new_pose))
std::cout << "Cannot get robot pose\n"; std::cout << "Cannot get robot pose\n";
} }
@ -390,14 +390,14 @@ void Costmap2DROBOT::updateMap()
if (!stop_updates_) if (!stop_updates_)
{ {
// get global pose // get global pose
geometry_msgs::PoseStamped pose; robot_geometry_msgs::PoseStamped pose;
if (getRobotPose (pose)) if (getRobotPose (pose))
{ {
double x = pose.pose.position.x, double x = pose.pose.position.x,
y = pose.pose.position.y, y = pose.pose.position.y,
yaw = data_convert::getYaw(pose.pose.orientation); yaw = data_convert::getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw); layered_costmap_->updateMap(x, y, yaw);
geometry_msgs::PolygonStamped footprint; robot_geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_; footprint.header.frame_id = global_frame_;
footprint.header.stamp = robot::Time::now(); footprint.header.stamp = robot::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint); 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; robot_geometry_msgs::PoseStamped robot_pose;
geometry_msgs::Pose pose_default; robot_geometry_msgs::Pose pose_default;
global_pose.pose = pose_default; global_pose.pose = pose_default;
robot_pose.pose = pose_default; robot_pose.pose = pose_default;
@ -555,10 +555,10 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
return true; 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"); // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getOrientedFootprint");
geometry_msgs::PoseStamped global_pose; robot_geometry_msgs::PoseStamped global_pose;
if (!getRobotPose(global_pose)) if (!getRobotPose(global_pose))
return; 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); 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; bool c = false;
int i, j, nvert = polygon.size(); int i, j, nvert = polygon.size();
@ -75,7 +75,7 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float t
return c; 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++) for (unsigned int i = 0; i < polygon1.size(); i++)
if (intersects(polygon2, polygon1[i].x, polygon1[i].y)) 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; 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); 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 <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h> #include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h> #include <costmap_2d/array_parser.h>
#include <geometry_msgs/Point32.h> #include <robot_geometry_msgs/Point32.h>
namespace costmap_2d 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(); min_dist = std::numeric_limits<double>::max();
max_dist = 0.0; 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)); 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.x = pt.x;
point32.y = pt.y; point32.y = pt.y;
point32.z = pt.z; point32.z = pt.z;
return point32; 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.x = pt.x;
point.y = pt.y; point.y = pt.y;
point.z = pt.z; point.z = pt.z;
return point; 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++){ for (int i = 0; i < pts.size(); i++){
polygon.points.push_back(toPoint32(pts[i])); polygon.points.push_back(toPoint32(pts[i]));
} }
return polygon; 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++) for (int i = 0; i < polygon.points.size(); i++)
{ {
pts.push_back(toPoint(polygon.points[i])); pts.push_back(toPoint(polygon.points[i]));
@ -103,8 +103,8 @@ std::vector<geometry_msgs::Point> toPointVector(geometry_msgs::Polygon polygon)
return pts; return pts;
} }
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
std::vector<geometry_msgs::Point>& oriented_footprint) std::vector<robot_geometry_msgs::Point>& oriented_footprint)
{ {
// build the oriented footprint at a given location // build the oriented footprint at a given location
oriented_footprint.clear(); oriented_footprint.clear();
@ -112,15 +112,15 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
double sin_th = sin(theta); double sin_th = sin(theta);
for (unsigned int i = 0; i < footprint_spec.size(); ++i) 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.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); new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.push_back(new_pt); oriented_footprint.push_back(new_pt);
} }
} }
void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec, void transformFootprint(double x, double y, double theta, const std::vector<robot_geometry_msgs::Point>& footprint_spec,
geometry_msgs::PolygonStamped& oriented_footprint) robot_geometry_msgs::PolygonStamped& oriented_footprint)
{ {
// build the oriented footprint at a given location // build the oriented footprint at a given location
oriented_footprint.polygon.points.clear(); 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); double sin_th = sin(theta);
for (unsigned int i = 0; i < footprint_spec.size(); ++i) 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.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); new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
oriented_footprint.polygon.points.push_back(new_pt); 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 // pad footprint in place
for (unsigned int i = 0; i < footprint.size(); i++) 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.x += sign0(pt.x) * padding;
pt.y += sign0(pt.y) * 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 // Loop over 16 angles around a circle making a point each time
int N = 16; int N = 16;
geometry_msgs::Point pt; robot_geometry_msgs::Point pt;
for (int i = 0; i < N; ++i) for (int i = 0; i < N; ++i)
{ {
double angle = i * 2 * M_PI / N; 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::string error;
std::vector<std::vector<float> > vvf = parseVVF(footprint_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) if (vvf[ i ].size() == 2)
{ {
geometry_msgs::Point point; robot_geometry_msgs::Point point;
point.x = vvf[ i ][ 0 ]; point.x = vvf[ i ][ 0 ];
point.y = vvf[ i ][ 1 ]; point.y = vvf[ i ][ 1 ];
point.z = 0; point.z = 0;
@ -207,17 +207,17 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
return true; 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"); YAML::Node ft = nh.getParamValue("footprint");
std::vector<geometry_msgs::Point> footprint; std::vector<robot_geometry_msgs::Point> footprint;
if (ft && ft.IsSequence()) if (ft && ft.IsSequence())
{ {
for (size_t i = 0; i < ft.size(); ++i) for (size_t i = 0; i < ft.size(); ++i)
{ {
auto pt = ft[i]; auto pt = ft[i];
geometry_msgs::Point p; robot_geometry_msgs::Point p;
p.x = pt[0].as<double>(); p.x = pt[0].as<double>();
p.y = pt[1].as<double>(); p.y = pt[1].as<double>();
p.z = 0.0; p.z = 0.0;
@ -228,13 +228,13 @@ std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
return footprint; 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; // std::ostringstream oss;
// bool first = true; // bool first = true;
// for (unsigned int i = 0; i < footprint.size(); i++) // for (unsigned int i = 0; i < footprint.size(); i++)
// { // {
// geometry_msgs::Point p = footprint[ i ]; // robot_geometry_msgs::Point p = footprint[ i ];
// if (first) // if (first)
// { // {
// oss << "[[" << p.x << "," << p.y << "]"; // 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); 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) const std::string& full_param_name)
{ {
// Make sure we have an array of at least 3 elements. // 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]]"); "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
} }
std::vector<geometry_msgs::Point> footprint; std::vector<robot_geometry_msgs::Point> footprint;
geometry_msgs::Point pt; robot_geometry_msgs::Point pt;
for (int i = 0; i < footprint_xmlrpc.size(); ++i) 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(); onInitialize();
} }
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const const std::vector<robot_geometry_msgs::Point>& Layer::getFootprint() const
{ {
return layered_costmap_->getFootprint(); return layered_costmap_->getFootprint();
} }

2
src/layered_costmap.cpp
View File

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

2
test/static_layer_test.cpp
View File

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