00001 #include <nav2d_operator/cmd.h>
00002 #include <nav_msgs/GridCells.h>
00003 #include <visualization_msgs/Marker.h>
00004
00005 #include <nav2d_navigator/RobotNavigator.h>
00006 #include <nav2d_navigator/ExplorationPlanner.h>
00007
00008 #include <set>
00009 #include <map>
00010
00011 #define PI 3.14159265
00012 #define FREQUENCY 5.0
00013
00014 using namespace ros;
00015 using namespace tf;
00016
00017 RobotNavigator::RobotNavigator()
00018 {
00019 NodeHandle robotNode;
00020
00021 std::string serviceName;
00022 robotNode.param("map_service", serviceName, std::string("get_map"));
00023 mGetMapClient = robotNode.serviceClient<nav_msgs::GetMap>(serviceName);
00024
00025 mCommandPublisher = robotNode.advertise<nav2d_operator::cmd>("cmd", 1);
00026 mStopServer = robotNode.advertiseService(NAV_STOP_SERVICE, &RobotNavigator::receiveStop, this);
00027 mPauseServer = robotNode.advertiseService(NAV_PAUSE_SERVICE, &RobotNavigator::receivePause, this);
00028 mCurrentPlan = NULL;
00029
00030 NodeHandle navigatorNode("~/");
00031 mPlanPublisher = navigatorNode.advertise<nav_msgs::GridCells>("plan", 1);
00032 mMarkerPublisher = navigatorNode.advertise<visualization_msgs::Marker>("markers", 1, true);
00033
00034
00035 navigatorNode.param("map_inflation_radius", mInflationRadius, 1.0);
00036 navigatorNode.param("robot_radius", mRobotRadius, 0.3);
00037 navigatorNode.param("exploration_strategy", mExplorationStrategy, std::string("NearestFrontierPlanner"));
00038 navigatorNode.param("navigation_goal_distance", mNavigationGoalDistance, 1.0);
00039 navigatorNode.param("navigation_goal_angle", mNavigationGoalAngle, 1.0);
00040 navigatorNode.param("exploration_goal_distance", mExplorationGoalDistance, 3.0);
00041 navigatorNode.param("navigation_homing_distance", mNavigationHomingDistance, 3.0);
00042 navigatorNode.param("min_replanning_period", mMinReplanningPeriod, 3.0);
00043 navigatorNode.param("max_replanning_period", mMaxReplanningPeriod, 1.0);
00044 mCostObstacle = 100;
00045 mCostLethal = (1.0 - (mRobotRadius / mInflationRadius)) * (double)mCostObstacle;
00046
00047 robotNode.param("map_frame", mMapFrame, std::string("map"));
00048 robotNode.param("robot_frame", mRobotFrame, std::string("robot"));
00049 robotNode.param("robot_id", mRobotID, 1);
00050 robotNode.param("move_action_topic", mMoveActionTopic, std::string(NAV_MOVE_ACTION));
00051 robotNode.param("explore_action_topic", mExploreActionTopic, std::string(NAV_EXPLORE_ACTION));
00052 robotNode.param("getmap_action_topic", mGetMapActionTopic, std::string(NAV_GETMAP_ACTION));
00053 robotNode.param("localize_action_topic", mLocalizeActionTopic, std::string(NAV_LOCALIZE_ACTION));
00054
00055
00056 mRobotFrame = mTfListener.resolve(mRobotFrame);
00057 mMapFrame = mTfListener.resolve(mMapFrame);
00058
00059 try
00060 {
00061 mPlanLoader = new PlanLoader("nav2d_navigator", "ExplorationPlanner");
00062 mExplorationPlanner = mPlanLoader->createInstance(mExplorationStrategy);
00063 ROS_INFO("Successfully loaded exploration strategy [%s].", mExplorationStrategy.c_str());
00064
00065 mExploreActionServer = new ExploreActionServer(mExploreActionTopic, boost::bind(&RobotNavigator::receiveExploreGoal, this, _1), false);
00066 mExploreActionServer->start();
00067 }
00068 catch(pluginlib::PluginlibException& ex)
00069 {
00070 ROS_ERROR("Failed to load exploration plugin! Error: %s", ex.what());
00071 mExploreActionServer = NULL;
00072 mPlanLoader = NULL;
00073 }
00074
00075
00076 mMoveActionServer = new MoveActionServer(mMoveActionTopic, boost::bind(&RobotNavigator::receiveMoveGoal, this, _1), false);
00077 mMoveActionServer->start();
00078
00079 mLocalizeActionServer = new LocalizeActionServer(mLocalizeActionTopic, boost::bind(&RobotNavigator::receiveLocalizeGoal, this, _1), false);
00080 mLocalizeActionServer->start();
00081
00082 if(mRobotID == 1)
00083 {
00084 mGetMapActionServer = new GetMapActionServer(mGetMapActionTopic, boost::bind(&RobotNavigator::receiveGetMapGoal, this, _1), false);
00085 mGetMapActionServer->start();
00086 }else
00087 {
00088 mGetMapActionServer = NULL;
00089 }
00090
00091 mHasNewMap = false;
00092 mIsStopped = false;
00093 mIsPaused = false;
00094 mStatus = NAV_ST_IDLE;
00095 mCellInflationRadius = 0;
00096 }
00097
00098 RobotNavigator::~RobotNavigator()
00099 {
00100 delete[] mCurrentPlan;
00101 delete mMoveActionServer;
00102 delete mExploreActionServer;
00103 delete mGetMapActionServer;
00104 mExplorationPlanner.reset();
00105 delete mPlanLoader;
00106 }
00107
00108 bool RobotNavigator::getMap()
00109 {
00110 if(mHasNewMap) return true;
00111
00112 if(!mGetMapClient.isValid())
00113 {
00114 ROS_ERROR("GetMap-Client is invalid!");
00115 return false;
00116 }
00117
00118 nav_msgs::GetMap srv;
00119 if(!mGetMapClient.call(srv))
00120 {
00121 ROS_INFO("Could not get a map.");
00122 return false;
00123 }
00124 mCurrentMap.update(srv.response.map);
00125
00126 if(mCurrentPlan) delete[] mCurrentPlan;
00127 mCurrentPlan = new double[mCurrentMap.getSize()];
00128
00129 if(mCellInflationRadius == 0)
00130 {
00131 ROS_INFO("Navigator is now initialized.");
00132 mCellInflationRadius = mInflationRadius / mCurrentMap.getResolution();
00133 mCellRobotRadius = mRobotRadius / mCurrentMap.getResolution();
00134 mInflationTool.computeCaches(mCellInflationRadius);
00135 mCurrentMap.setLethalCost(mCostLethal);
00136 }
00137
00138 mHasNewMap = true;
00139 return true;
00140 }
00141
00142 bool RobotNavigator::receiveStop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00143 {
00144 mIsStopped = true;
00145 res.success = true;
00146 res.message = "Navigator received stop signal.";
00147 return true;
00148 }
00149
00150 bool RobotNavigator::receivePause(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00151 {
00152 if(mIsPaused)
00153 {
00154 mIsPaused = false;
00155 res.success = false;
00156 res.message = "Navigator continues.";
00157 }else
00158 {
00159 mIsPaused = true;
00160 nav2d_operator::cmd stopMsg;
00161 stopMsg.Turn = 0;
00162 stopMsg.Velocity = 0;
00163 mCommandPublisher.publish(stopMsg);
00164 res.success = true;
00165 res.message = "Navigator pauses.";
00166 }
00167 return true;
00168 }
00169
00170 typedef std::multimap<double,unsigned int> Queue;
00171 typedef std::pair<double,unsigned int> Entry;
00172
00173 bool RobotNavigator::preparePlan()
00174 {
00175
00176 if(!getMap())
00177 {
00178 if(mCellInflationRadius == 0) return false;
00179 ROS_WARN("Could not get a new map, trying to go with the old one...");
00180 }
00181
00182
00183 if(!setCurrentPosition()) return false;
00184
00185
00186 unsigned int x = 0, y = 0;
00187 if(mCurrentMap.getCoordinates(x, y, mStartPoint))
00188 for(int i = -mCellRobotRadius; i < (int)mCellRobotRadius; i++)
00189 for(int j = -mCellRobotRadius; j < (int)mCellRobotRadius; j++)
00190 mCurrentMap.setData(x+i, y+j, 0);
00191
00192 mInflationTool.inflateMap(&mCurrentMap);
00193 return true;
00194 }
00195
00196 bool RobotNavigator::createPlan()
00197 {
00198 ROS_DEBUG("Map-Value of goal point is %d, lethal threshold is %d.", mCurrentMap.getData(mGoalPoint), mCostLethal);
00199
00200 unsigned int goal_x = 0, goal_y = 0;
00201 if(mCurrentMap.getCoordinates(goal_x,goal_y,mGoalPoint))
00202 {
00203 visualization_msgs::Marker marker;
00204 marker.header.frame_id = "/map";
00205 marker.header.stamp = ros::Time();
00206 marker.id = 0;
00207 marker.type = visualization_msgs::Marker::CYLINDER;
00208 marker.action = visualization_msgs::Marker::ADD;
00209 marker.pose.position.x = mCurrentMap.getOriginX() + (((double)goal_x+0.5) * mCurrentMap.getResolution());
00210 marker.pose.position.y = mCurrentMap.getOriginY() + (((double)goal_y+0.5) * mCurrentMap.getResolution());
00211 marker.pose.position.z = 0.5;
00212 marker.pose.orientation.x = 0.0;
00213 marker.pose.orientation.y = 0.0;
00214 marker.pose.orientation.z = 0.0;
00215 marker.pose.orientation.w = 1.0;
00216 marker.scale.x = mCurrentMap.getResolution() * 3.0;
00217 marker.scale.y = mCurrentMap.getResolution() * 3.0;
00218 marker.scale.z = 1.0;
00219 marker.color.a = 1.0;
00220 marker.color.r = 1.0;
00221 marker.color.g = 0.0;
00222 marker.color.b = 0.0;
00223 mMarkerPublisher.publish(marker);
00224 }else
00225 {
00226 ROS_ERROR("Couldn't ressolve goal point coordinates!");
00227 }
00228
00229 Queue queue;
00230
00231
00232 int mapSize = mCurrentMap.getSize();
00233 for(int i = 0; i < mapSize; i++)
00234 {
00235 mCurrentPlan[i] = -1;
00236 }
00237
00238 if(mCurrentMap.isFree(mGoalPoint))
00239 {
00240 queue.insert(Entry(0.0, mGoalPoint));
00241 mCurrentPlan[mGoalPoint] = 0;
00242 }else
00243 {
00244
00245 int reach = mCellRobotRadius + (1.0 / mCurrentMap.getResolution());
00246 std::vector<unsigned int> neighbors = mCurrentMap.getFreeNeighbors(mGoalPoint, reach);
00247 for(unsigned int i = 0; i < neighbors.size(); i++)
00248 {
00249 queue.insert(Entry(0.0, neighbors[i]));
00250 mCurrentPlan[neighbors[i]] = 0;
00251 }
00252 }
00253
00254 Queue::iterator next;
00255 double distance;
00256 unsigned int x, y, index;
00257 double linear = mCurrentMap.getResolution();
00258 double diagonal = std::sqrt(2.0) * linear;
00259
00260
00261 while(!queue.empty())
00262 {
00263
00264 next = queue.begin();
00265 distance = next->first;
00266 index = next->second;
00267 queue.erase(next);
00268
00269 if(mCurrentPlan[index] >= 0 && mCurrentPlan[index] < distance) continue;
00270
00271
00272
00273
00274 if(!mCurrentMap.getCoordinates(x, y, index)) continue;
00275 std::vector<unsigned int> ind;
00276 ind.push_back(index - 1);
00277 ind.push_back(index + 1);
00278 ind.push_back(index - mCurrentMap.getWidth());
00279 ind.push_back(index + mCurrentMap.getWidth());
00280 ind.push_back(index - mCurrentMap.getWidth() - 1);
00281 ind.push_back(index - mCurrentMap.getWidth() + 1);
00282 ind.push_back(index + mCurrentMap.getWidth() - 1);
00283 ind.push_back(index + mCurrentMap.getWidth() + 1);
00284
00285 for(unsigned int it = 0; it < ind.size(); it++)
00286 {
00287 unsigned int i = ind[it];
00288 if(mCurrentMap.isFree(i))
00289 {
00290 double delta = (it < 4) ? linear : diagonal;
00291 double newDistance = distance + delta + (10 * delta * (double)mCurrentMap.getData(i) / (double)mCostObstacle);
00292 if(mCurrentPlan[i] == -1 || newDistance < mCurrentPlan[i])
00293 {
00294 queue.insert(Entry(newDistance, i));
00295 mCurrentPlan[i] = newDistance;
00296 }
00297 }
00298 }
00299 }
00300
00301 if(mCurrentPlan[mStartPoint] < 0)
00302 {
00303 ROS_ERROR("No way between robot and goal!");
00304 return false;
00305 }
00306
00307 publishPlan();
00308 return true;
00309 }
00310
00311 void RobotNavigator::publishPlan()
00312 {
00313 nav_msgs::GridCells plan_msg;
00314 plan_msg.header.frame_id = mMapFrame.c_str();
00315 plan_msg.header.stamp = Time::now();
00316
00317 plan_msg.cell_width = mCurrentMap.getResolution();
00318 plan_msg.cell_height = mCurrentMap.getResolution();
00319
00320 unsigned int index = mStartPoint;
00321 std::vector<std::pair<double, double> > points;
00322 while(true)
00323 {
00324 unsigned int x = 0, y = 0;
00325 if(mCurrentMap.getCoordinates(x,y,index))
00326 points.push_back(std::pair<double, double>(
00327 ((x+0.5) * mCurrentMap.getResolution()) + mCurrentMap.getOriginX(),
00328 ((y+0.5) * mCurrentMap.getResolution()) + mCurrentMap.getOriginY()
00329 ));
00330
00331 if(mCurrentPlan[index] == 0) break;
00332
00333 unsigned int next_index = index;
00334
00335 std::vector<unsigned int> neighbors = mCurrentMap.getFreeNeighbors(index);
00336 for(unsigned int i = 0; i < neighbors.size(); i++)
00337 {
00338 if(mCurrentPlan[neighbors[i]] >= 0 && mCurrentPlan[neighbors[i]] < mCurrentPlan[next_index])
00339 next_index = neighbors[i];
00340 }
00341
00342 if(index == next_index) break;
00343 index = next_index;
00344 }
00345
00346 plan_msg.cells.resize(points.size());
00347 for(unsigned int i = 0; i < points.size(); i++)
00348 {
00349 plan_msg.cells[i].x = points[i].first;
00350 plan_msg.cells[i].y = points[i].second;
00351 plan_msg.cells[i].z = 0.0;
00352 }
00353 mPlanPublisher.publish(plan_msg);
00354 }
00355
00356 bool RobotNavigator::correctGoalPose()
00357 {
00358
00359 int mapSize = mCurrentMap.getSize();
00360 for(int i = 0; i < mapSize; i++)
00361 {
00362 mCurrentPlan[i] = -1;
00363 }
00364
00365
00366 Queue queue;
00367 Entry goal(0.0, mGoalPoint);
00368 queue.insert(goal);
00369 mCurrentPlan[mGoalPoint] = 0;
00370
00371 Queue::iterator next;
00372 double linear = mCurrentMap.getResolution();
00373
00374
00375 while(!queue.empty())
00376 {
00377
00378 next = queue.begin();
00379 double distance = next->first;
00380 unsigned int index = next->second;
00381 queue.erase(next);
00382
00383 if(mCurrentPlan[index] >= 0 && mCurrentPlan[index] < distance) continue;
00384
00385
00386 std::vector<unsigned int> neighbors = mCurrentMap.getNeighbors(index);
00387 for(unsigned int i = 0; i < neighbors.size(); i++)
00388 {
00389 if(mCurrentMap.isFree(neighbors[i]))
00390 {
00391 mGoalPoint = neighbors[i];
00392 return true;
00393 }else
00394 {
00395 double newDistance = distance + linear;
00396 if(mCurrentPlan[neighbors[i]] == -1)
00397 {
00398 queue.insert(Entry(newDistance, neighbors[i]));
00399 mCurrentPlan[neighbors[i]] = newDistance;
00400 }
00401 }
00402 }
00403 }
00404 return false;
00405 }
00406
00407 void RobotNavigator::stop()
00408 {
00409 nav2d_operator::cmd stopMsg;
00410 stopMsg.Turn = 0;
00411 stopMsg.Velocity = 0;
00412 mCommandPublisher.publish(stopMsg);
00413 mStatus = NAV_ST_IDLE;
00414 mIsPaused = false;
00415 mIsStopped = false;
00416 }
00417
00418 bool RobotNavigator::generateCommand()
00419 {
00420
00421 if(mIsPaused)
00422 {
00423 ROS_INFO_THROTTLE(1.0, "Navigator is paused and will not move now.");
00424 return true;
00425 }
00426
00427 if(mStatus != NAV_ST_NAVIGATING && mStatus != NAV_ST_EXPLORING)
00428 {
00429 ROS_WARN_THROTTLE(1.0, "Navigator has status %d when generateCommand() was called!", mStatus);
00430 return false;
00431 }
00432
00433
00434 unsigned int current_x = 0, current_y = 0;
00435 if(!mCurrentMap.getCoordinates(current_x, current_y, mStartPoint))
00436 {
00437 ROS_ERROR("Plan execution failed, robot not in map!");
00438 return false;
00439 }
00440
00441 unsigned int target = mStartPoint;
00442 int steps = 1.0 / mCurrentMap.getResolution();
00443 for(int i = 0; i < steps; i++)
00444 {
00445 unsigned int bestPoint = target;
00446 std::vector<unsigned int> neighbors = mCurrentMap.getFreeNeighbors(target);
00447 for(unsigned int i = 0; i < neighbors.size(); i++)
00448 {
00449 if(mCurrentPlan[neighbors[i]] >= (unsigned int)0 && mCurrentPlan[neighbors[i]] < mCurrentPlan[bestPoint])
00450 bestPoint = neighbors[i];
00451 }
00452 target = bestPoint;
00453 }
00454
00455
00456 unsigned int x = 0, y = 0;
00457 if(!mCurrentMap.getCoordinates(x, y, target))
00458 {
00459 ROS_ERROR("Plan execution failed, target pose not in map!");
00460 return false;
00461 }
00462 double map_angle = atan2((double)y - current_y, (double)x - current_x);
00463
00464 double angle = map_angle - mCurrentDirection;
00465 if(angle < -PI) angle += 2*PI;
00466 if(angle > PI) angle -= 2*PI;
00467
00468
00469 nav2d_operator::cmd msg;
00470 msg.Turn = -2.0 * angle / PI;
00471 if(msg.Turn < -1) msg.Turn = -1;
00472 if(msg.Turn > 1) msg.Turn = 1;
00473
00474 if(mCurrentPlan[mStartPoint] > mNavigationHomingDistance || mStatus == NAV_ST_EXPLORING)
00475 msg.Mode = 0;
00476 else
00477 msg.Mode = 1;
00478
00479 if(mCurrentPlan[mStartPoint] > 1.0 || mCurrentPlan[mStartPoint] < 0)
00480 {
00481 msg.Velocity = 1.0;
00482 }else
00483 {
00484 msg.Velocity = 0.5 + (mCurrentPlan[mStartPoint] / 2.0);
00485 }
00486 mCommandPublisher.publish(msg);
00487 return true;
00488 }
00489
00490 void RobotNavigator::receiveGetMapGoal(const nav2d_navigator::GetFirstMapGoal::ConstPtr &goal)
00491 {
00492 if(mStatus != NAV_ST_IDLE)
00493 {
00494 ROS_WARN("Navigator is busy!");
00495 mGetMapActionServer->setAborted();
00496 return;
00497 }
00498
00499
00500 mStatus = NAV_ST_RECOVERING;
00501 nav2d_operator::cmd msg;
00502 msg.Turn = 0;
00503 msg.Velocity = 1.0;
00504 msg.Mode = 0;
00505
00506 nav2d_navigator::GetFirstMapFeedback f;
00507
00508 Rate loopRate(FREQUENCY);
00509 unsigned int cycles = 0;
00510 while(true)
00511 {
00512 if(!ok() || mGetMapActionServer->isPreemptRequested() || mIsStopped)
00513 {
00514 ROS_INFO("GetFirstMap has been preempted externally.");
00515 mGetMapActionServer->setPreempted();
00516 stop();
00517 return;
00518 }
00519
00520 if(cycles >= 4*FREQUENCY) break;
00521 cycles++;
00522
00523 mGetMapActionServer->publishFeedback(f);
00524 mCommandPublisher.publish(msg);
00525 spinOnce();
00526 loopRate.sleep();
00527 }
00528
00529 if(!getMap() || !setCurrentPosition())
00530 {
00531 mGetMapActionServer->setAborted();
00532 stop();
00533 return;
00534 }
00535
00536
00537 msg.Turn = 1;
00538 msg.Mode = 1;
00539 double lastDirection = mCurrentDirection;
00540 double turn = 0;
00541 while(true)
00542 {
00543 if(!ok() || mGetMapActionServer->isPreemptRequested() || mIsStopped)
00544 {
00545 ROS_INFO("GetFirstMap has been preempted externally.");
00546 mGetMapActionServer->setPreempted();
00547 stop();
00548 return;
00549 }
00550
00551 setCurrentPosition();
00552 double deltaTheta = mCurrentDirection - lastDirection;
00553 while(deltaTheta < -PI) deltaTheta += 2*PI;
00554 while(deltaTheta > PI) deltaTheta -= 2*PI;
00555 turn += deltaTheta;
00556 lastDirection = mCurrentDirection;
00557 if(turn > 2*PI || turn < -2*PI)
00558 {
00559 break;
00560 }
00561
00562 mGetMapActionServer->publishFeedback(f);
00563 mCommandPublisher.publish(msg);
00564 spinOnce();
00565 loopRate.sleep();
00566 }
00567
00568 stop();
00569 mHasNewMap = false;
00570
00571 if(getMap() && setCurrentPosition())
00572 {
00573 mGetMapActionServer->setSucceeded();
00574 }else
00575 {
00576 ROS_WARN("Navigator could not be initialized!");
00577 mGetMapActionServer->setAborted();
00578 }
00579 }
00580
00581 void RobotNavigator::receiveLocalizeGoal(const nav2d_navigator::LocalizeGoal::ConstPtr &goal)
00582 {
00583 if(mStatus != NAV_ST_IDLE)
00584 {
00585 ROS_WARN("[Localize] Action aborted, Navigator is busy!");
00586 mGetMapActionServer->setAborted();
00587 return;
00588 }
00589
00590
00591 mStatus = NAV_ST_RECOVERING;
00592 nav2d_operator::cmd msg;
00593 msg.Turn = 0;
00594 msg.Velocity = goal->velocity;
00595 msg.Mode = 0;
00596
00597 nav2d_navigator::LocalizeFeedback f;
00598
00599 mHasNewMap = false;
00600 Rate loopRate(1);
00601 while(true)
00602 {
00603
00604 if(!ok() || mLocalizeActionServer->isPreemptRequested() || mIsStopped)
00605 {
00606 ROS_INFO("[Localize] Action has been preempted externally.");
00607 mLocalizeActionServer->setPreempted();
00608 stop();
00609 return;
00610 }
00611
00612 if(mHasNewMap)
00613 {
00614 mCommandPublisher.publish(msg);
00615 }else
00616 {
00617 getMap();
00618 }
00619
00620
00621 if(isLocalized())
00622 {
00623 ROS_INFO("[Localize] Action succeeded.");
00624 mLocalizeActionServer->setSucceeded();
00625 stop();
00626 return;
00627 }
00628
00629 mLocalizeActionServer->publishFeedback(f);
00630 spinOnce();
00631 loopRate.sleep();
00632 }
00633 }
00634
00635 void RobotNavigator::receiveMoveGoal(const nav2d_navigator::MoveToPosition2DGoal::ConstPtr &goal)
00636 {
00637 if(mStatus != NAV_ST_IDLE)
00638 {
00639 ROS_WARN("Navigator is busy!");
00640 mMoveActionServer->setAborted();
00641 return;
00642 }
00643
00644 ROS_DEBUG("Received Goal: %.2f, %.2f (in frame '%s')", goal->target_pose.x, goal->target_pose.y, goal->header.frame_id.c_str());
00645
00646
00647 Rate loopRate(FREQUENCY);
00648 unsigned int cycle = 0;
00649 bool reached = false;
00650 int recheckCycles = mMinReplanningPeriod * FREQUENCY;
00651
00652 double targetDistance = (goal->target_distance > 0) ? goal->target_distance : mNavigationGoalDistance;
00653 double targetAngle = (goal->target_angle > 0) ? goal->target_angle : mNavigationGoalAngle;
00654
00655 while(true)
00656 {
00657
00658 if(!ok() || mMoveActionServer->isPreemptRequested() || mIsStopped)
00659 {
00660 ROS_INFO("Navigation has been preempted externally.");
00661 mMoveActionServer->setPreempted();
00662 stop();
00663 return;
00664 }
00665
00666
00667 if(cycle % recheckCycles == 0)
00668 {
00669 WallTime startTime = WallTime::now();
00670 mStatus = NAV_ST_NAVIGATING;
00671
00672
00673 mHasNewMap = false;
00674 if(!preparePlan())
00675 {
00676 ROS_ERROR("Prepare failed!");
00677 mMoveActionServer->setAborted();
00678 stop();
00679 return;
00680 }
00681
00682 int mapX = (double)(goal->target_pose.x - mCurrentMap.getOriginX()) / mCurrentMap.getResolution();
00683 int mapY = (double)(goal->target_pose.y - mCurrentMap.getOriginY()) / mCurrentMap.getResolution();
00684 if(mapX < 0) mapX = 0;
00685 if(mapX >= (int)mCurrentMap.getWidth()) mapX = mCurrentMap.getWidth() - 1;
00686 if(mapY < 0) mapY = 0;
00687 if(mapY >= (int)mCurrentMap.getHeight()) mapY = mCurrentMap.getHeight() - 1;
00688
00689 bool success = false;
00690 if(mCurrentMap.getIndex(mapX, mapY, mGoalPoint))
00691 success = createPlan();
00692
00693 if(!success)
00694 {
00695 if(correctGoalPose())
00696 success = createPlan();
00697 }
00698
00699 if(!success)
00700 {
00701 ROS_ERROR("Planning failed!");
00702 mMoveActionServer->setAborted();
00703 stop();
00704 return;
00705 }
00706
00707 WallTime endTime = WallTime::now();
00708 WallDuration d = endTime - startTime;
00709 ROS_INFO("Path planning took %.09f seconds, distance is %.2f m.", d.toSec(), mCurrentPlan[mStartPoint]);
00710 }
00711
00712
00713 mHasNewMap = false;
00714 if(!setCurrentPosition())
00715 {
00716 ROS_ERROR("Navigation failed, could not get current position.");
00717 mMoveActionServer->setAborted();
00718 stop();
00719 return;
00720 }
00721
00722
00723 if(!reached && mCurrentPlan[mStartPoint] <= targetDistance && mCurrentPlan[mStartPoint] >= 0)
00724 {
00725 ROS_INFO("Reached target, now turning to desired direction.");
00726 reached = true;
00727 }
00728
00729 if(reached)
00730 {
00731
00732 double deltaTheta = mCurrentDirection - goal->target_pose.theta;
00733 while(deltaTheta < -PI) deltaTheta += 2*PI;
00734 while(deltaTheta > PI) deltaTheta -= 2*PI;
00735
00736 double diff = (deltaTheta > 0) ? deltaTheta : -deltaTheta;
00737 ROS_INFO_THROTTLE(1.0,"Heading: %.2f / Desired: %.2f / Difference: %.2f / Tolerance: %.2f", mCurrentDirection, goal->target_pose.theta, diff, targetAngle);
00738 if(diff <= targetAngle)
00739 {
00740 ROS_INFO("Final Heading: %.2f / Desired: %.2f / Difference: %.2f / Tolerance: %.2f", mCurrentDirection, goal->target_pose.theta, diff, targetAngle);
00741 break;
00742 }
00743
00744 nav2d_operator::cmd msg;
00745 if(deltaTheta > 0)
00746 {
00747 msg.Turn = 1;
00748 msg.Velocity = deltaTheta;
00749 }else
00750 {
00751 msg.Turn = -1;
00752 msg.Velocity = -deltaTheta;
00753 }
00754 if(msg.Velocity > 1) msg.Velocity = 1;
00755 msg.Mode = 1;
00756
00757 mCommandPublisher.publish(msg);
00758 }else
00759 {
00760 generateCommand();
00761 }
00762
00763
00764 if(cycle%10 == 0)
00765 {
00766 nav2d_navigator::MoveToPosition2DFeedback fb;
00767 fb.distance = mCurrentPlan[mStartPoint];
00768 mMoveActionServer->publishFeedback(fb);
00769 }
00770
00771
00772 cycle++;
00773 spinOnce();
00774 loopRate.sleep();
00775 if(loopRate.cycleTime() > ros::Duration(1.0 / FREQUENCY))
00776 ROS_WARN("Missed desired rate of %.2fHz! Loop actually took %.4f seconds!",FREQUENCY, loopRate.cycleTime().toSec());
00777 }
00778
00779
00780 ROS_INFO("Goal reached.");
00781 nav2d_navigator::MoveToPosition2DResult r;
00782 r.final_pose.x = mCurrentPositionX;
00783 r.final_pose.y = mCurrentPositionY;
00784 r.final_pose.theta = mCurrentDirection;
00785 r.final_distance = mCurrentPlan[mStartPoint];
00786 mMoveActionServer->setSucceeded(r);
00787 stop();
00788
00789 }
00790
00791 void RobotNavigator::receiveExploreGoal(const nav2d_navigator::ExploreGoal::ConstPtr &goal)
00792 {
00793 if(mStatus != NAV_ST_IDLE)
00794 {
00795 ROS_WARN("Navigator is busy!");
00796 mExploreActionServer->setAborted();
00797 return;
00798 }
00799
00800 mStatus = NAV_ST_EXPLORING;
00801 unsigned int cycle = 0;
00802 unsigned int lastCheck = 0;
00803 unsigned int recheckCycles = mMinReplanningPeriod * FREQUENCY;
00804 unsigned int recheckThrottle = mMaxReplanningPeriod * FREQUENCY;
00805
00806
00807 Rate loopRate(FREQUENCY);
00808 while(true)
00809 {
00810
00811 if(!ok() || mExploreActionServer->isPreemptRequested() || mIsStopped)
00812 {
00813 ROS_INFO("Exploration has been preempted externally.");
00814 mExploreActionServer->setPreempted();
00815 stop();
00816 return;
00817 }
00818
00819
00820 mHasNewMap = false;
00821 if(!setCurrentPosition())
00822 {
00823 ROS_ERROR("Exploration failed, could not get current position.");
00824 mExploreActionServer->setAborted();
00825 stop();
00826 return;
00827 }
00828
00829
00830 cycle++;
00831 bool reCheck = lastCheck == 0 || cycle - lastCheck > recheckCycles;
00832 bool planOk = mCurrentPlan && mCurrentPlan[mStartPoint] >= 0;
00833 bool nearGoal = planOk && ((cycle - lastCheck) > recheckThrottle && mCurrentPlan[mStartPoint] <= mExplorationGoalDistance);
00834
00835 if(reCheck || nearGoal)
00836 {
00837 WallTime startTime = WallTime::now();
00838 lastCheck = cycle;
00839
00840 bool success = false;
00841 if(preparePlan())
00842 {
00843 int result = mExplorationPlanner->findExplorationTarget(&mCurrentMap, mStartPoint, mGoalPoint);
00844 switch(result)
00845 {
00846 case EXPL_TARGET_SET:
00847 success = createPlan();
00848 mStatus = NAV_ST_EXPLORING;
00849 break;
00850 case EXPL_FINISHED:
00851 {
00852 nav2d_navigator::ExploreResult r;
00853 r.final_pose.x = mCurrentPositionX;
00854 r.final_pose.y = mCurrentPositionY;
00855 r.final_pose.theta = mCurrentDirection;
00856 mExploreActionServer->setSucceeded(r);
00857 }
00858 stop();
00859 ROS_INFO("Exploration has finished.");
00860 return;
00861 case EXPL_WAITING:
00862 mStatus = NAV_ST_WAITING;
00863 {
00864 nav2d_operator::cmd stopMsg;
00865 stopMsg.Turn = 0;
00866 stopMsg.Velocity = 0;
00867 mCommandPublisher.publish(stopMsg);
00868 }
00869 ROS_INFO("Exploration is waiting.");
00870 break;
00871 case EXPL_FAILED:
00872 break;
00873 default:
00874 ROS_ERROR("Exploration planner returned invalid status code: %d!", result);
00875 }
00876 }
00877
00878 if(mStatus == NAV_ST_EXPLORING)
00879 {
00880 if(success)
00881 {
00882 WallTime endTime = WallTime::now();
00883 WallDuration d = endTime - startTime;
00884 ROS_DEBUG("Exploration planning took %.09f seconds, distance is %.2f m.", d.toSec(), mCurrentPlan[mStartPoint]);
00885 }else
00886 {
00887 mExploreActionServer->setAborted();
00888 stop();
00889 ROS_WARN("Exploration has failed!");
00890 return;
00891 }
00892 }
00893 }
00894
00895 if(mStatus == NAV_ST_EXPLORING)
00896 {
00897
00898 if(cycle%10 == 0)
00899 {
00900 nav2d_navigator::ExploreFeedback fb;
00901 fb.distance = mCurrentPlan[mStartPoint];
00902 fb.robot_pose.x = mCurrentPositionX;
00903 fb.robot_pose.y = mCurrentPositionY;
00904 fb.robot_pose.theta = mCurrentDirection;
00905 mExploreActionServer->publishFeedback(fb);
00906 }
00907
00908
00909 generateCommand();
00910 }
00911
00912
00913 spinOnce();
00914 loopRate.sleep();
00915 if(loopRate.cycleTime() > ros::Duration(1.0 / FREQUENCY))
00916 ROS_WARN("Missed desired rate of %.2fHz! Loop actually took %.4f seconds!",FREQUENCY, loopRate.cycleTime().toSec());
00917 }
00918 }
00919
00920 bool RobotNavigator::isLocalized()
00921 {
00922 return mTfListener.waitForTransform(mMapFrame, mRobotFrame, Time::now(), Duration(0.1));
00923 }
00924
00925 bool RobotNavigator::setCurrentPosition()
00926 {
00927 StampedTransform transform;
00928 try
00929 {
00930 mTfListener.lookupTransform(mMapFrame, mRobotFrame, Time(0), transform);
00931 }catch(TransformException ex)
00932 {
00933 ROS_ERROR("Could not get robot position: %s", ex.what());
00934 return false;
00935 }
00936 double world_x = transform.getOrigin().x();
00937 double world_y = transform.getOrigin().y();
00938 double world_theta = getYaw(transform.getRotation());
00939
00940 unsigned int current_x = (world_x - mCurrentMap.getOriginX()) / mCurrentMap.getResolution();
00941 unsigned int current_y = (world_y - mCurrentMap.getOriginY()) / mCurrentMap.getResolution();
00942 unsigned int i;
00943
00944 if(!mCurrentMap.getIndex(current_x, current_y, i))
00945 {
00946 if(mHasNewMap || !getMap() || !mCurrentMap.getIndex(current_x, current_y, i))
00947 {
00948 ROS_ERROR("Is the robot out of the map?");
00949 return false;
00950 }
00951 }
00952 mStartPoint = i;
00953 mCurrentDirection = world_theta;
00954 mCurrentPositionX = world_x;
00955 mCurrentPositionY = world_y;
00956 return true;
00957 }