31 #include <geometry_msgs/PolygonStamped.h> 32 #include <nav_msgs/OccupancyGrid.h> 33 #include <sensor_msgs/PointCloud.h> 39 #include <costmap_cspace_msgs/CSpace3D.h> 40 #include <costmap_cspace_msgs/CSpace3DUpdate.h> 60 std::pair<nav_msgs::OccupancyGrid::ConstPtr,
64 const nav_msgs::OccupancyGrid::ConstPtr& msg,
67 if (map->getAngularGrid() <= 0)
70 std::runtime_error(
"ang_resolution is not set.");
77 if (map_buffer_.size() > 0)
79 for (
auto& map : map_buffer_)
81 ROS_INFO(
"%ld buffered costmaps processed", map_buffer_.size());
86 const nav_msgs::OccupancyGrid::ConstPtr& msg,
91 auto map_msg = map->getMap();
92 if (map_msg->info.width < 1 ||
93 map_msg->info.height < 1)
95 map_buffer_.push_back(
96 std::pair<nav_msgs::OccupancyGrid::ConstPtr,
101 map->processMapOverlay(msg);
106 const costmap_cspace_msgs::CSpace3DUpdate::Ptr update)
109 pub_costmap_.
publish<costmap_cspace_msgs::CSpace3D>(*map);
114 const costmap_cspace_msgs::CSpace3DUpdate::Ptr update)
119 pub_costmap_update_.
publish(*update);
123 ROS_WARN(
"Updated region of the costmap is empty. The position may be out-of-boundary, or input map is wrong.");
131 sensor_msgs::PointCloud pc;
132 pc.header = map.header;
134 for (
size_t yaw = 0; yaw < map.info.angle; yaw++)
136 for (
unsigned int i = 0; i < map.info.width * map.info.height; i++)
138 int gx = i % map.info.width;
139 int gy = i / map.info.width;
140 if (map.data[i + yaw * map.info.width * map.info.height] < 100)
142 geometry_msgs::Point32 p;
143 p.x = gx * map.info.linear_resolution + map.info.origin.position.x;
144 p.y = gy * map.info.linear_resolution + map.info.origin.position.y;
146 pc.points.push_back(p);
153 auto footprint = msg;
155 pub_footprint_.
publish(footprint);
159 const std::string overlay_mode_str)
161 if (overlay_mode_str ==
"overwrite")
165 else if (overlay_mode_str ==
"max")
169 ROS_FATAL(
"Unknown overlay_mode \"%s\"", overlay_mode_str.c_str());
170 throw std::runtime_error(
"Unknown overlay_mode.");
179 pub_costmap_ = neonavigation_common::compat::advertise<costmap_cspace_msgs::CSpace3D>(
181 pnh_,
"costmap", 1,
true);
182 pub_costmap_update_ = neonavigation_common::compat::advertise<costmap_cspace_msgs::CSpace3DUpdate>(
183 nh_,
"costmap_update",
184 pnh_,
"costmap_update", 1,
true);
185 pub_footprint_ = pnh_.advertise<geometry_msgs::PolygonStamped>(
"footprint", 2,
true);
186 pub_debug_ = pnh_.advertise<sensor_msgs::PointCloud>(
"debug", 1,
true);
189 pnh_.param(
"ang_resolution", ang_resolution, 16);
192 if (!pnh_.hasParam(
"footprint"))
194 ROS_FATAL(
"Footprint doesn't specified");
195 throw std::runtime_error(
"Footprint doesn't specified.");
197 pnh_.getParam(
"footprint", footprint_xml);
203 catch (
const std::exception& e)
214 pnh_.param(
"linear_expand", linear_expand, 0.2
f);
215 pnh_.param(
"linear_spread", linear_spread, 0.5
f);
217 root_layer->setFootprint(footprint);
219 if (pnh_.hasParam(
"static_layers"))
222 pnh_.getParam(
"static_layers", layers_xml);
226 ROS_FATAL(
"static_layers parameter must contain at least one layer config.");
228 "Migration from old version:\n" 231 " YOUR_LAYER_NAME:\n" 232 " type: LAYER_TYPE\n" 233 " parameters: values\n" 236 " - name: YOUR_LAYER_NAME\n" 237 " type: LAYER_TYPE\n" 238 " parameters: values\n" 240 throw std::runtime_error(
"layers parameter must contain at least one layer config.");
242 for (
int i = 0; i < layers_xml.
size(); ++i)
244 auto layer_xml = std::pair<std::string, XmlRpc::XmlRpcValue>(
245 layers_xml[i][
"name"], layers_xml[i]);
246 ROS_INFO(
"New static layer: %s", layer_xml.first.c_str());
251 layer_xml.second[
"overlay_mode"]);
253 ROS_WARN(
"overlay_mode of the static layer is not specified. Using MAX mode.");
257 type = std::string(layer_xml.second[
"type"]);
260 ROS_FATAL(
"Layer type is not specified.");
261 throw std::runtime_error(
"Layer type is not specified.");
264 if (!layer_xml.second.hasMember(
"footprint"))
265 layer_xml.second[
"footprint"] = footprint_xml;
269 costmap_->addLayer(layer, overlay_mode);
270 layer->loadConfig(layer_xml.second);
272 sub_map_overlay_.push_back(nh_.subscribe<nav_msgs::OccupancyGrid>(
281 sub_map_ = nh_.subscribe<nav_msgs::OccupancyGrid>(
285 if (pnh_.hasParam(
"layers"))
288 pnh_.getParam(
"layers", layers_xml);
292 ROS_FATAL(
"layers parameter must contain at least one layer config.");
294 "Migration from old version:\n" 297 " YOUR_LAYER_NAME:\n" 298 " type: LAYER_TYPE\n" 299 " parameters: values\n" 302 " - name: YOUR_LAYER_NAME\n" 303 " type: LAYER_TYPE\n" 304 " parameters: values\n" 306 throw std::runtime_error(
"layers parameter must contain at least one layer config.");
308 for (
int i = 0; i < layers_xml.
size(); ++i)
310 auto layer_xml = std::pair<std::string, XmlRpc::XmlRpcValue>(
311 layers_xml[i][
"name"], layers_xml[i]);
312 ROS_INFO(
"New layer: %s", layer_xml.first.c_str());
317 layer_xml.second[
"overlay_mode"]);
319 ROS_WARN(
"overlay_mode of the layer is not specified. Using MAX mode.");
323 type = std::string(layer_xml.second[
"type"]);
326 ROS_FATAL(
"Layer type is not specified.");
327 throw std::runtime_error(
"Layer type is not specified.");
330 if (!layer_xml.second.hasMember(
"footprint"))
331 layer_xml.second[
"footprint"] = footprint_xml;
335 costmap_->addLayer(layer, overlay_mode);
336 layer->loadConfig(layer_xml.second);
338 sub_map_overlay_.push_back(nh_.subscribe<nav_msgs::OccupancyGrid>(
347 std::string overlay_mode_str;
348 pnh_.param(
"overlay_mode", overlay_mode_str, std::string(
"max"));
349 if (overlay_mode_str.compare(
"overwrite") == 0)
351 else if (overlay_mode_str.compare(
"max") == 0)
355 ROS_FATAL(
"Unknown overlay_mode \"%s\"", overlay_mode_str.c_str());
356 throw std::runtime_error(
"Unknown overlay_mode.");
358 ROS_INFO(
"costmap_3d: %s mode", overlay_mode_str.c_str());
361 layer_xml[
"footprint"] = footprint_xml;
362 layer_xml[
"linear_expand"] = linear_expand;
363 layer_xml[
"linear_spread"] = linear_spread;
367 sub_map_overlay_.push_back(nh_.subscribe<nav_msgs::OccupancyGrid>(
375 const geometry_msgs::PolygonStamped footprint_msg = footprint.
toMsg();
376 timer_footprint_ = nh_.createTimer(
382 int main(
int argc,
char* argv[])
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
geometry_msgs::PolygonStamped toMsg() const
void cbMapOverlay(const nav_msgs::OccupancyGrid::ConstPtr &msg, const costmap_cspace::Costmap3dLayerBase::Ptr map)
Type const & getType() const
std::vector< ros::Subscriber > sub_map_overlay_
bool cbUpdate(const costmap_cspace::CSpace3DMsg::Ptr map, const costmap_cspace_msgs::CSpace3DUpdate::Ptr update)
ROSCPP_DECL void spin(Spinner &spinner)
ros::Timer timer_footprint_
ros::Publisher pub_debug_
bool cbUpdateStatic(const costmap_cspace::CSpace3DMsg::Ptr map, const costmap_cspace_msgs::CSpace3DUpdate::Ptr update)
std::vector< std::pair< nav_msgs::OccupancyGrid::ConstPtr, costmap_cspace::Costmap3dLayerBase::Ptr > > map_buffer_
uint32_t getNumSubscribers() const
static costmap_cspace::MapOverlayMode getMapOverlayModeFromString(const std::string overlay_mode_str)
std::shared_ptr< CSpace3DMsg > Ptr
ros::Publisher pub_footprint_
void publishDebug(const costmap_cspace_msgs::CSpace3D &map)
costmap_cspace::Costmap3d::Ptr costmap_
std::shared_ptr< Costmap3dLayerBase > Ptr
ros::Publisher pub_costmap_update_
void cbPublishFootprint(const ros::TimerEvent &event, const geometry_msgs::PolygonStamped msg)
void setHandler(Callback cb)
std::shared_ptr< Costmap3d > Ptr
ros::Publisher pub_costmap_
int main(int argc, char *argv[])
static Costmap3dLayerBase::Ptr loadClass(const std::string &name)
void cbMap(const nav_msgs::OccupancyGrid::ConstPtr &msg, const costmap_cspace::Costmap3dLayerBase::Ptr map)