router_node.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, <copyright holder> <email>
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the <organization> nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY <copyright holder> <email> ''AS IS'' AND ANY
17  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL <copyright holder> <email> BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27  */
28 #define POT_HIGH 1.0e10
29 
32 #include <tuw_multi_robot_msgs/Route.h>
33 #include <chrono>
34 #include <boost/functional/hash.hpp>
35 #include <boost/regex.hpp>
36 #include <tf/tf.h>
37 
38 //TODO add Weights from robots...
39 
40 int main ( int argc, char **argv ) {
41 
42  ros::init ( argc, argv, "tuw_multi_robot_router" );
44 
45  ros::Rate r ( 1 );
46 
48 
49  while ( ros::ok() ) {
50  r.sleep();
51  ros::spinOnce();
52  node.monitorExecution();
53  node.updateTimeout ( r.expectedCycleTime().toSec() );
54  }
55 
56  return 0;
57 }
58 
59 
60 namespace multi_robot_router {
61 
63  n_ ( _n ),
64  n_param_ ( "~" ),
65  monitor_enabled_ ( false ),
66  attempts_total_(0),
67  attempts_successful_(0),
68  sum_processing_time_total_(.0),
69  sum_processing_time_successful_(.0){
70  id_ = 0;
71 
72  n_param_.param<std::string> ( "planner_status_topic", planner_status_topic_, "planner_status" );
73 
74  n_param_.param<std::string> ( "goal_topic", goal_topic_, "goals" );
75 
76  n_param_.param<std::string> ( "map_topic", map_topic_, "/map" );
77 
78  n_param_.param<std::string> ( "graph_topic", voronoi_topic_, "segments" );
79 
80  n_param_.param<std::string> ( "robot_info", robot_info_topic_, "/robot_info" );
81 
82  n_param_.param<std::string> ( "robot_name", singleRobotName_, "" );
83 
84  n_param_.param<std::string> ( "robot_goal", singleRobotGoalTopic_, "/goal" );
85 
86  // static subscriptions
87  subGoalSet_ = n_.subscribe ( goal_topic_, 1, &Router_Node::goalsCallback, this );
88  subMap_ = n_.subscribe ( map_topic_, 1, &Router_Node::mapCallback, this );
89  subVoronoiGraph_ = n_.subscribe ( voronoi_topic_, 1, &Router_Node::graphCallback, this );
90  subRobotInfo_ = n_.subscribe ( robot_info_topic_, 10000, &Router_Node::robotInfoCallback, this );
91 
92  if ( !singleRobotName_.size() == 0 ) {
93  subSingleRobotGoal_ = n_.subscribe ( singleRobotGoalTopic_, 1, &Router_Node::goalCallback, this );
94  }
95 
96  //static publishers
97  pubPlannerStatus_ = n_.advertise<tuw_multi_robot_msgs::RouterStatus> ( planner_status_topic_, 1 );
98 
99  //dynamic reconfigure
100  call_type = boost::bind ( &Router_Node::parametersCallback, this, _1, _2 );
101  param_server.setCallback ( call_type );
102 }
103 
105  for ( const RobotInfoPtr robot: active_robots_ ) {
106  std::map<std::string, double>::iterator it = finished_robots_.find ( robot->robot_name );
107  if ( robot->status == tuw_multi_robot_msgs::RobotInfo::STATUS_DRIVING ) {
108  if ( it != finished_robots_.end() ) {
109  finished_robots_.erase ( it );
110  if ( monitor_enabled_ ) {
111  ROS_INFO ( "%10s started!", robot->robot_name.c_str() );
112  }
113  }
114  } else {
115  if ( it == finished_robots_.end() ) {
116  double duration = (ros::Time::now() - time_first_robot_started_).toSec();
117  finished_robots_[robot->robot_name] = duration;
118  int nr_of_driving_robots = active_robots_.size() - finished_robots_.size();
119  if ( monitor_enabled_ ) {
120  ROS_INFO ( "%10s stopped @ %6.2lf sec, %3i robots left", robot->robot_name.c_str(), duration, nr_of_driving_robots );
121  }
122  }
123  }
124  }
125  if ( finished_robots_.size() == active_robots_.size() ) {
126  if ( monitor_enabled_ ) {
128  ROS_INFO ( "Execution finished after %lf sec!", duration.toSec() );
129  std::stringstream ss;
130  for(std::map<std::string, double>::iterator it = finished_robots_.begin(); it!=finished_robots_.end(); ++it){
131  ss << it->second << ", ";
132  }
133  ROS_INFO ( "Duration by robot: \n [%s]", ss.str().c_str() );
134  }
135  monitor_enabled_ = false;
136  } else {
137  monitor_enabled_ = true;
138  }
139 
140 }
141 
142 void Router_Node::goalCallback ( const geometry_msgs::PoseStamped &_goal ) {
143  tuw_multi_robot_msgs::RobotGoals goal;
144  goal.robot_name = singleRobotName_;
145  goal.destinations.push_back ( _goal.pose );
146 
147  tuw_multi_robot_msgs::RobotGoalsArray goals;
148  goals.robots.push_back ( goal );
149 
150  goalsCallback ( goals );
151 }
152 
153 void Router_Node::updateTimeout ( const float _secs ) {
154  //Todo update timeouts and clear old messages
155  for ( auto it = subscribed_robots_.begin(); it != subscribed_robots_.end(); it++ ) {
156  ( *it )->updateOnlineStatus ( _secs );
157  }
158 }
159 void Router_Node::parametersCallback ( tuw_multi_robot_router::routerConfig &config, uint32_t level ) {
160  //Important set router before settings
161  uint32_t threads = config.nr_threads;
162  if ( config.router_type == 1 )
164  else
166 
167  if ( config.collision_resolver == 0 ){
169  collisionResolver_ = false;
170  } else if ( config.collision_resolver == 1 ) {
172  collisionResolver_ = true;
173  }else{
175  collisionResolver_ = true;
176  }
177 
178  if ( config.voronoi_graph )
180  else
182 
183  if ( config.goal_mode == 0 )
185  else if ( config.goal_mode == 1 )
187  else
189 
190  routerTimeLimit_s_ = config.router_time_limit_s;
191  topic_timeout_s_ = config.topic_timeout_s;
192 
193  priorityRescheduling_ = config.priority_rescheduling;
194  speedRescheduling_ = config.speed_rescheduling;
195  segmentOptimizations_ = config.path_endpoint_optimization;
196  publish_routing_table_ = config.publish_routing_table;
197 }
198 
199 void Router_Node::mapCallback ( const nav_msgs::OccupancyGrid &_map ) {
200  std::vector<signed char> map = _map.data;
201 
202  Eigen::Vector2d origin;
203  origin[0] = _map.info.origin.position.x;
204  origin[1] = _map.info.origin.position.y;
205 
206  size_t new_hash = getHash ( map, origin, _map.info.resolution );
207 
208  ROS_INFO ( "map %f %f %f", origin[0], origin[1], _map.info.resolution );
209 
210  if ( new_hash != current_map_hash_ ) {
211  mapOrigin_[0] = _map.info.origin.position.x;
212  mapOrigin_[1] = _map.info.origin.position.y;
213  mapResolution_ = _map.info.resolution;
214 
215  cv::Mat m ( _map.info.height, _map.info.width, CV_8SC1, map.data() );
216 
217  m.convertTo ( m, CV_8UC1 );
218  cv::bitwise_not ( m, m );
219 
220  cv::threshold ( m, m, 40, 255, CV_THRESH_BINARY | CV_THRESH_OTSU );
221  cv::distanceTransform ( m, distMap_, CV_DIST_L1, 3 );
222 
223  current_map_hash_ = new_hash;
224  got_map_ = true;
225 
226  ROS_INFO ( "%s: New Map %i %i %lu", n_param_.getNamespace().c_str() , _map.info.width, _map.info.height, current_map_hash_ );
227  }
228 }
229 
230 
231 float Router_Node::calcRadius ( const int shape, const std::vector<float> &shape_variables ) const {
232  tuw_multi_robot_msgs::RobotInfo ri;
233  if ( shape == ri.SHAPE_CIRCLE ) {
234  return shape_variables[0];
235  }
236 
237  return -1;
238 }
239 
240 void Router_Node::robotInfoCallback ( const tuw_multi_robot_msgs::RobotInfo &_robotInfo ) {
241 
242  auto robot = RobotInfo::findObj ( subscribed_robots_, _robotInfo.robot_name );
243  if ( robot == subscribed_robots_.end() ) {
244  // create new entry
245  RobotInfoPtr robot_new = std::make_shared<RobotInfo> ( _robotInfo );
246  robot_new->initTopics ( n_ );
247  subscribed_robots_.push_back ( robot_new );
248  } else {
249  ( *robot )->updateInfo ( _robotInfo );
250  }
251 
252  robot_radius_max_ = 0;
253  for ( RobotInfoPtr &r: subscribed_robots_ ) {
254  if ( r->radius() > robot_radius_max_ )
255  robot_radius_max_ = r->radius();
256  }
257 }
258 
259 void Router_Node::graphCallback ( const tuw_multi_robot_msgs::Graph &msg ) {
260  std::vector<Segment> graph;
261 
262  for ( const tuw_multi_robot_msgs::Vertex &segment : msg.vertices ) {
263  std::vector<Eigen::Vector2d> points;
264 
265  for ( const geometry_msgs::Point &point : segment.path ) {
266  points.emplace_back ( point.x, point.y );
267  }
268 
269  std::vector<uint32_t> successors;
270 
271  for ( const auto &succ : segment.successors ) {
272  successors.emplace_back ( succ );
273  }
274 
275  std::vector<uint32_t> predecessors;
276 
277  for ( const auto &pred : segment.predecessors ) {
278  predecessors.emplace_back ( pred );
279  }
280 
281  if ( segment.valid ) {
282  graph.emplace_back ( segment.id, points, successors, predecessors, 2 * robot_radius_max_ / mapResolution_ ); //segment.width);
283  } else {
284  graph.emplace_back ( segment.id, points, successors, predecessors, 0 );
285  }
286  }
287 
288  std::sort ( graph.begin(), graph.end(), sortSegments );
289 
290  size_t hash = getHash ( graph );
291 
292  if ( current_graph_hash_ != hash ) {
293  current_graph_hash_ = hash;
294  graph_ = graph;
295  ROS_INFO ( "%s: Graph %lu", n_param_.getNamespace().c_str() , hash );
296  }
297  got_graph_ = true;
298 }
299 
300 bool Router_Node::preparePlanning ( std::vector<float> &_radius, std::vector<Eigen::Vector3d> &_starts, std::vector<Eigen::Vector3d> &_goals, const tuw_multi_robot_msgs::RobotGoalsArray &goal_msg, std::vector<std::string> &robot_names ) {
301  bool retval = true;
302  active_robots_.clear();
303  _starts.clear();
304  _goals.clear();
305  _radius.clear();
306 
307  for ( int k = 0; k < goal_msg.robots.size(); k++ ) {
308  const tuw_multi_robot_msgs::RobotGoals &route_request = goal_msg.robots[k];
309  RobotInfoPtrIterator active_robot = RobotInfo::findObj ( subscribed_robots_, route_request.robot_name );
310  if ( active_robot == subscribed_robots_.end() ) {
311  ROS_INFO ( "No robot subsribed with the name: %s", route_request.robot_name.c_str() );
312  } else {
313  if ( route_request.destinations.empty() ) {
314  ROS_INFO ( "No robot: %s has not goal defined", route_request.robot_name.c_str() );
315  continue;
316  } else {
317 
318  active_robots_.push_back ( *active_robot );
319 
320  _radius.push_back ( ( *active_robot )->radius() );
321  if ( route_request.destinations.size() > 1 ) {
322  geometry_msgs::Pose p = route_request.destinations[0];
323  _starts.push_back ( Eigen::Vector3d ( p.position.x, p.position.y, getYaw ( p.orientation ) ) );
324  } else {
325  _starts.push_back ( ( *active_robot )->getPose() );
326  }
327 
328  geometry_msgs::Pose p = route_request.destinations.back();
329  _goals.push_back ( Eigen::Vector3d ( p.position.x, p.position.y, getYaw ( p.orientation ) ) );
330  }
331 
332  }
333  }
334  robot_names.resize ( active_robots_.size() );
335  for ( size_t i=0; i < active_robots_.size(); i++ ) {
336  robot_names[i] = active_robots_[i]->robot_name;
337  }
338  return retval;
339 }
340 
341 void Router_Node::goalsCallback ( const tuw_multi_robot_msgs::RobotGoalsArray &_goals ) {
342  //Get robots
343  std::vector<Eigen::Vector3d> starts;
344  std::vector<Eigen::Vector3d> goals;
345  std::vector<float> radius;
346  std::vector<std::string> robot_names;
347 
348 
349 
350  bool preparationSuccessful = preparePlanning ( radius, starts, goals, _goals, robot_names );
351  ROS_INFO ( "%s: Number of active robots %lu", n_param_.getNamespace().c_str(), active_robots_.size() );
352 
353  if ( preparationSuccessful && got_map_ && got_graph_ ) {
354  attempts_total_++;
355  auto t1 = std::chrono::high_resolution_clock::now();
356  preparationSuccessful &= makePlan ( starts, goals, radius, distMap_, mapResolution_, mapOrigin_, graph_, robot_names );
357  auto t2 = std::chrono::high_resolution_clock::now();
358  int duration = std::chrono::duration_cast<std::chrono::milliseconds> ( t2 - t1 ).count();
359  sum_processing_time_total_ += duration;
360  if ( preparationSuccessful ) {
361  int nx = distMap_.cols;
362  int ny = distMap_.rows;
363 
364  double res = mapResolution_;
365  int cx = mapOrigin_[0];
366  int cy = mapOrigin_[1];
367 
368  publish();
371  freshPlan_ = false;
372  } else {
373  publishEmpty();
374  }
375  float rate_success = ((float) attempts_successful_) / (float) attempts_total_;
376  float avr_duration_total = sum_processing_time_total_ / (float) attempts_total_;
377  float avr_duration_successful = sum_processing_time_successful_ / (float) attempts_successful_;
378  ROS_INFO ( "\nSuccess %i, %i = %4.3f, avr %4.0f ms, success: %4.0f ms, %s, %s, %s \n [%4.3f, %4.0f, %4.0f]",
379  attempts_successful_, attempts_total_, rate_success, avr_duration_total, avr_duration_successful,
380  (priorityRescheduling_?"PR= on":"PR= off"), (speedRescheduling_?"SR= on":"SR= off"), (collisionResolver_?"CR= on":"CR= off"),
381  rate_success, avr_duration_total, avr_duration_successful);
382 
383 
384  id_++;
385  } else if ( !got_map_ || !got_graph_ ) {
386  publishEmpty();
387  ROS_INFO ( "%s: Multi Robot Router: No Map or Graph received", n_param_.getNamespace().c_str() );
388  } else {
389  publishEmpty();
390  }
391 }
392 
393 float Router_Node::getYaw ( const geometry_msgs::Quaternion &_rot ) {
394  double roll, pitch, yaw;
395 
396  tf::Quaternion q ( _rot.x, _rot.y, _rot.z, _rot.w );
397  tf::Matrix3x3 ( q ).getRPY ( roll, pitch, yaw );
398  return yaw;
399 }
400 
402  if(publish_routing_table_ == false) return;
403  ROS_INFO ( "%s: No Plan found!!!!, publishing empty plan", n_param_.getNamespace().c_str());
405  finished_robots_.clear();
406  nav_msgs::Path msg_path;
407  tuw_multi_robot_msgs::Route msg_route;
408  msg_path.header.seq = 0;
409  msg_path.header.stamp = time_first_robot_started_;
410  msg_path.header.frame_id = "map";
411  msg_route.header = msg_path.header;
412 
413  for ( RobotInfoPtr &robot: subscribed_robots_ ) {
414  robot->pubPaths_.publish ( msg_path );
415  robot->pubRoute_.publish ( msg_route );
416  }
417 
418  mrrp_status_.id = id_;
419  mrrp_status_.success = 0;
420  mrrp_status_.duration = getDuration_ms();
422 }
423 
425  if(publish_routing_table_ == false) return;
426  ROS_INFO ( "%s: Plan found :-), publishing plan", n_param_.getNamespace().c_str());
428  finished_robots_.clear();
429  nav_msgs::Path msg_path;
430  tuw_multi_robot_msgs::Route msg_route;
431  msg_path.header.seq = 0;
432  msg_path.header.stamp = time_first_robot_started_;
433  msg_path.header.frame_id = "map";
434  msg_route.header = msg_path.header;
435 
436  for ( int i = 0; i < active_robots_.size(); i++ ) {
437  RobotInfoPtr robot = active_robots_[i];
438  const std::vector<Checkpoint> &route = getRoute ( i );
439  msg_path.poses.clear();
440 
441  //Add first point
442  geometry_msgs::PoseStamped pose_1;
443  pose_1.header.seq = 0;
444  pose_1.header.stamp = time_first_robot_started_;
445  pose_1.header.frame_id = "map";
446 
447  Eigen::Vector2d pos ( route[0].start[0] * mapResolution_, route[0].start[1] * mapResolution_ );
448  pose_1.pose.position.x = pos[0] + mapOrigin_[0];
449  pose_1.pose.position.y = pos[1] + mapOrigin_[1];
450 
451  pose_1.pose.orientation.w = 1;
452  msg_path.poses.push_back ( pose_1 );
453 
454  //Add other points
455  for ( const Checkpoint &c : route ) {
456  geometry_msgs::PoseStamped pose;
457  pose.header.seq = 0;
458  pose.header.stamp = time_first_robot_started_;
459  pose.header.frame_id = "map";
460 
461  Eigen::Vector2d pos ( c.end[0] * mapResolution_, c.end[1] * mapResolution_ );
462  pose.pose.position.x = pos[0] + mapOrigin_[0];
463  pose.pose.position.y = pos[1] + mapOrigin_[1];
464 
466  q.setEuler ( 0, 0, c.end[2] );
467 
468  pose.pose.orientation.w = q.w();
469  pose.pose.orientation.x = q.x();
470  pose.pose.orientation.y = q.y();
471  pose.pose.orientation.z = q.z();
472  msg_path.poses.push_back ( pose );
473  }
474 
475  robot->pubPaths_.publish ( msg_path );
476 
477 
478  msg_route.segments.clear();
479 
480  for ( const Checkpoint &cp : route ) {
481  tuw_multi_robot_msgs::RouteSegment seg;
482 
483  Eigen::Vector2d posStart ( cp.start[0] * mapResolution_, cp.start[1] * mapResolution_ );
484  tf::Quaternion qStart;
485  qStart.setEuler ( 0, 0, cp.start[2] );
486 
487  seg.start.position.x = posStart[0] + mapOrigin_[0];
488  seg.start.position.y = posStart[1] + mapOrigin_[1];
489  seg.start.orientation.w = qStart.w();
490  seg.start.orientation.x = qStart.x();
491  seg.start.orientation.y = qStart.y();
492  seg.start.orientation.z = qStart.z();
493 
494  Eigen::Vector2d posEnd ( cp.end[0] * mapResolution_, cp.end[1] * mapResolution_ );
495  tf::Quaternion qEnd;
496  qEnd.setEuler ( 0, 0, cp.end[2] );
497 
498  seg.end.position.x = posEnd[0] + mapOrigin_[0];
499  seg.end.position.y = posEnd[1] + mapOrigin_[1];
500  seg.end.orientation.w = qEnd.w();
501  seg.end.orientation.x = qEnd.x();
502  seg.end.orientation.y = qEnd.y();
503  seg.end.orientation.z = qEnd.z();
504 
505  seg.segment_id = cp.segId;
506  seg.width = graph_[cp.segId].width() * mapResolution_;
507 
508  for ( int j = 0; j < cp.preconditions.size(); j++ ) {
509  tuw_multi_robot_msgs::RoutePrecondition pc;
510  pc.robot_id = active_robots_[cp.preconditions[j].robotId]->robot_name;
511  pc.current_route_segment = cp.preconditions[j].stepCondition;
512  seg.preconditions.push_back ( pc );
513  }
514 
515  msg_route.segments.push_back ( seg );
516  }
517 
518  robot->pubRoute_.publish ( msg_route );
519  }
520 
521  tuw_multi_robot_msgs::RouterStatus ps;
522  ps.id = id_;
523  ps.success = 1;
524  ps.overall_path_length = ( int32_t ) getOverallPathLength();
525  ps.longest_path_length = ( int32_t ) getLongestPathLength();
526  ps.priority_scheduling_attemps = ( int32_t ) getPriorityScheduleAttemps();
527  ps.speed_scheduling_attemps = ( int32_t ) getSpeedScheduleAttemps();
528  ps.duration = ( int32_t ) getDuration_ms();
529 
530  pubPlannerStatus_.publish ( ps );
531 }
532 
533 size_t Router_Node::getHash ( const std::vector<signed char> &_map, const Eigen::Vector2d &_origin, const float &_resolution ) {
534  std::size_t seed = 0;
535 
536  boost::hash_combine ( seed, _origin[0] );
537  boost::hash_combine ( seed, _origin[1] );
538  boost::hash_combine ( seed, _resolution );
539 
540  for ( const signed char &val : _map ) {
541  boost::hash_combine ( seed, val );
542  }
543 
544  return seed;
545 }
546 
547 std::size_t Router_Node::getHash ( const std::vector<Segment> &_graph ) {
548  std::size_t seed = 0;
549 
550  for ( const Segment &seg : _graph ) {
551  boost::hash_combine ( seed, seg.width() );
552  boost::hash_combine ( seed, seg.length() );
553  boost::hash_combine ( seed, seg.getSegmentId() );
554 
555  for ( const int &p : seg.getPredecessors() ) {
556  boost::hash_combine ( seed, p );
557  }
558 
559  for ( const int &s : seg.getSuccessors() ) {
560  boost::hash_combine ( seed, s );
561  }
562 
563  for ( const Eigen::Vector2d &vec : seg.getPoints() ) {
564  boost::hash_combine ( seed, vec[0] );
565  boost::hash_combine ( seed, vec[1] );
566  }
567  }
568 
569  return seed;
570 }
571 
572 
573 
574 
575 } // namespace multi_robot_router
576 
577 
void goalsCallback(const tuw_multi_robot_msgs::RobotGoalsArray &_goals)
static std::vector< std::shared_ptr< RobotInfo > >::iterator findObj(std::vector< std::shared_ptr< RobotInfo > > &data, const std::string &name)
Definition: robot_info.cpp:84
float getYaw(const geometry_msgs::Quaternion &_rot)
dynamic_reconfigure::Server< tuw_multi_robot_router::routerConfig > param_server
Definition: router_node.h:93
float getOverallPathLength() const
getter
Definition: router.cpp:474
void parametersCallback(tuw_multi_robot_router::routerConfig &config, uint32_t level)
int main(int argc, char **argv)
Definition: router_node.cpp:40
void publish(const boost::shared_ptr< M > &message) const
std::vector< RobotInfoPtr > subscribed_robots_
Definition: router_node.h:104
void monitorExecution()
monitors the execution
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
XmlRpcServer s
ros::Publisher pubPlannerStatus_
Definition: router_node.h:95
void setPlannerType(routerType _type, uint32_t _nr_threads)
Definition: router.cpp:59
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Router_Node(ros::NodeHandle &n)
Construct a new Router_Node object.
Definition: router_node.cpp:62
std::vector< std::shared_ptr< RobotInfo > >::iterator RobotInfoPtrIterator
Definition: robot_info.h:124
ros::Subscriber subVoronoiGraph_
Definition: router_node.h:101
size_t getHash(const std::vector< signed char > &_map, const Eigen::Vector2d &_origin, const float &_resolution)
void mapCallback(const nav_msgs::OccupancyGrid &_map)
void publish()
publishes a RoutingTable
tuw_multi_robot_msgs::RouterStatus mrrp_status_
Definition: router_node.h:91
ros::Subscriber subSingleRobotGoal_
Definition: router_node.h:100
ros::NodeHandle n_
Node handler to the root node.
Definition: router_node.h:79
ros::NodeHandle n_param_
Node handler to the current node.
Definition: router_node.h:80
const std::vector< Checkpoint > & getRoute(const uint32_t _robot) const
returns the found Routing Table
Definition: router.cpp:459
float getLongestPathLength() const
getter
Definition: router.cpp:469
void robotInfoCallback(const tuw_multi_robot_msgs::RobotInfo &_robotInfo)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
Duration expectedCycleTime() const
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void graphCallback(const tuw_multi_robot_msgs::Graph &msg)
ROSCPP_DECL bool ok()
float calcRadius(const int shape, const std::vector< float > &shape_variables) const
void publishEmpty()
publishes an empty RoutingTable
const std::string & getNamespace() const
std::map< std::string, double > finished_robots_
robots currently used by the planner
Definition: router_node.h:106
bool makePlan(const std::vector< Eigen::Vector3d > &_starts, const std::vector< Eigen::Vector3d > &_goals, const std::vector< float > &_radius, const cv::Mat &_map, const float &_resolution, const Eigen::Vector2d &_origin, const std::vector< Segment > &_graph, const std::vector< std::string > &_robot_names)
generates the plan from (Vertex[odom robotPose] to Vertex[_goals]
Definition: router.cpp:328
static bool sortSegments(const Segment &i, const Segment &j)
Definition: router_node.h:142
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
uint32_t getPriorityScheduleAttemps() const
getter
Definition: router.cpp:479
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
bool sleep()
void setCollisionResolutionType(const SegmentExpander::CollisionResolverType _cr)
sets the CollisionResolverType used
Definition: router.cpp:54
dynamic_reconfigure::Server< tuw_multi_robot_router::routerConfig >::CallbackType call_type
Definition: router_node.h:94
bool preparePlanning(std::vector< float > &_radius, std::vector< Eigen::Vector3d > &_starts, std::vector< Eigen::Vector3d > &_goals, const tuw_multi_robot_msgs::RobotGoalsArray &_ros_goals, std::vector< std::string > &robot_names)
std::vector< Segment > graph_
Definition: router_node.h:124
static Time now()
std::shared_ptr< RobotInfo > RobotInfoPtr
Definition: robot_info.h:123
uint32_t getSpeedScheduleAttemps() const
getter
Definition: router.cpp:484
void goalCallback(const geometry_msgs::PoseStamped &_goal)
ROSCPP_DECL void spinOnce()
uint32_t getDuration_ms() const
getter
Definition: router.cpp:464
std::queue< Index > q
std::vector< RobotInfoPtr > active_robots_
robots avaliable
Definition: router_node.h:105
void updateTimeout(const float _secs)
used to update the nodes timeout to latch topics


tuw_multi_robot_router
Author(s): Benjamin Binder
autogenerated on Mon Jun 10 2019 15:42:49