RobotOperator.cpp
Go to the documentation of this file.
00001 #include <nav_msgs/GridCells.h>
00002 #include <math.h>
00003 
00004 #include <nav2d_operator/RobotOperator.h>
00005 
00006 #define PI 3.14159265
00007 
00008 RobotOperator::RobotOperator()
00009 {
00010         // Create the local costmap
00011         mLocalMap = new costmap_2d::Costmap2DROS("local_map", mTfListener);
00012         mRasterSize = mLocalMap->getCostmap()->getResolution();
00013         
00014         // Publish / subscribe to ROS topics
00015         ros::NodeHandle robotNode;
00016         robotNode.param("robot_frame", mRobotFrame, std::string("robot"));
00017         robotNode.param("odometry_frame", mOdometryFrame, std::string("odometry_base"));
00018         mCommandSubscriber = robotNode.subscribe(COMMAND_TOPIC, 1, &RobotOperator::receiveCommand, this);
00019         mControlPublisher = robotNode.advertise<geometry_msgs::Twist>(CONTROL_TOPIC, 1);
00020         mCostPublisher = robotNode.advertise<geometry_msgs::Vector3>("costs", 1);
00021         
00022         // Get parameters from the parameter server
00023         ros::NodeHandle operatorNode("~/");
00024         operatorNode.param("publish_route", mPublishRoute, false);
00025         if(mPublishRoute)
00026         {
00027                 ROS_INFO("Will publish desired direction on '%s' and control direction on '%s'.", ROUTE_TOPIC, PLAN_TOPIC);
00028                 mTrajectoryPublisher = operatorNode.advertise<nav_msgs::GridCells>(ROUTE_TOPIC, 1);
00029                 mPlanPublisher = operatorNode.advertise<nav_msgs::GridCells>(PLAN_TOPIC, 1);
00030         }
00031         operatorNode.param("max_free_space", mMaxFreeSpace, 5.0);
00032         operatorNode.param("safety_decay", mSafetyDecay, 0.95);
00033         operatorNode.param("distance_weight", mDistanceWeight, 1);
00034         operatorNode.param("safety_weight", mSafetyWeight, 1);
00035         operatorNode.param("conformance_weight", mConformanceWeight, 1);
00036         operatorNode.param("continue_weight", mContinueWeight, 1);
00037         operatorNode.param("max_velocity", mMaxVelocity, 1.0);
00038 
00039         // Apply tf_prefix to all used frame-id's
00040         mRobotFrame = mTfListener.resolve(mRobotFrame);
00041         mOdometryFrame = mTfListener.resolve(mOdometryFrame);
00042 
00043         // Initialize the lookup table for the driving directions
00044         ROS_INFO("Initializing LUT...");
00045         initTrajTable();
00046         ROS_INFO("...done!");
00047         
00048         // Set internal parameters
00049         mDesiredDirection = 0;
00050         mDesiredVelocity = 0;
00051         mCurrentDirection = 0;
00052         mCurrentVelocity = 0;
00053         mDriveMode = 0;
00054         mRecoverySteps = 0;
00055 }
00056 
00057 RobotOperator::~RobotOperator()
00058 {
00059         for(int i = 0; i < LUT_RESOLUTION; i++)
00060         {
00061                 delete mTrajTable[i];
00062         }
00063 }
00064 
00065 void RobotOperator::initTrajTable()
00066 {
00067         for(int i = 0; i < (LUT_RESOLUTION * 4) + 2; i++)
00068         {
00069                 mTrajTable[i] = NULL;
00070         }       
00071         for(int i = 1; i < LUT_RESOLUTION; i++)
00072         {
00073                 double tw = -PI * i / LUT_RESOLUTION;
00074                 double tx = cos(tw) + 1;
00075                 double ty = -sin(tw);
00076                 double tr = ((tx*tx)+(ty*ty))/(ty+ty);
00077                 std::vector<geometry_msgs::Point32> points;
00078                 double alpha = 0;
00079                 while(alpha < PI)
00080                 {
00081                         double x = tr * sin(alpha);
00082                         double y = tr * (1.0 - cos(alpha));
00083                         geometry_msgs::Point32 p;
00084                         p.x = x;
00085                         p.y = y;
00086                         p.z = 0;
00087                         points.push_back(p);
00088                         alpha += mRasterSize / tr;
00089                 }
00090                 // Add the PointCloud to the LUT
00091                 // Circle in forward-left direction
00092                 sensor_msgs::PointCloud* flcloud = new sensor_msgs::PointCloud();
00093                 flcloud->header.stamp = ros::Time(0);
00094                 flcloud->header.frame_id = mRobotFrame;
00095                 flcloud->points.resize(points.size());
00096                 
00097                 // Circle in forward-right direction
00098                 sensor_msgs::PointCloud* frcloud = new sensor_msgs::PointCloud();
00099                 frcloud->header.stamp = ros::Time(0);
00100                 frcloud->header.frame_id = mRobotFrame;
00101                 frcloud->points.resize(points.size());
00102                 
00103                 // Circle in backward-left direction
00104                 sensor_msgs::PointCloud* blcloud = new sensor_msgs::PointCloud();
00105                 blcloud->header.stamp = ros::Time(0);
00106                 blcloud->header.frame_id = mRobotFrame;
00107                 blcloud->points.resize(points.size());
00108                 
00109                 // Circle in backward-right direction
00110                 sensor_msgs::PointCloud* brcloud = new sensor_msgs::PointCloud();
00111                 brcloud->header.stamp = ros::Time(0);
00112                 brcloud->header.frame_id = mRobotFrame;
00113                 brcloud->points.resize(points.size());
00114                 
00115                 for(unsigned int j = 0; j < points.size(); j++)
00116                 {
00117                         flcloud->points[j] = points[j];
00118                         frcloud->points[j] = points[j];
00119                         blcloud->points[j] = points[j];
00120                         brcloud->points[j] = points[j];
00121                         
00122                         frcloud->points[j].y *= -1;
00123                         blcloud->points[j].x *= -1;
00124                         brcloud->points[j].x *= -1;
00125                         brcloud->points[j].y *= -1;
00126                 }
00127                 mTrajTable[LUT_RESOLUTION - i] = flcloud;
00128                 mTrajTable[LUT_RESOLUTION + i] = frcloud;
00129                 mTrajTable[(3 * LUT_RESOLUTION + 1) - i] = blcloud;
00130                 mTrajTable[(3 * LUT_RESOLUTION + 1) + i] = brcloud;
00131         }
00132         
00133         // Add First and Last LUT-element
00134         geometry_msgs::Point32 p;
00135         p.x = 0;
00136         p.y = 0;
00137         p.z = 0;
00138         
00139         sensor_msgs::PointCloud* turn = new sensor_msgs::PointCloud();
00140         turn->header.stamp = ros::Time(0);
00141         turn->header.frame_id = mRobotFrame;
00142         turn->points.resize(1);
00143         turn->points[0] = p;
00144         
00145         int straight_len = 5.0 / mRasterSize;
00146         
00147         sensor_msgs::PointCloud* fscloud = new sensor_msgs::PointCloud();
00148         fscloud->header.stamp = ros::Time(0);
00149         fscloud->header.frame_id = mRobotFrame;
00150         fscloud->points.resize(straight_len);
00151         
00152         sensor_msgs::PointCloud* bscloud = new sensor_msgs::PointCloud();
00153         bscloud->header.stamp = ros::Time(0);
00154         bscloud->header.frame_id = mRobotFrame;
00155         bscloud->points.resize(straight_len);
00156         
00157         for(int i = 0; i < straight_len; i++)
00158         {
00159                 fscloud->points[i] = p;
00160                 bscloud->points[i] = p;
00161                 bscloud->points[i].x *= -1;
00162                 p.x += mRasterSize;
00163         }
00164         
00165         mTrajTable[LUT_RESOLUTION] = fscloud;
00166         mTrajTable[LUT_RESOLUTION*3 + 1] = bscloud;
00167         
00168         mTrajTable[0] = turn;
00169         mTrajTable[LUT_RESOLUTION*2] = turn;
00170         mTrajTable[LUT_RESOLUTION*2 + 1] = turn;
00171         mTrajTable[LUT_RESOLUTION*4 + 1] = turn;
00172         
00173         for(int i = 0; i < (LUT_RESOLUTION * 4) + 2; i++)
00174         {
00175                 if(!mTrajTable[i])
00176                 {
00177                         ROS_ERROR("Table entry %d has not been initialized!", i);
00178                 }
00179         }       
00180 }
00181 
00182 void RobotOperator::receiveCommand(const nav2d_operator::cmd::ConstPtr& msg)
00183 {
00184         if(msg->Turn < -1 || msg->Turn > 1)
00185         {
00186                 // The given direction is invalid.
00187                 // Something is going wrong, so better stop the robot:
00188                 mDesiredDirection = 0;
00189                 mDesiredVelocity = 0;
00190                 mCurrentDirection = 0;
00191                 mCurrentVelocity = 0;
00192                 ROS_ERROR("Invalid turn direction on topic '%s'!", COMMAND_TOPIC);
00193                 return;
00194         }
00195         mDesiredDirection = msg->Turn;
00196         mDesiredVelocity = msg->Velocity * mMaxVelocity;
00197         mDriveMode = msg->Mode;
00198 }
00199 
00200 void RobotOperator::executeCommand()
00201 {
00202         // 1. Get a copy of the costmap to work on.
00203         mCostmap = mLocalMap->getCostmap();
00204         boost::unique_lock<boost::shared_mutex> lock(*(mCostmap->getLock()));
00205         double bestDirection, d;
00206         
00207         // 2. Set velocity and direction depending on drive mode
00208         switch(mDriveMode)
00209         {
00210         case 0:
00211                 bestDirection = findBestDirection();
00212                 d = bestDirection - mCurrentDirection;
00213                 if(d < -0.2) d = -0.2;
00214                 if(d > 0.2) d = 0.2;
00215                 mCurrentDirection += d;
00216                 mCurrentVelocity = mDesiredVelocity;
00217                 break;
00218         case 1:
00219                 mCurrentDirection = mDesiredDirection;
00220                 mCurrentVelocity = mDesiredVelocity;
00221                 break;
00222         default:
00223                 ROS_ERROR("Invalid drive mode!");
00224                 mCurrentVelocity = 0.0;
00225         }
00226         
00227         // Create some Debug-Info
00228         evaluateAction(mCurrentDirection, mCurrentVelocity, true);
00229         
00230         sensor_msgs::PointCloud* originalCloud = getPointCloud(mCurrentDirection, mDesiredVelocity);
00231         sensor_msgs::PointCloud transformedCloud;
00232 
00233         try
00234         {
00235                 mTfListener.transformPointCloud(mOdometryFrame,*originalCloud,transformedCloud);
00236         }
00237         catch(tf::TransformException ex)
00238         {
00239                 ROS_ERROR("%s", ex.what());
00240                 return;
00241         }
00242         
00243         // Determine maximum linear velocity
00244         int freeCells = calculateFreeSpace(&transformedCloud);
00245         double freeSpace = mRasterSize * freeCells;
00246 
00247         double safeVelocity = (freeSpace / mMaxFreeSpace) + 0.05;
00248         if(freeCells == transformedCloud.points.size() && safeVelocity < 0.5)
00249                 safeVelocity = 0.5;
00250                 
00251         if(freeSpace < 0.3 && freeCells < transformedCloud.points.size())
00252                 safeVelocity = 0;
00253 
00254         if(safeVelocity > mMaxVelocity)
00255                 safeVelocity = mMaxVelocity;
00256 
00257         // Check whether the robot is stuck
00258         if(mRecoverySteps > 0) mRecoverySteps--;
00259         if(safeVelocity < 0.1)
00260         {
00261                 if(mDriveMode == 0)
00262                 {
00263                         mRecoverySteps = 30; // Recover for 3 seconds
00264                         ROS_WARN_THROTTLE(1, "Robot is stuck! Trying to recover...");
00265                 }else
00266                 {
00267                         mCurrentVelocity = 0;
00268                         ROS_WARN_THROTTLE(1, "Robot cannot move further in this direction!");
00269                 }
00270         }
00271 
00272         // Publish route via ROS (mainly for debugging)
00273         if(mPublishRoute)
00274         {
00275                 nav_msgs::GridCells route_msg;
00276                 route_msg.header.frame_id = mOdometryFrame;
00277                 route_msg.header.stamp = ros::Time::now();
00278         
00279                 route_msg.cell_width = mCostmap->getResolution();
00280                 route_msg.cell_height = mCostmap->getResolution();
00281         
00282                 route_msg.cells.resize(freeCells);
00283                 for(int i = 0; i < freeCells; i++)
00284                 {
00285                         route_msg.cells[i].x = transformedCloud.points[i].x;
00286                         route_msg.cells[i].y = transformedCloud.points[i].y;
00287                         route_msg.cells[i].z = transformedCloud.points[i].z;
00288                 }
00289                 mTrajectoryPublisher.publish(route_msg);
00290         
00291                 // Publish plan via ROS (mainly for debugging)
00292                 sensor_msgs::PointCloud* originalPlanCloud = getPointCloud(mDesiredDirection, mDesiredVelocity);
00293                 sensor_msgs::PointCloud transformedPlanCloud;
00294 
00295                 try
00296                 {
00297                         mTfListener.transformPointCloud(mOdometryFrame,*originalPlanCloud,transformedPlanCloud);
00298                 }
00299                 catch(tf::TransformException ex)
00300                 {
00301                         ROS_ERROR("%s", ex.what());
00302                         return;
00303                 }
00304                 nav_msgs::GridCells plan_msg;
00305                 plan_msg.header = route_msg.header;
00306         
00307                 plan_msg.cell_width = mCostmap->getResolution();
00308                 plan_msg.cell_height = mCostmap->getResolution();
00309         
00310                 int freeSpacePlan = calculateFreeSpace(&transformedPlanCloud);
00311                 plan_msg.cells.resize(freeSpacePlan);
00312                 for(int i = 0; i < freeSpacePlan; i++)
00313                 {
00314                         plan_msg.cells[i].x = transformedPlanCloud.points[i].x;
00315                         plan_msg.cells[i].y = transformedPlanCloud.points[i].y;
00316                         plan_msg.cells[i].z = transformedPlanCloud.points[i].z;
00317                 }
00318                 mPlanPublisher.publish(plan_msg);
00319         }
00320         
00321         // Publish result via Twist-Message
00322         geometry_msgs::Twist controlMsg;
00323         double velocity = mCurrentVelocity;
00324         if(mCurrentDirection == 0)
00325         {
00326                 if(velocity > safeVelocity)
00327                 {
00328                         ROS_DEBUG("Desired velocity of %.2f is limited to %.2f", velocity, safeVelocity);
00329                         velocity = safeVelocity;
00330                 }else if(velocity < -safeVelocity)
00331                 {
00332                         ROS_DEBUG("Desired velocity of %.2f is limited to %.2f", velocity, -safeVelocity);
00333                         velocity = -safeVelocity;
00334                 }
00335                 controlMsg.linear.x = velocity;
00336                 controlMsg.angular.z = 0;
00337         }else if(mCurrentDirection == -1 || mCurrentDirection == 1)
00338         {
00339                 controlMsg.linear.x = 0;
00340                 controlMsg.angular.z = -1.0 * mCurrentDirection * velocity;
00341         }else
00342         {
00343                 double x = sin(mCurrentDirection * PI);
00344                 double y = (cos(mCurrentDirection * PI) + 1);
00345                 double r = ((x*x) + (y*y)) / (2*x);
00346                 double abs_r = (r > 0) ? r : -r;
00347                 velocity /= (1 + (1.0/abs_r));
00348                 if(velocity > safeVelocity)
00349                 {
00350                         ROS_DEBUG("Desired velocity of %.2f is limited to %.2f", velocity, safeVelocity);
00351                         velocity = safeVelocity;
00352                 }else if(velocity < -safeVelocity)
00353                 {
00354                         ROS_DEBUG("Desired velocity of %.2f is limited to %.2f", velocity, -safeVelocity);
00355                         velocity = -safeVelocity;
00356                 }
00357                 
00358                 controlMsg.linear.x = velocity;
00359                 controlMsg.angular.z = -1.0 / r * controlMsg.linear.x;
00360         }
00361         mControlPublisher.publish(controlMsg);
00362 }
00363 
00364 int RobotOperator::calculateFreeSpace(sensor_msgs::PointCloud* cloud)
00365 {       
00366         unsigned int mx, my;
00367         int length = cloud->points.size();
00368         int freeSpace = 0;
00369         for(int i = 0; i < length; i++)
00370         {
00371                 if(mCostmap->worldToMap(cloud->points[i].x, cloud->points[i].y, mx, my))
00372                 {
00373                         if(mCostmap->getCost(mx,my) < costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
00374                         {
00375                                 freeSpace++;
00376                         }else
00377                         {
00378                                 break;
00379                         }
00380                 }else
00381                 {
00382                         break;
00383                 }
00384         }
00385         return freeSpace;
00386 }
00387 
00388 double RobotOperator::evaluateAction(double direction, double velocity, bool debug)
00389 {
00390         sensor_msgs::PointCloud* originalCloud = getPointCloud(direction, velocity);
00391         sensor_msgs::PointCloud transformedCloud;
00392         try
00393         {
00394                 mTfListener.transformPointCloud(mOdometryFrame, *originalCloud,transformedCloud);
00395         }
00396         catch(tf::TransformException ex)
00397         {
00398                 ROS_ERROR("%s", ex.what());
00399                 return 1;
00400         }
00401         
00402         double valueDistance = 0.0;    // How far can the robot move in this direction?
00403         double valueSafety = 0.0;      // How safe is it to move in that direction?
00404         double valueConformance = 0.0; // How conform is it with the desired direction?
00405         
00406         double freeSpace = 0.0;
00407         double decay = 1.0;
00408         unsigned char cell_cost;
00409         double safety;
00410         
00411         // Calculate safety value
00412         int length = transformedCloud.points.size();
00413         bool gettingBetter = true;
00414         for(int i = 0; i < length; i++)
00415         {
00416                 unsigned int mx, my;
00417                 if(mCostmap->worldToMap(transformedCloud.points[i].x, transformedCloud.points[i].y, mx, my))
00418                 {
00419                         cell_cost = mCostmap->getCost(mx,my);
00420                         if(cell_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
00421                         {
00422                                 // Trajectory hit an obstacle
00423                                 break;
00424                         }
00425                 }
00426                 freeSpace += mRasterSize;
00427                         
00428                 safety = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - (cell_cost * decay);
00429                 if(gettingBetter)
00430                 {
00431                         if(safety >= valueSafety) valueSafety = safety;
00432                         else gettingBetter = false;
00433                 }else
00434                 {
00435                         if(safety < valueSafety) valueSafety = safety;
00436                 }
00437                 decay *= mSafetyDecay;
00438         }
00439         
00440         double action_value = 0.0;
00441         double normFactor = 0.0;
00442         valueSafety /= costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
00443         
00444         // Calculate distance value
00445         if(freeSpace >= mMaxFreeSpace)
00446         {
00447                 freeSpace = mMaxFreeSpace;
00448         }
00449         valueDistance = freeSpace / std::min(mMaxFreeSpace, length*mRasterSize);
00450         normFactor = mDistanceWeight + mSafetyWeight;
00451 
00452         if(mRecoverySteps == 0)
00453         {
00454                 // Calculate continuety value
00455                 double valueContinue = mCurrentDirection - direction;
00456                 if(valueContinue < 0) valueContinue *= -1;
00457                 valueContinue = 1.0 / (1.0 + exp(pow(valueContinue-0.5,15)));
00458                 
00459                 // Calculate conformance value
00460                 double desired_sq = (mDesiredDirection > 0) ? mDesiredDirection * mDesiredDirection : mDesiredDirection * -mDesiredDirection;
00461                 double evaluated_sq = (direction > 0) ? direction * direction : direction * -direction;
00462                 valueConformance = cos(PI / 2.0 * (desired_sq - evaluated_sq)); // cos(-PI/2 ... +PI/2) --> [0 .. 1 .. 0]
00463                 
00464                 action_value += valueConformance * mConformanceWeight;
00465                 action_value += valueContinue * mContinueWeight;
00466                 normFactor += mConformanceWeight + mContinueWeight;
00467         }
00468         
00469         action_value += valueDistance * mDistanceWeight;
00470         action_value += valueSafety * mSafetyWeight;
00471         action_value /=  normFactor;
00472         
00473         if(debug)
00474         {
00475                 geometry_msgs::Vector3 cost_msg;
00476                 cost_msg.x = valueDistance;
00477                 cost_msg.y = valueSafety;
00478 //              cost_msg.y = valueContinue;
00479                 cost_msg.z = valueConformance;
00480                 mCostPublisher.publish(cost_msg); 
00481         }
00482         
00483         return action_value;
00484 }
00485 
00486 double diff(double v1, double v2)
00487 {
00488         if(v1 > v2)
00489                 return v1 - v2;
00490         else
00491                 return v2 - v1;
00492 }
00493 
00494 double RobotOperator::findBestDirection()
00495 {
00496         double best_dir = -1.0;
00497         double best_value = 0.0;
00498         double step = 0.01;
00499         double dir = -1.0;
00500         
00501         while(dir <= 1.0)
00502         {
00503                 double value = evaluateAction(dir, mDesiredVelocity);
00504                 if(value > best_value)
00505                 {
00506                         best_dir = dir;
00507                         best_value = value;
00508                 }
00509                 dir += step;
00510         }
00511         return best_dir;
00512 }
00513 
00514 sensor_msgs::PointCloud* RobotOperator::getPointCloud(double direction, double velocity)
00515 {
00516         if(direction < -1) direction = -1;
00517         if(direction > 1) direction = 1;
00518         int offset = (velocity >= 0) ? LUT_RESOLUTION : 3*LUT_RESOLUTION + 1;
00519         int table_index = (direction * LUT_RESOLUTION) + offset;
00520         return mTrajTable[table_index];
00521 }


nav2d_operator
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:10