RobotNavigator.cpp
Go to the documentation of this file.
1 #include <nav2d_operator/cmd.h>
2 #include <nav_msgs/GridCells.h>
3 #include <visualization_msgs/Marker.h>
4 
5 #include <nav2d_navigator/RobotNavigator.h>
6 #include <nav2d_navigator/ExplorationPlanner.h>
7 
8 #include <set>
9 #include <map>
10 
11 #define PI 3.14159265
12 #define FREQUENCY 5.0
13 
14 using namespace ros;
15 using namespace tf;
16 
17 RobotNavigator::RobotNavigator()
18 {
19  NodeHandle robotNode;
20 
21  std::string serviceName;
22  robotNode.param("map_service", serviceName, std::string("get_map"));
23  mGetMapClient = robotNode.serviceClient<nav_msgs::GetMap>(serviceName);
24 
25  mCommandPublisher = robotNode.advertise<nav2d_operator::cmd>("cmd", 1);
26  mStopServer = robotNode.advertiseService(NAV_STOP_SERVICE, &RobotNavigator::receiveStop, this);
27  mPauseServer = robotNode.advertiseService(NAV_PAUSE_SERVICE, &RobotNavigator::receivePause, this);
28  mCurrentPlan = NULL;
29 
30  NodeHandle navigatorNode("~/");
31  mPlanPublisher = navigatorNode.advertise<nav_msgs::GridCells>("plan", 1);
32  mMarkerPublisher = navigatorNode.advertise<visualization_msgs::Marker>("markers", 1, true);
33 
34  // Get parameters
35  navigatorNode.param("map_inflation_radius", mInflationRadius, 1.0);
36  navigatorNode.param("robot_radius", mRobotRadius, 0.3);
37  navigatorNode.param("exploration_strategy", mExplorationStrategy, std::string("NearestFrontierPlanner"));
38  navigatorNode.param("navigation_goal_distance", mNavigationGoalDistance, 1.0);
39  navigatorNode.param("navigation_goal_angle", mNavigationGoalAngle, 1.0);
40  navigatorNode.param("exploration_goal_distance", mExplorationGoalDistance, 3.0);
41  navigatorNode.param("navigation_homing_distance", mNavigationHomingDistance, 3.0);
42  navigatorNode.param("min_replanning_period", mMinReplanningPeriod, 3.0);
43  navigatorNode.param("max_replanning_period", mMaxReplanningPeriod, 1.0);
44  mCostObstacle = 100;
45  mCostLethal = (1.0 - (mRobotRadius / mInflationRadius)) * (double)mCostObstacle;
46 
47  robotNode.param("map_frame", mMapFrame, std::string("map"));
48  robotNode.param("robot_frame", mRobotFrame, std::string("robot"));
49  robotNode.param("robot_id", mRobotID, 1);
50  robotNode.param("move_action_topic", mMoveActionTopic, std::string(NAV_MOVE_ACTION));
51  robotNode.param("explore_action_topic", mExploreActionTopic, std::string(NAV_EXPLORE_ACTION));
52  robotNode.param("getmap_action_topic", mGetMapActionTopic, std::string(NAV_GETMAP_ACTION));
53  robotNode.param("localize_action_topic", mLocalizeActionTopic, std::string(NAV_LOCALIZE_ACTION));
54 
55  // Apply tf_prefix to all used frame-id's
56  mRobotFrame = mTfListener.resolve(mRobotFrame);
57  mMapFrame = mTfListener.resolve(mMapFrame);
58 
59  try
60  {
61  mPlanLoader = new PlanLoader("nav2d_navigator", "ExplorationPlanner");
62  mExplorationPlanner = mPlanLoader->createInstance(mExplorationStrategy);
63  ROS_INFO("Successfully loaded exploration strategy [%s].", mExplorationStrategy.c_str());
64 
65  mExploreActionServer = new ExploreActionServer(mExploreActionTopic, boost::bind(&RobotNavigator::receiveExploreGoal, this, _1), false);
66  mExploreActionServer->start();
67  }
68  catch(pluginlib::PluginlibException& ex)
69  {
70  ROS_ERROR("Failed to load exploration plugin! Error: %s", ex.what());
71  mExploreActionServer = NULL;
72  mPlanLoader = NULL;
73  }
74 
75  // Create action servers
76  mMoveActionServer = new MoveActionServer(mMoveActionTopic, boost::bind(&RobotNavigator::receiveMoveGoal, this, _1), false);
77  mMoveActionServer->start();
78 
79  mLocalizeActionServer = new LocalizeActionServer(mLocalizeActionTopic, boost::bind(&RobotNavigator::receiveLocalizeGoal, this, _1), false);
80  mLocalizeActionServer->start();
81 
82  if(mRobotID == 1)
83  {
84  mGetMapActionServer = new GetMapActionServer(mGetMapActionTopic, boost::bind(&RobotNavigator::receiveGetMapGoal, this, _1), false);
85  mGetMapActionServer->start();
86  }else
87  {
88  mGetMapActionServer = NULL;
89  }
90 
91  mHasNewMap = false;
92  mIsStopped = false;
93  mIsPaused = false;
94  mStatus = NAV_ST_IDLE;
95  mCellInflationRadius = 0;
96 }
97 
98 RobotNavigator::~RobotNavigator()
99 {
100  delete[] mCurrentPlan;
101  delete mMoveActionServer;
102  delete mExploreActionServer;
103  delete mGetMapActionServer;
104  mExplorationPlanner.reset();
105  delete mPlanLoader;
106 }
107 
108 bool RobotNavigator::getMap()
109 {
110  if(mHasNewMap) return true;
111 
112  if(!mGetMapClient.isValid())
113  {
114  ROS_ERROR("GetMap-Client is invalid!");
115  return false;
116  }
117 
118  nav_msgs::GetMap srv;
119  if(!mGetMapClient.call(srv))
120  {
121  ROS_INFO("Could not get a map.");
122  return false;
123  }
124  mCurrentMap.update(srv.response.map);
125 
126  if(mCurrentPlan) delete[] mCurrentPlan;
127  mCurrentPlan = new double[mCurrentMap.getSize()];
128 
129  if(mCellInflationRadius == 0)
130  {
131  ROS_INFO("Navigator is now initialized.");
132  mCellInflationRadius = mInflationRadius / mCurrentMap.getResolution();
133  mCellRobotRadius = mRobotRadius / mCurrentMap.getResolution();
134  mInflationTool.computeCaches(mCellInflationRadius);
135  mCurrentMap.setLethalCost(mCostLethal);
136  }
137 
138  mHasNewMap = true;
139  return true;
140 }
141 
142 bool RobotNavigator::receiveStop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
143 {
144  mIsStopped = true;
145  res.success = true;
146  res.message = "Navigator received stop signal.";
147  return true;
148 }
149 
150 bool RobotNavigator::receivePause(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
151 {
152  if(mIsPaused)
153  {
154  mIsPaused = false;
155  res.success = false;
156  res.message = "Navigator continues.";
157  }else
158  {
159  mIsPaused = true;
160  nav2d_operator::cmd stopMsg;
161  stopMsg.Turn = 0;
162  stopMsg.Velocity = 0;
163  mCommandPublisher.publish(stopMsg);
164  res.success = true;
165  res.message = "Navigator pauses.";
166  }
167  return true;
168 }
169 
170 typedef std::multimap<double,unsigned int> Queue;
171 typedef std::pair<double,unsigned int> Entry;
172 
173 bool RobotNavigator::preparePlan()
174 {
175  // Get the current map
176  if(!getMap()) // return false;
177  {
178  if(mCellInflationRadius == 0) return false;
179  ROS_WARN("Could not get a new map, trying to go with the old one...");
180  }
181 
182  // Where am I?
183  if(!setCurrentPosition()) return false;
184 
185  // Clear robot footprint in map
186  unsigned int x = 0, y = 0;
187  if(mCurrentMap.getCoordinates(x, y, mStartPoint))
188  for(int i = -mCellRobotRadius; i < (int)mCellRobotRadius; i++)
189  for(int j = -mCellRobotRadius; j < (int)mCellRobotRadius; j++)
190  mCurrentMap.setData(x+i, y+j, 0);
191 
192  mInflationTool.inflateMap(&mCurrentMap);
193  return true;
194 }
195 
196 bool RobotNavigator::createPlan()
197 {
198  ROS_DEBUG("Map-Value of goal point is %d, lethal threshold is %d.", mCurrentMap.getData(mGoalPoint), mCostLethal);
199 
200  unsigned int goal_x = 0, goal_y = 0;
201  if(mCurrentMap.getCoordinates(goal_x,goal_y,mGoalPoint))
202  {
203  visualization_msgs::Marker marker;
204  marker.header.frame_id = "/map";
205  marker.header.stamp = ros::Time();
206  marker.id = 0;
207  marker.type = visualization_msgs::Marker::CYLINDER;
208  marker.action = visualization_msgs::Marker::ADD;
209  marker.pose.position.x = mCurrentMap.getOriginX() + (((double)goal_x+0.5) * mCurrentMap.getResolution());
210  marker.pose.position.y = mCurrentMap.getOriginY() + (((double)goal_y+0.5) * mCurrentMap.getResolution());
211  marker.pose.position.z = 0.5;
212  marker.pose.orientation.x = 0.0;
213  marker.pose.orientation.y = 0.0;
214  marker.pose.orientation.z = 0.0;
215  marker.pose.orientation.w = 1.0;
216  marker.scale.x = mCurrentMap.getResolution() * 3.0;
217  marker.scale.y = mCurrentMap.getResolution() * 3.0;
218  marker.scale.z = 1.0;
219  marker.color.a = 1.0;
220  marker.color.r = 1.0;
221  marker.color.g = 0.0;
222  marker.color.b = 0.0;
223  mMarkerPublisher.publish(marker);
224  }else
225  {
226  ROS_ERROR("Couldn't ressolve goal point coordinates!");
227  }
228 
229  Queue queue;
230 
231  // Reset the plan
232  int mapSize = mCurrentMap.getSize();
233  for(int i = 0; i < mapSize; i++)
234  {
235  mCurrentPlan[i] = -1;
236  }
237 
238  if(mCurrentMap.isFree(mGoalPoint))
239  {
240  queue.insert(Entry(0.0, mGoalPoint));
241  mCurrentPlan[mGoalPoint] = 0;
242  }else
243  {
244  // Initialize the queue with area around the goal point
245  int reach = mCellRobotRadius + (1.0 / mCurrentMap.getResolution());
246  std::vector<unsigned int> neighbors = mCurrentMap.getFreeNeighbors(mGoalPoint, reach);
247  for(unsigned int i = 0; i < neighbors.size(); i++)
248  {
249  queue.insert(Entry(0.0, neighbors[i]));
250  mCurrentPlan[neighbors[i]] = 0;
251  }
252  }
253 
254  Queue::iterator next;
255  double distance;
256  unsigned int x, y, index;
257  double linear = mCurrentMap.getResolution();
258  double diagonal = std::sqrt(2.0) * linear;
259 
260  // Do full search with Dijkstra-Algorithm
261  while(!queue.empty())
262  {
263  // Get the nearest cell from the queue
264  next = queue.begin();
265  distance = next->first;
266  index = next->second;
267  queue.erase(next);
268 
269  if(mCurrentPlan[index] >= 0 && mCurrentPlan[index] < distance) continue;
270 
271 // if(index == mStartPoint) break;
272 
273  // Add all adjacent cells
274  if(!mCurrentMap.getCoordinates(x, y, index)) continue;
275  std::vector<unsigned int> ind;
276  ind.push_back(index - 1);
277  ind.push_back(index + 1);
278  ind.push_back(index - mCurrentMap.getWidth());
279  ind.push_back(index + mCurrentMap.getWidth());
280  ind.push_back(index - mCurrentMap.getWidth() - 1);
281  ind.push_back(index - mCurrentMap.getWidth() + 1);
282  ind.push_back(index + mCurrentMap.getWidth() - 1);
283  ind.push_back(index + mCurrentMap.getWidth() + 1);
284 
285  for(unsigned int it = 0; it < ind.size(); it++)
286  {
287  unsigned int i = ind[it];
288  if(mCurrentMap.isFree(i))
289  {
290  double delta = (it < 4) ? linear : diagonal;
291  double newDistance = distance + delta + (10 * delta * (double)mCurrentMap.getData(i) / (double)mCostObstacle);
292  if(mCurrentPlan[i] == -1 || newDistance < mCurrentPlan[i])
293  {
294  queue.insert(Entry(newDistance, i));
295  mCurrentPlan[i] = newDistance;
296  }
297  }
298  }
299  }
300 
301  if(mCurrentPlan[mStartPoint] < 0)
302  {
303  ROS_ERROR("No way between robot and goal!");
304  return false;
305  }
306 
307  publishPlan();
308  return true;
309 }
310 
311 void RobotNavigator::publishPlan()
312 {
313  nav_msgs::GridCells plan_msg;
314  plan_msg.header.frame_id = mMapFrame.c_str();
315  plan_msg.header.stamp = Time::now();
316 
317  plan_msg.cell_width = mCurrentMap.getResolution();
318  plan_msg.cell_height = mCurrentMap.getResolution();
319 
320  unsigned int index = mStartPoint;
321  std::vector<std::pair<double, double> > points;
322  while(true)
323  {
324  unsigned int x = 0, y = 0;
325  if(mCurrentMap.getCoordinates(x,y,index))
326  points.push_back(std::pair<double, double>(
327  ((x+0.5) * mCurrentMap.getResolution()) + mCurrentMap.getOriginX(),
328  ((y+0.5) * mCurrentMap.getResolution()) + mCurrentMap.getOriginY()
329  ));
330 
331  if(mCurrentPlan[index] == 0) break;
332 
333  unsigned int next_index = index;
334 
335  std::vector<unsigned int> neighbors = mCurrentMap.getFreeNeighbors(index);
336  for(unsigned int i = 0; i < neighbors.size(); i++)
337  {
338  if(mCurrentPlan[neighbors[i]] >= 0 && mCurrentPlan[neighbors[i]] < mCurrentPlan[next_index])
339  next_index = neighbors[i];
340  }
341 
342  if(index == next_index) break;
343  index = next_index;
344  }
345 
346  plan_msg.cells.resize(points.size());
347  for(unsigned int i = 0; i < points.size(); i++)
348  {
349  plan_msg.cells[i].x = points[i].first;
350  plan_msg.cells[i].y = points[i].second;
351  plan_msg.cells[i].z = 0.0;
352  }
353  mPlanPublisher.publish(plan_msg);
354 }
355 
356 bool RobotNavigator::correctGoalPose()
357 {
358  // Reset the plan
359  int mapSize = mCurrentMap.getSize();
360  for(int i = 0; i < mapSize; i++)
361  {
362  mCurrentPlan[i] = -1;
363  }
364 
365  // Initialize the queue with the goal point
366  Queue queue;
367  Entry goal(0.0, mGoalPoint);
368  queue.insert(goal);
369  mCurrentPlan[mGoalPoint] = 0;
370 
371  Queue::iterator next;
372  double linear = mCurrentMap.getResolution();
373 
374  // Do full search with Dijkstra-Algorithm
375  while(!queue.empty())
376  {
377  // Get the nearest cell from the queue
378  next = queue.begin();
379  double distance = next->first;
380  unsigned int index = next->second;
381  queue.erase(next);
382 
383  if(mCurrentPlan[index] >= 0 && mCurrentPlan[index] < distance) continue;
384 
385  // Add all adjacent cells
386  std::vector<unsigned int> neighbors = mCurrentMap.getNeighbors(index);
387  for(unsigned int i = 0; i < neighbors.size(); i++)
388  {
389  if(mCurrentMap.isFree(neighbors[i]))
390  {
391  mGoalPoint = neighbors[i];
392  return true;
393  }else
394  {
395  double newDistance = distance + linear;
396  if(mCurrentPlan[neighbors[i]] == -1)
397  {
398  queue.insert(Entry(newDistance, neighbors[i]));
399  mCurrentPlan[neighbors[i]] = newDistance;
400  }
401  }
402  }
403  }
404  return false;
405 }
406 
407 void RobotNavigator::stop()
408 {
409  nav2d_operator::cmd stopMsg;
410  stopMsg.Turn = 0;
411  stopMsg.Velocity = 0;
412  mCommandPublisher.publish(stopMsg);
413  mStatus = NAV_ST_IDLE;
414  mIsPaused = false;
415  mIsStopped = false;
416 }
417 
418 bool RobotNavigator::generateCommand()
419 {
420  // Do nothing when paused
421  if(mIsPaused)
422  {
423  ROS_INFO_THROTTLE(1.0, "Navigator is paused and will not move now.");
424  return true;
425  }
426 
427  if(mStatus != NAV_ST_NAVIGATING && mStatus != NAV_ST_EXPLORING)
428  {
429  ROS_WARN_THROTTLE(1.0, "Navigator has status %d when generateCommand() was called!", mStatus);
430  return false;
431  }
432 
433  // Generate direction command from plan
434  unsigned int current_x = 0, current_y = 0;
435  if(!mCurrentMap.getCoordinates(current_x, current_y, mStartPoint)) // || !mCurrentMap.isFree(mStartPoint))
436  {
437  ROS_ERROR("Plan execution failed, robot not in map!");
438  return false;
439  }
440 
441  unsigned int target = mStartPoint;
442  int steps = 1.0 / mCurrentMap.getResolution();
443  for(int i = 0; i < steps; i++)
444  {
445  unsigned int bestPoint = target;
446  std::vector<unsigned int> neighbors = mCurrentMap.getFreeNeighbors(target);
447  for(unsigned int i = 0; i < neighbors.size(); i++)
448  {
449  if(mCurrentPlan[neighbors[i]] >= (unsigned int)0 && mCurrentPlan[neighbors[i]] < mCurrentPlan[bestPoint])
450  bestPoint = neighbors[i];
451  }
452  target = bestPoint;
453  }
454 
455  // Head towards (x,y)
456  unsigned int x = 0, y = 0;
457  if(!mCurrentMap.getCoordinates(x, y, target))
458  {
459  ROS_ERROR("Plan execution failed, target pose not in map!");
460  return false;
461  }
462  double map_angle = atan2((double)y - current_y, (double)x - current_x);
463 
464  double angle = map_angle - mCurrentDirection;
465  if(angle < -PI) angle += 2*PI;
466  if(angle > PI) angle -= 2*PI;
467 
468  // Create the command message
469  nav2d_operator::cmd msg;
470  msg.Turn = -2.0 * angle / PI;
471  if(msg.Turn < -1) msg.Turn = -1;
472  if(msg.Turn > 1) msg.Turn = 1;
473 
474  if(mCurrentPlan[mStartPoint] > mNavigationHomingDistance || mStatus == NAV_ST_EXPLORING)
475  msg.Mode = 0;
476  else
477  msg.Mode = 1;
478 
479  if(mCurrentPlan[mStartPoint] > 1.0 || mCurrentPlan[mStartPoint] < 0)
480  {
481  msg.Velocity = 1.0;
482  }else
483  {
484  msg.Velocity = 0.5 + (mCurrentPlan[mStartPoint] / 2.0);
485  }
486  mCommandPublisher.publish(msg);
487  return true;
488 }
489 
490 void RobotNavigator::receiveGetMapGoal(const nav2d_navigator::GetFirstMapGoal::ConstPtr &goal)
491 {
492  if(mStatus != NAV_ST_IDLE)
493  {
494  ROS_WARN("Navigator is busy!");
495  mGetMapActionServer->setAborted();
496  return;
497  }
498 
499  // Move the robot slowly ahead
500  mStatus = NAV_ST_RECOVERING;
501  nav2d_operator::cmd msg;
502  msg.Turn = 0;
503  msg.Velocity = 1.0;
504  msg.Mode = 0;
505 
506  nav2d_navigator::GetFirstMapFeedback f;
507 
508  Rate loopRate(FREQUENCY);
509  unsigned int cycles = 0;
510  while(true)
511  {
512  if(!ok() || mGetMapActionServer->isPreemptRequested() || mIsStopped)
513  {
514  ROS_INFO("GetFirstMap has been preempted externally.");
515  mGetMapActionServer->setPreempted();
516  stop();
517  return;
518  }
519 
520  if(cycles >= 4*FREQUENCY) break;
521  cycles++;
522 
523  mGetMapActionServer->publishFeedback(f);
524  mCommandPublisher.publish(msg);
525  spinOnce();
526  loopRate.sleep();
527  }
528 
529  if(!getMap() || !setCurrentPosition())
530  {
531  mGetMapActionServer->setAborted();
532  stop();
533  return;
534  }
535 
536  // Do a full turn to have a initial map
537  msg.Turn = 1;
538  msg.Mode = 1;
539  double lastDirection = mCurrentDirection;
540  double turn = 0;
541  while(true)
542  {
543  if(!ok() || mGetMapActionServer->isPreemptRequested() || mIsStopped)
544  {
545  ROS_INFO("GetFirstMap has been preempted externally.");
546  mGetMapActionServer->setPreempted();
547  stop();
548  return;
549  }
550 
551  setCurrentPosition();
552  double deltaTheta = mCurrentDirection - lastDirection;
553  while(deltaTheta < -PI) deltaTheta += 2*PI;
554  while(deltaTheta > PI) deltaTheta -= 2*PI;
555  turn += deltaTheta;
556  lastDirection = mCurrentDirection;
557  if(turn > 2*PI || turn < -2*PI)
558  {
559  break;
560  }
561 
562  mGetMapActionServer->publishFeedback(f);
563  mCommandPublisher.publish(msg);
564  spinOnce();
565  loopRate.sleep();
566  }
567 
568  stop();
569  mHasNewMap = false;
570 
571  if(getMap() && setCurrentPosition())
572  {
573  mGetMapActionServer->setSucceeded();
574  }else
575  {
576  ROS_WARN("Navigator could not be initialized!");
577  mGetMapActionServer->setAborted();
578  }
579 }
580 
581 void RobotNavigator::receiveLocalizeGoal(const nav2d_navigator::LocalizeGoal::ConstPtr &goal)
582 {
583  if(mStatus != NAV_ST_IDLE)
584  {
585  ROS_WARN("[Localize] Action aborted, Navigator is busy!");
586  mGetMapActionServer->setAborted();
587  return;
588  }
589 
590  // Move the robot slowly ahead
591  mStatus = NAV_ST_RECOVERING;
592  nav2d_operator::cmd msg;
593  msg.Turn = 0;
594  msg.Velocity = goal->velocity;
595  msg.Mode = 0;
596 
597  nav2d_navigator::LocalizeFeedback f;
598 
599  mHasNewMap = false;
600  Rate loopRate(1);
601  while(true)
602  {
603  // Check if we are asked to preempt
604  if(!ok() || mLocalizeActionServer->isPreemptRequested() || mIsStopped)
605  {
606  ROS_INFO("[Localize] Action has been preempted externally.");
607  mLocalizeActionServer->setPreempted();
608  stop();
609  return;
610  }
611 
612  if(mHasNewMap)
613  {
614  mCommandPublisher.publish(msg);
615  }else
616  {
617  getMap();
618  }
619 
620  // Check if we are localized successfully
621  if(isLocalized())
622  {
623  ROS_INFO("[Localize] Action succeeded.");
624  mLocalizeActionServer->setSucceeded();
625  stop();
626  return;
627  }
628 
629  mLocalizeActionServer->publishFeedback(f);
630  spinOnce();
631  loopRate.sleep();
632  }
633 }
634 
635 void RobotNavigator::receiveMoveGoal(const nav2d_navigator::MoveToPosition2DGoal::ConstPtr &goal)
636 {
637  if(mStatus != NAV_ST_IDLE)
638  {
639  ROS_WARN("Navigator is busy!");
640  mMoveActionServer->setAborted();
641  return;
642  }
643 
644  ROS_DEBUG("Received Goal: %.2f, %.2f (in frame '%s')", goal->target_pose.x, goal->target_pose.y, goal->header.frame_id.c_str());
645 
646  // Start navigating according to the generated plan
647  Rate loopRate(FREQUENCY);
648  unsigned int cycle = 0;
649  bool reached = false;
650  int recheckCycles = mMinReplanningPeriod * FREQUENCY;
651 
652  double targetDistance = (goal->target_distance > 0) ? goal->target_distance : mNavigationGoalDistance;
653  double targetAngle = (goal->target_angle > 0) ? goal->target_angle : mNavigationGoalAngle;
654 
655  while(true)
656  {
657  // Check if we are asked to preempt
658  if(!ok() || mMoveActionServer->isPreemptRequested() || mIsStopped)
659  {
660  ROS_INFO("Navigation has been preempted externally.");
661  mMoveActionServer->setPreempted();
662  stop();
663  return;
664  }
665 
666  // Constantly replan every 3 seconds
667  if(cycle % recheckCycles == 0)
668  {
669  WallTime startTime = WallTime::now();
670  mStatus = NAV_ST_NAVIGATING;
671 
672  // Create the plan for navigation
673  mHasNewMap = false;
674  if(!preparePlan())
675  {
676  ROS_ERROR("Prepare failed!");
677  mMoveActionServer->setAborted();
678  stop();
679  return;
680  }
681 
682  int mapX = (double)(goal->target_pose.x - mCurrentMap.getOriginX()) / mCurrentMap.getResolution();
683  int mapY = (double)(goal->target_pose.y - mCurrentMap.getOriginY()) / mCurrentMap.getResolution();
684  if(mapX < 0) mapX = 0;
685  if(mapX >= (int)mCurrentMap.getWidth()) mapX = mCurrentMap.getWidth() - 1;
686  if(mapY < 0) mapY = 0;
687  if(mapY >= (int)mCurrentMap.getHeight()) mapY = mCurrentMap.getHeight() - 1;
688 
689  bool success = false;
690  if(mCurrentMap.getIndex(mapX, mapY, mGoalPoint))
691  success = createPlan();
692 
693  if(!success)
694  {
695  if(correctGoalPose())
696  success = createPlan();
697  }
698 
699  if(!success)
700  {
701  ROS_ERROR("Planning failed!");
702  mMoveActionServer->setAborted();
703  stop();
704  return;
705  }
706 
707  WallTime endTime = WallTime::now();
708  WallDuration d = endTime - startTime;
709  ROS_INFO("Path planning took %.09f seconds, distance is %.2f m.", d.toSec(), mCurrentPlan[mStartPoint]);
710  }
711 
712  // Where are we now
713  mHasNewMap = false;
714  if(!setCurrentPosition())
715  {
716  ROS_ERROR("Navigation failed, could not get current position.");
717  mMoveActionServer->setAborted();
718  stop();
719  return;
720  }
721 
722  // Are we already close enough?
723  if(!reached && mCurrentPlan[mStartPoint] <= targetDistance && mCurrentPlan[mStartPoint] >= 0)
724  {
725  ROS_INFO("Reached target, now turning to desired direction.");
726  reached = true;
727  }
728 
729  if(reached)
730  {
731  // Are we also headed correctly?
732  double deltaTheta = mCurrentDirection - goal->target_pose.theta;
733  while(deltaTheta < -PI) deltaTheta += 2*PI;
734  while(deltaTheta > PI) deltaTheta -= 2*PI;
735 
736  double diff = (deltaTheta > 0) ? deltaTheta : -deltaTheta;
737  ROS_INFO_THROTTLE(1.0,"Heading: %.2f / Desired: %.2f / Difference: %.2f / Tolerance: %.2f", mCurrentDirection, goal->target_pose.theta, diff, targetAngle);
738  if(diff <= targetAngle)
739  {
740  ROS_INFO("Final Heading: %.2f / Desired: %.2f / Difference: %.2f / Tolerance: %.2f", mCurrentDirection, goal->target_pose.theta, diff, targetAngle);
741  break;
742  }
743 
744  nav2d_operator::cmd msg;
745  if(deltaTheta > 0)
746  {
747  msg.Turn = 1;
748  msg.Velocity = deltaTheta;
749  }else
750  {
751  msg.Turn = -1;
752  msg.Velocity = -deltaTheta;
753  }
754  if(msg.Velocity > 1) msg.Velocity = 1;
755  msg.Mode = 1;
756 
757  mCommandPublisher.publish(msg);
758  }else
759  {
760  generateCommand();
761  }
762 
763  // Publish feedback via ActionServer
764  if(cycle%10 == 0)
765  {
766  nav2d_navigator::MoveToPosition2DFeedback fb;
767  fb.distance = mCurrentPlan[mStartPoint];
768  mMoveActionServer->publishFeedback(fb);
769  }
770 
771  // Sleep remaining time
772  cycle++;
773  spinOnce();
774  loopRate.sleep();
775  if(loopRate.cycleTime() > ros::Duration(1.0 / FREQUENCY))
776  ROS_WARN("Missed desired rate of %.2fHz! Loop actually took %.4f seconds!",FREQUENCY, loopRate.cycleTime().toSec());
777  }
778 
779  // Set ActionServer suceeded
780  ROS_INFO("Goal reached.");
781  nav2d_navigator::MoveToPosition2DResult r;
782  r.final_pose.x = mCurrentPositionX;
783  r.final_pose.y = mCurrentPositionY;
784  r.final_pose.theta = mCurrentDirection;
785  r.final_distance = mCurrentPlan[mStartPoint];
786  mMoveActionServer->setSucceeded(r);
787  stop();
788 
789 }
790 
791 void RobotNavigator::receiveExploreGoal(const nav2d_navigator::ExploreGoal::ConstPtr &goal)
792 {
793  if(mStatus != NAV_ST_IDLE)
794  {
795  ROS_WARN("Navigator is busy!");
796  mExploreActionServer->setAborted();
797  return;
798  }
799 
800  mStatus = NAV_ST_EXPLORING;
801  unsigned int cycle = 0;
802  unsigned int lastCheck = 0;
803  unsigned int recheckCycles = mMinReplanningPeriod * FREQUENCY;
804  unsigned int recheckThrottle = mMaxReplanningPeriod * FREQUENCY;
805 
806  // Move to exploration target
807  Rate loopRate(FREQUENCY);
808  while(true)
809  {
810  // Check if we are asked to preempt
811  if(!ok() || mExploreActionServer->isPreemptRequested() || mIsStopped)
812  {
813  ROS_INFO("Exploration has been preempted externally.");
814  mExploreActionServer->setPreempted();
815  stop();
816  return;
817  }
818 
819  // Where are we now
820  mHasNewMap = false;
821  if(!setCurrentPosition())
822  {
823  ROS_ERROR("Exploration failed, could not get current position.");
824  mExploreActionServer->setAborted();
825  stop();
826  return;
827  }
828 
829  // Regularly recheck for exploration target
830  cycle++;
831  bool reCheck = lastCheck == 0 || cycle - lastCheck > recheckCycles;
832  bool planOk = mCurrentPlan && mCurrentPlan[mStartPoint] >= 0;
833  bool nearGoal = planOk && ((cycle - lastCheck) > recheckThrottle && mCurrentPlan[mStartPoint] <= mExplorationGoalDistance);
834 
835  if(reCheck || nearGoal)
836  {
837  WallTime startTime = WallTime::now();
838  lastCheck = cycle;
839 
840  bool success = false;
841  if(preparePlan())
842  {
843  int result = mExplorationPlanner->findExplorationTarget(&mCurrentMap, mStartPoint, mGoalPoint);
844  switch(result)
845  {
846  case EXPL_TARGET_SET:
847  success = createPlan();
848  mStatus = NAV_ST_EXPLORING;
849  break;
850  case EXPL_FINISHED:
851  {
852  nav2d_navigator::ExploreResult r;
853  r.final_pose.x = mCurrentPositionX;
854  r.final_pose.y = mCurrentPositionY;
855  r.final_pose.theta = mCurrentDirection;
856  mExploreActionServer->setSucceeded(r);
857  }
858  stop();
859  ROS_INFO("Exploration has finished.");
860  return;
861  case EXPL_WAITING:
862  mStatus = NAV_ST_WAITING;
863  {
864  nav2d_operator::cmd stopMsg;
865  stopMsg.Turn = 0;
866  stopMsg.Velocity = 0;
867  mCommandPublisher.publish(stopMsg);
868  }
869  ROS_INFO("Exploration is waiting.");
870  break;
871  case EXPL_FAILED:
872  break;
873  default:
874  ROS_ERROR("Exploration planner returned invalid status code: %d!", result);
875  }
876  }
877 
878  if(mStatus == NAV_ST_EXPLORING)
879  {
880  if(success)
881  {
882  WallTime endTime = WallTime::now();
883  WallDuration d = endTime - startTime;
884  ROS_DEBUG("Exploration planning took %.09f seconds, distance is %.2f m.", d.toSec(), mCurrentPlan[mStartPoint]);
885  }else
886  {
887  mExploreActionServer->setAborted();
888  stop();
889  ROS_WARN("Exploration has failed!");
890  return;
891  }
892  }
893  }
894 
895  if(mStatus == NAV_ST_EXPLORING)
896  {
897  // Publish feedback via ActionServer
898  if(cycle%10 == 0)
899  {
900  nav2d_navigator::ExploreFeedback fb;
901  fb.distance = mCurrentPlan[mStartPoint];
902  fb.robot_pose.x = mCurrentPositionX;
903  fb.robot_pose.y = mCurrentPositionY;
904  fb.robot_pose.theta = mCurrentDirection;
905  mExploreActionServer->publishFeedback(fb);
906  }
907 
908  // Create a new command and send it to Operator
909  generateCommand();
910  }
911 
912  // Sleep remaining time
913  spinOnce();
914  loopRate.sleep();
915  if(loopRate.cycleTime() > ros::Duration(1.0 / FREQUENCY))
916  ROS_WARN("Missed desired rate of %.2fHz! Loop actually took %.4f seconds!",FREQUENCY, loopRate.cycleTime().toSec());
917  }
918 }
919 
920 bool RobotNavigator::isLocalized()
921 {
922  return mTfListener.waitForTransform(mMapFrame, mRobotFrame, Time::now(), Duration(0.1));
923 }
924 
925 bool RobotNavigator::setCurrentPosition()
926 {
927  StampedTransform transform;
928  try
929  {
930  mTfListener.lookupTransform(mMapFrame, mRobotFrame, Time(0), transform);
931  }catch(TransformException ex)
932  {
933  ROS_ERROR("Could not get robot position: %s", ex.what());
934  return false;
935  }
936  double world_x = transform.getOrigin().x();
937  double world_y = transform.getOrigin().y();
938  double world_theta = getYaw(transform.getRotation());
939 
940  unsigned int current_x = (world_x - mCurrentMap.getOriginX()) / mCurrentMap.getResolution();
941  unsigned int current_y = (world_y - mCurrentMap.getOriginY()) / mCurrentMap.getResolution();
942  unsigned int i;
943 
944  if(!mCurrentMap.getIndex(current_x, current_y, i))
945  {
946  if(mHasNewMap || !getMap() || !mCurrentMap.getIndex(current_x, current_y, i))
947  {
948  ROS_ERROR("Is the robot out of the map?");
949  return false;
950  }
951  }
952  mStartPoint = i;
953  mCurrentDirection = world_theta;
954  mCurrentPositionX = world_x;
955  mCurrentPositionY = world_y;
956  return true;
957 }
d
d
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
RobotNavigator::receiveStop
bool receiveStop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: RobotNavigator.cpp:142
NAV_PAUSE_SERVICE
#define NAV_PAUSE_SERVICE
Definition: commands.h:6
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(rate,...)
RobotNavigator::receiveExploreGoal
void receiveExploreGoal(const nav2d_navigator::ExploreGoal::ConstPtr &goal)
Definition: RobotNavigator.cpp:791
f
f
EXPL_WAITING
#define EXPL_WAITING
Definition: ExplorationPlanner.h:6
FREQUENCY
#define FREQUENCY
Definition: RobotNavigator.cpp:12
NAV_STOP_SERVICE
#define NAV_STOP_SERVICE
Definition: commands.h:5
Entry
std::pair< double, unsigned int > Entry
Definition: RobotNavigator.cpp:171
NAV_ST_WAITING
#define NAV_ST_WAITING
Definition: commands.h:20
ros::Rate::cycleTime
Duration cycleTime() const
NAV_ST_NAVIGATING
#define NAV_ST_NAVIGATING
Definition: commands.h:18
NAV_ST_IDLE
#define NAV_ST_IDLE
Definition: commands.h:17
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROS_WARN
#define ROS_WARN(...)
NAV_MOVE_ACTION
#define NAV_MOVE_ACTION
Definition: commands.h:12
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
NAV_LOCALIZE_ACTION
#define NAV_LOCALIZE_ACTION
Definition: commands.h:15
Queue
std::multimap< double, unsigned int > Queue
Definition: RobotNavigator.cpp:170
EXPL_FINISHED
#define EXPL_FINISHED
Definition: ExplorationPlanner.h:5
tf::Vector3::x
TFSIMD_FORCE_INLINE const tfScalar & x() const
PlanLoader
pluginlib::ClassLoader< ExplorationPlanner > PlanLoader
Definition: RobotNavigator.h:23
distance
double distance(double x0, double y0, double x1, double y1)
NAV_ST_EXPLORING
#define NAV_ST_EXPLORING
Definition: commands.h:19
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ExploreActionServer
actionlib::SimpleActionServer< nav2d_navigator::ExploreAction > ExploreActionServer
Definition: RobotNavigator.h:20
EXPL_TARGET_SET
#define EXPL_TARGET_SET
Definition: ExplorationPlanner.h:4
RobotNavigator::receiveGetMapGoal
void receiveGetMapGoal(const nav2d_navigator::GetFirstMapGoal::ConstPtr &goal)
Definition: RobotNavigator.cpp:490
NAV_GETMAP_ACTION
#define NAV_GETMAP_ACTION
Definition: commands.h:14
ros::ok
ROSCPP_DECL bool ok()
tf::Vector3::y
TFSIMD_FORCE_INLINE const tfScalar & y() const
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
getYaw
double getYaw(const tf2::Quaternion &q)
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
RobotNavigator::receiveLocalizeGoal
void receiveLocalizeGoal(const nav2d_navigator::LocalizeGoal::ConstPtr &goal)
Definition: RobotNavigator.cpp:581
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
ros::Rate::sleep
bool sleep()
RobotNavigator::receiveMoveGoal
void receiveMoveGoal(const nav2d_navigator::MoveToPosition2DGoal::ConstPtr &goal)
Definition: RobotNavigator.cpp:635
GetMapActionServer
actionlib::SimpleActionServer< nav2d_navigator::GetFirstMapAction > GetMapActionServer
Definition: RobotNavigator.h:21
ros::WallTime::now
static WallTime now()
tf::Transform::getRotation
Quaternion getRotation() const
EXPL_FAILED
#define EXPL_FAILED
Definition: ExplorationPlanner.h:7
diff
double diff(double v1, double v2)
ros::Time::now
static Time now()
ROS_INFO_THROTTLE
#define ROS_INFO_THROTTLE(rate,...)
NAV_ST_RECOVERING
#define NAV_ST_RECOVERING
Definition: commands.h:21
RobotNavigator::receivePause
bool receivePause(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: RobotNavigator.cpp:150
LocalizeActionServer
actionlib::SimpleActionServer< nav2d_navigator::LocalizeAction > LocalizeActionServer
Definition: RobotNavigator.h:22
ros::spinOnce
ROSCPP_DECL void spinOnce()
ROS_ERROR
#define ROS_ERROR(...)
MoveActionServer
actionlib::SimpleActionServer< nav2d_navigator::MoveToPosition2DAction > MoveActionServer
Definition: RobotNavigator.h:19
PI
#define PI
Definition: RobotNavigator.cpp:11
NAV_EXPLORE_ACTION
#define NAV_EXPLORE_ACTION
Definition: commands.h:13
ROS_DEBUG
#define ROS_DEBUG(...)

nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Tue Nov 7 2017 06:02:48