RobotNavigator.cpp
Go to the documentation of this file.
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         // Get parameters
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         // Apply tf_prefix to all used frame-id's
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         // Create action servers
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         // Get the current map
00176         if(!getMap()) // return false;
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         // Where am I?
00183         if(!setCurrentPosition()) return false;
00184         
00185         // Clear robot footprint in map
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         // Reset the plan
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                 // Initialize the queue with area around the goal point
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         // Do full search with Dijkstra-Algorithm
00261         while(!queue.empty())
00262         {
00263                 // Get the nearest cell from the queue
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 //              if(index == mStartPoint) break;
00272                 
00273                 // Add all adjacent cells
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         // Reset the plan
00359         int mapSize = mCurrentMap.getSize();
00360         for(int i = 0; i < mapSize; i++)
00361         {
00362                 mCurrentPlan[i] = -1;
00363         }
00364         
00365         // Initialize the queue with the goal point
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         // Do full search with Dijkstra-Algorithm
00375         while(!queue.empty())
00376         {
00377                 // Get the nearest cell from the queue
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                 // Add all adjacent cells
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         // Do nothing when paused
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         // Generate direction command from plan
00434         unsigned int current_x = 0, current_y = 0;
00435         if(!mCurrentMap.getCoordinates(current_x, current_y, mStartPoint)) // || !mCurrentMap.isFree(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         // Head towards (x,y)
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         // Create the command message
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         // Move the robot slowly ahead
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         // Do a full turn to have a initial map
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         // Move the robot slowly ahead
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                 // Check if we are asked to preempt
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                 // Check if we are localized successfully
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         // Start navigating according to the generated plan
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                 // Check if we are asked to preempt
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                 // Constantly replan every 3 seconds
00667                 if(cycle % recheckCycles == 0)
00668                 {
00669                         WallTime startTime = WallTime::now();
00670                         mStatus = NAV_ST_NAVIGATING;
00671                         
00672                         // Create the plan for navigation
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                 // Where are we now
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                 // Are we already close enough?
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                         // Are we also headed correctly?
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                 // Publish feedback via ActionServer
00764                 if(cycle%10 == 0)
00765                 {
00766                         nav2d_navigator::MoveToPosition2DFeedback fb;
00767                         fb.distance = mCurrentPlan[mStartPoint];
00768                         mMoveActionServer->publishFeedback(fb);
00769                 }
00770 
00771                 // Sleep remaining time
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         // Set ActionServer suceeded
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         // Move to exploration target
00807         Rate loopRate(FREQUENCY);
00808         while(true)
00809         {
00810                 // Check if we are asked to preempt
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                 // Where are we now
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                 // Regularly recheck for exploration target
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                         // Publish feedback via ActionServer
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                         // Create a new command and send it to Operator
00909                         generateCommand();
00910                 }
00911                 
00912                 // Sleep remaining time
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 }


nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:53:43