map_grid_visualizer.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Eric Perko
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Eric Perko nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
36 #include <vector>
37 
39 
40 namespace base_local_planner {
42 
43 
44  void MapGridVisualizer::initialize(const std::string& name, std::string frame_id, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function) {
45  name_ = name;
46  cost_function_ = cost_function;
47 
48  ns_nh_ = ros::NodeHandle("~/" + name_);
49 
50  cost_cloud_ = new pcl::PointCloud<MapGridCostPoint>;
51  cost_cloud_->header.frame_id = frame_id;
52  pub_.advertise(ns_nh_, "cost_cloud", 1);
53  }
54 
56  unsigned int x_size = costmap_p_->getSizeInCellsX();
57  unsigned int y_size = costmap_p_->getSizeInCellsY();
58  double z_coord = 0.0;
59  double x_coord, y_coord;
61  cost_cloud_->points.clear();
62  std_msgs::Header header = pcl_conversions::fromPCL(cost_cloud_->header);
63  header.stamp = ros::Time::now();
64  cost_cloud_->header = pcl_conversions::toPCL(header);
65  float path_cost, goal_cost, occ_cost, total_cost;
66  for (unsigned int cx = 0; cx < x_size; cx++) {
67  for (unsigned int cy = 0; cy < y_size; cy++) {
68  costmap_p_->mapToWorld(cx, cy, x_coord, y_coord);
69  if (cost_function_(cx, cy, path_cost, goal_cost, occ_cost, total_cost)) {
70  pt.x = x_coord;
71  pt.y = y_coord;
72  pt.z = z_coord;
73  pt.path_cost = path_cost;
74  pt.goal_cost = goal_cost;
75  pt.occ_cost = occ_cost;
76  pt.total_cost = total_cost;
77  cost_cloud_->push_back(pt);
78  }
79  }
80  }
81  pub_.publish(*cost_cloud_);
82  ROS_DEBUG("Cost PointCloud published");
83  }
84 };
pcl::PointCloud< MapGridCostPoint > * cost_cloud_
std::string name_
The name to get parameters relative to.
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
void publishCostCloud(const costmap_2d::Costmap2D *costmap_p_)
Build and publish a PointCloud if the publish_cost_grid_pc parameter was true. Only include points fo...
pcl_ros::Publisher< MapGridCostPoint > pub_
void initialize(const std::string &name, std::string frame, boost::function< bool(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function)
Initializes the MapGridVisualizer.
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
boost::function< bool(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function_
The function to be used to generate the cost components for the output PointCloud.
unsigned int getSizeInCellsY() const
unsigned int getSizeInCellsX() const
static Time now()
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
#define ROS_DEBUG(...)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49