GridMapVisualization.cpp
Go to the documentation of this file.
1 /*
2  * GridMapVisualization.cpp
3  *
4  * Created on: Nov 19, 2013
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
12 
13 using namespace std;
14 using namespace ros;
15 
16 namespace grid_map_visualization {
17 
18 GridMapVisualization::GridMapVisualization(ros::NodeHandle& nodeHandle,
19  const std::string& parameterName)
20  : nodeHandle_(nodeHandle),
21  visualizationsParameter_(parameterName),
22  factory_(nodeHandle_),
23  isSubscribed_(false)
24 {
25  ROS_INFO("Grid map visualization node started.");
29  this);
30  initialize();
31 }
32 
34 {
35 }
36 
38 {
39  nodeHandle_.param("grid_map_topic", mapTopic_, string("/grid_map"));
40 
41  double activityCheckRate;
42  nodeHandle_.param("activity_check_rate", activityCheckRate, 2.0);
43  activityCheckDuration_.fromSec(1.0 / activityCheckRate);
45 
46  // Configure the visualizations from a configuration stored on the parameter server.
47  XmlRpc::XmlRpcValue config;
49  ROS_WARN(
50  "Could not load the visualizations configuration from parameter %s,are you sure it"
51  "was pushed to the parameter server? Assuming that you meant to leave it empty.",
52  visualizationsParameter_.c_str());
53  return false;
54  }
55 
56  // Verify proper naming and structure,
57  if (config.getType() != XmlRpc::XmlRpcValue::TypeArray) {
58  ROS_ERROR("%s: The visualization specification must be a list, but it is of XmlRpcType %d",
59  visualizationsParameter_.c_str(), config.getType());
60  ROS_ERROR("The XML passed in is formatted as follows:\n %s", config.toXml().c_str());
61  return false;
62  }
63 
64  // Iterate over all visualizations (may be just one),
65  for (unsigned int i = 0; i < config.size(); ++i) {
66  if (config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
67  ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
68  visualizationsParameter_.c_str(), config[i].getType());
69  return false;
70  } else if (!config[i].hasMember("type")) {
71  ROS_ERROR("%s: Could not add a visualization because no type was given",
72  visualizationsParameter_.c_str());
73  return false;
74  } else if (!config[i].hasMember("name")) {
75  ROS_ERROR("%s: Could not add a visualization because no name was given",
76  visualizationsParameter_.c_str());
77  return false;
78  } else {
79  //Check for name collisions within the list itself.
80  for (int j = i + 1; j < config.size(); ++j) {
81  if (config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
82  ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
83  visualizationsParameter_.c_str(), config[j].getType());
84  return false;
85  }
86 
87  if (!config[j].hasMember("name")
88  || config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
89  || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString) {
90  ROS_ERROR("%s: Visualizations names must be strings, but they are XmlRpcTypes:%d and %d",
91  visualizationsParameter_.c_str(), config[i].getType(), config[j].getType());
92  return false;
93  }
94 
95  std::string namei = config[i]["name"];
96  std::string namej = config[j]["name"];
97  if (namei == namej) {
98  ROS_ERROR("%s: A visualization with the name '%s' already exists.",
99  visualizationsParameter_.c_str(), namei.c_str());
100  return false;
101  }
102  }
103  }
104 
105  // Make sure the visualization has a valid type.
106  if (!factory_.isValidType(config[i]["type"])) {
107  ROS_ERROR("Could not find visualization of type '%s'.", std::string(config[i]["type"]).c_str());
108  return false;
109  }
110  }
111 
112  for (int i = 0; i < config.size(); ++i) {
113  std::string type = config[i]["type"];
114  std::string name = config[i]["name"];
115  auto visualization = factory_.getInstance(type, name);
116  visualization->readParameters(config[i]);
117  visualizations_.push_back(visualization);
118  ROS_INFO("%s: Configured visualization of type '%s' with name '%s'.",
119  visualizationsParameter_.c_str(), type.c_str(), name.c_str());
120  }
121 
122  return true;
123 }
124 
126 {
127  for (auto& visualization : visualizations_) {
128  visualization->initialize();
129  }
131  ROS_INFO("Grid map visualization initialized.");
132  return true;
133 }
134 
136 {
137  bool isActive = false;
138 
139  for (auto& visualization : visualizations_) {
140  if (visualization->isActive()) {
141  isActive = true;
142  break;
143  }
144  }
145 
146  if (!isSubscribed_ && isActive) {
148  isSubscribed_ = true;
149  ROS_DEBUG("Subscribed to grid map at '%s'.", mapTopic_.c_str());
150  }
151  if (isSubscribed_ && !isActive) {
153  isSubscribed_ = false;
154  ROS_DEBUG("Cancelled subscription to grid map.");
155  }
156 }
157 
158 void GridMapVisualization::callback(const grid_map_msgs::GridMap& message)
159 {
160  ROS_DEBUG("Grid map visualization received a map (timestamp %f) for visualization.",
161  message.info.header.stamp.toSec());
162  grid_map::GridMap map;
164 
165  for (auto& visualization : visualizations_) {
166  visualization->visualize(map);
167  }
168 }
169 
170 } /* namespace */
std::vector< std::shared_ptr< VisualizationBase > > visualizations_
List of visualizations.
std::shared_ptr< VisualizationBase > getInstance(const std::string &type, const std::string &name)
ros::Timer activityCheckTimer_
Timer to check the activity of the visualizations.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool isSubscribed_
If the grid map visualization is subscribed to the grid map.
VisualizationFactory factory_
Visualization factory.
#define ROS_WARN(...)
static bool fromMessage(const grid_map_msgs::GridMap &message, grid_map::GridMap &gridMap)
Duration & fromSec(double t)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ros::Subscriber mapSubscriber_
ROS subscriber to the grid map.
std::string mapTopic_
Topic name of the grid map to be visualized.
bool getParam(const std::string &key, std::string &s) const
void updateSubscriptionCallback(const ros::TimerEvent &timerEvent)
#define ROS_ASSERT(cond)
#define ROS_ERROR(...)
ros::Duration activityCheckDuration_
Duration of checking the activity of the visualizations.
void callback(const grid_map_msgs::GridMap &message)
ros::NodeHandle & nodeHandle_
ROS nodehandle.
std::string visualizationsParameter_
Parameter name of the visualizer configuration list.
#define ROS_DEBUG(...)


grid_map_visualization
Author(s): Péter Fankhauser
autogenerated on Tue Jun 25 2019 20:02:32