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


nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:13