bipedRobin_local_planner: bipedRobin_planner

Go to the documentation of this file.
00001 #include <pluginlib/class_list_macros.h>
00002 #include <bipedRobin_local_planner/bipedRobin_planner_ros.h>
00003 #include <base_local_planner/goal_functions.h>
00004 #include <boost/thread.hpp>
00005 
00006 #include <footstep_planner/helper.h>
00007 #include <footstep_planner/FootstepPlanner.h>
00008 #include <footstep_planner/State.h>
00009 
00010 //register this planner as a BaseLocalPlanner plugin
00011 PLUGINLIB_DECLARE_CLASS(bipedRobin_local_planner, BipedRobinPlannerROS, bipedRobin_local_planner::BipedRobinPlannerROS, nav_core::BaseLocalPlanner)
00012 
00013 namespace bipedRobin_local_planner {
00014         void BipedRobinPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) {
00015                 if(!initialized_){
00016                         tf_ = tf;
00017                         costmap_ros_ = costmap_ros;
00018 
00019                         ros::NodeHandle gn;
00020                         ros::NodeHandle pn("~/" + name);
00021 
00022                         g_plan_pub_ = pn.advertise<nav_msgs::Path>("global_plan", 1);
00023                         l_plan_pub_ = pn.advertise<nav_msgs::Path>("local_plan", 1);
00024                         ivMapPub = pn.advertise<nav_msgs::OccupancyGrid>("local_map", 10);
00025                         stepsLeftInBufferSub = gn.subscribe<std_msgs::UInt8>("stepsLeftInBuffer", 10, &BipedRobinPlannerROS::stepsLeftCallback, this);
00026                         ivFootstepIncSrv = gn.serviceClient<bipedRobin_msgs::StepTarget3DService>("footstep3DInc_srv");
00027 
00028                         pn.param("prune_plan", prune_plan_, true);
00029 
00030                         ivIdFootRight = "/r_sole";
00031                         ivIdFootLeft = "/l_sole";
00032                         ivIdMapFrame = "/odom";
00033                         pn.param("rfoot_frame_id", ivIdFootRight, ivIdFootRight);
00034                         pn.param("lfoot_frame_id", ivIdFootLeft, ivIdFootLeft);
00035                         pn.param("map_frame_id", ivIdMapFrame, ivIdMapFrame);
00036                         pn.param("planning_distance", ivPlanningDistance, 1.0);
00037                         
00038                         if (!initializeNavigator()) {
00039                                 ROS_ERROR("Error during initialization of local footstep planner");
00040                                 return;
00041                         }
00042                                 
00043                         initialized_ = true;
00044                 } else{
00045                         ROS_WARN("This planner has already been initialized, doing nothing.");
00046                 }
00047         }
00048 
00049         bool BipedRobinPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
00050                 if(!initialized_){
00051                   ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00052                   return false;
00053                 }
00054 
00055                 std::vector<geometry_msgs::PoseStamped> local_plan;
00056                 tf::Stamped<tf::Pose> global_pose;
00057                 if(!costmap_ros_->getRobotPose(global_pose)) return false;
00058                 
00059                 std::vector<geometry_msgs::PoseStamped> transformed_plan;
00060                 //get the global plan in our frame
00061                 if(!base_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan)) {
00062                   ROS_WARN("Could not transform the global plan to the frame of the controller");
00063                   return false;
00064                 }
00065 
00066                 //now we'll prune the plan based on the position of the robot
00067                 if(prune_plan_) {
00068                         base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
00069                 }
00070                         
00071                 //we also want to clear the robot footprint from the costmap we're using
00072                 costmap_ros_->clearRobotFootprint();
00073 
00074                 //if the global plan passed in is empty... we won't do anything
00075                 if(transformed_plan.empty()) {
00076                         return false;
00077                 }
00078 
00079                 /*//Print PLAN
00080                 for (unsigned i=0; i<transformed_plan.size(); i++) {
00081                         tf::Stamped<tf::Pose> point;
00082                         tf::poseStampedMsgToTF(transformed_plan.at(i), point);
00083                         ROS_INFO("%.2f ", tf::getYaw(point.getRotation()));
00084                 }*/
00085 
00086                 {
00087                         boost::mutex::scoped_lock lock(localPlan_mutex_);
00088                 
00089                         //ROS_INFO("globale pose %f %f", global_pose.getOrigin().x(), global_pose.getOrigin().y());
00090                         
00091                         // get the goal position
00092                         tf::Stamped<tf::Pose> point;
00093                         double distance;
00094                         unsigned int i = transformed_plan.size();
00095                         do  {
00096                                 i--;
00097                                 tf::poseStampedMsgToTF(transformed_plan.at(i), point);
00098                                 tf::Transform delta = global_pose.inverse() * point;
00099                                 distance = sqrt(pow(delta.getOrigin().x(), 2.0) + pow(delta.getOrigin().y(), 2.0));
00100                         } while (distance > ivPlanningDistance && i >= 0);
00101                         
00102                         tf::Stamped<tf::Pose> goal_point;
00103                         tf::poseStampedMsgToTF(transformed_plan.at(i), goal_point);
00104                         tf::Stamped<tf::Pose> temp_point;
00105                         tf::poseStampedMsgToTF(transformed_plan.at(i-1), temp_point);
00106                         
00107                         ivLocalGoalX = goal_point.getOrigin().getX();
00108                         ivLocalGoalY = goal_point.getOrigin().getY();
00109                         ivLocalGoalTheta = atan2(ivLocalGoalY-temp_point.getOrigin().getY(), ivLocalGoalX-temp_point.getOrigin().getX()) ; //+ 1.57
00110                         
00111                         
00112                         ivPlanSteps = true;
00113 
00114                         // check if there are steps in the local plan and if the robot is ready to get a new step
00115                         
00116                         //if (ivStepsToStart.size()) ROS_INFO("Listengroese rauslesen %d", ivStepsToStart.size());
00117                         if (ivStepsToStart.size() > 0 && ivSendStep>0) { 
00118                                 bipedRobin_msgs::StepTarget3DService stepSrv;
00119                                 bipedRobin_msgs::StepTarget3D footstep;
00120                                 
00121                                 footstep_planner::State supportStepPosition;
00122                                 const footstep_planner::State nextStepPosition = ivStepsToStart.front();
00123                                 
00124                                 ivStepsToStart.pop_front();
00125                                 
00126                                 if (nextStepPosition.getLeg() == footstep_planner::LEFT) {
00127                                         supportStepPosition = ivRightLegCurrent;
00128                                         ivLeftLegCurrent = nextStepPosition;
00129                                 } else {
00130                                         supportStepPosition = ivLeftLegCurrent;
00131                                         ivRightLegCurrent = nextStepPosition;
00132                                 }
00133                                         
00134                                         
00135                                 getFootstepFromState(supportStepPosition, nextStepPosition, &footstep);
00136                                 
00137                                 
00138                                 stepSrv.request.step = footstep;
00139                                 
00140                                 ivFootstepIncSrv.call(stepSrv);
00141                                 
00142                                 
00143                                 ROS_INFO("current step to go: %f %f %f %d %d", footstep.pose.position.x, footstep.pose.position.y, tf::getYaw(footstep.pose.orientation), footstep.leg, ivStepsToStart.size());
00144                                 
00145                                 ivSendStep--;
00146                         }
00147                 }
00148                         
00149 
00150                 /*if (ivFootstepPlanningMutex.try_lock()) {
00151                         updateMapToPlanner();
00152                         ivPlanner.setStart(ivLeftLegStart, ivRightLegStart);
00153                         ivPlanner.setGoal(goal_x, goal_y, goal_th);             
00154                         ivFootstepPlanningThread = new boost::thread(boost::bind(&BipedRobinPlannerROS::planningThread,this));
00155                 } else {
00156                         ROS_INFO("Planner is still looking for steps to the local goal");
00157                 }*/
00158 
00159                 //cmd_vel.linear.x = 0.0;
00160                 //cmd_vel.linear.y = 0.0;
00161                 //cmd_vel.angular.z = 0.0;
00162                 //ROS_INFO("A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
00163 
00164                 //publish information to the visualizer
00165                 base_local_planner::publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
00166                 base_local_planner::publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
00167                 return true;
00168         }
00169 
00170         bool BipedRobinPlannerROS::isGoalReached(){
00171                 if(!initialized_){
00172                         ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00173                         return false;
00174                 }
00175 
00176                 {
00177                         boost::mutex::scoped_lock lock(localPlan_mutex_);
00178                         //return (ivStepsToStart.size() == 1);
00179                 }
00180                 //TODO
00181                 //return base_local_planner::goalPositionReached(global_pose, goal_x, goal_y, xy_goal_tolerance_) && base_local_planner::goalOrientationReached(global_pose, goal_th, yaw_goal_tolerance_);
00182                 return false;
00183         }
00184         
00185         void BipedRobinPlannerROS::stepsLeftCallback(const std_msgs::UInt8 stepsLeftInBuffer) {
00186                 if(stepsLeftInBuffer.data == 0){ //if the step buffer or the robot is empty
00187                         //ros::Duration(0.8).sleep(); //wait a little bit shorter than one step time
00188                         ROS_INFO (" Step needed");
00189                         ivSendStep++;
00190                 }
00191         }
00192         
00193         void  BipedRobinPlannerROS::getFootstepFromState(const footstep_planner::State& from,  const footstep_planner::State& to, bipedRobin_msgs::StepTarget3D* footstep)  {
00194                 tf::Pose fromPose (tf::createQuaternionFromYaw(from.getTheta()), tf::Point(from.getX(), from.getY(), 0.0));
00195                 tf::Pose toPose (tf::createQuaternionFromYaw(to.getTheta()), tf::Point(to.getX(), to.getY(), 0.0));
00196                 
00197                 tf::Transform step = fromPose.inverse() * toPose;  // get footstep to reach 'to' from 'from'
00198 
00199                 //ROS_INFO("from step : %f %f %f %d", from.getX(), from.getY(), from.getTheta(), from.getLeg());
00200                 //ROS_INFO("to step : %f %f %f %d", to.getX(), to.getY(), to.getTheta(), to.getLeg());
00201                 
00202                 // set the footstep
00203                 footstep->pose.position.x = step.getOrigin().x();
00204                 footstep->pose.position.y = step.getOrigin().y();
00205                 footstep->pose.position.z = step.getOrigin().z();
00206                 footstep->pose.orientation.x = step.getRotation().x();
00207                 footstep->pose.orientation.y = step.getRotation().y();
00208                 footstep->pose.orientation.z = step.getRotation().z();
00209                 footstep->pose.orientation.w = step.getRotation().w();
00210                 if (to.getLeg() == footstep_planner::LEFT) {
00211                         footstep->leg = humanoid_nav_msgs::StepTarget::left;
00212                 } else {
00213                         footstep->leg = humanoid_nav_msgs::StepTarget::right;
00214                 }
00215                 
00216                 //ROS_INFO("delta step: %f %f %d", footstep->pose.position.x, footstep->pose.position.y, footstep->leg);
00217         }
00218                         
00219         bool BipedRobinPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
00220                 if(!initialized_){
00221                         ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00222                         return false;
00223                 }
00224 
00225                 //reset the global plan
00226                 global_plan_.clear();
00227                 global_plan_ = orig_global_plan;
00228                 
00229                 //TODO stop planning and reset local plan as well
00230                 
00231                 return true;
00232         }
00233 
00234         void BipedRobinPlannerROS::updateMapToPlanner() {
00235                 nav_msgs::OccupancyGrid::Ptr occGrid(new nav_msgs::OccupancyGrid());
00236 
00237                 costmap_2d::Costmap2D costmap;
00238                 costmap_ros_->getCostmapCopy(costmap);
00239 
00240                 const unsigned char* charMap = costmap.getCharMap();
00241                 for(unsigned int i = 0; i < costmap_ros_->getSizeInCellsX()*costmap_ros_->getSizeInCellsY(); i++) {
00242                         if(charMap[i] == costmap_2d::LETHAL_OBSTACLE)
00243                                 occGrid->data.push_back(100);
00244                         else if(charMap[i] == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
00245                                 occGrid->data.push_back(50);
00246                         else if(charMap[i] == costmap_2d::NO_INFORMATION)
00247                                 occGrid->data.push_back(-1);
00248                         else 
00249                                 occGrid->data.push_back(0); 
00250                 }
00251 
00252                 occGrid->header.frame_id = costmap_ros_->getGlobalFrameID();
00253                 occGrid->info.resolution=costmap_ros_->getResolution();
00254                 occGrid->info.width=costmap_ros_->getSizeInCellsX();
00255                 occGrid->info.height=costmap_ros_->getSizeInCellsY();
00256                 occGrid->info.origin.position.x=costmap.getOriginX();
00257                 occGrid->info.origin.position.y=costmap.getOriginY();
00258 
00259                 ivMapPub.publish(occGrid); //just for visualization
00260 
00261                 gridmap_2d::GridMap2DPtr map(new gridmap_2d::GridMap2D(occGrid));
00262                 ivIdMapFrame = map->getFrameID();
00263                 ivPlanner.updateMap(map);
00264         }
00265 
00266         bool BipedRobinPlannerROS::initializeNavigator() {
00267                 try {
00268                         tf::StampedTransform transOdomLeft;
00269                         tf::StampedTransform transOdomRight;
00270                         
00271                         const ros::Time now = ros::Time::now();
00272                         
00273                         tf_->waitForTransform(ivIdMapFrame, ivIdFootLeft, now, ros::Duration(1.0));
00274                         tf_->lookupTransform(ivIdMapFrame, ivIdFootLeft, now, transOdomLeft);
00275                         ivLeftLegStart.setX(transOdomLeft.getOrigin().x());
00276                         ivLeftLegStart.setY(transOdomLeft.getOrigin().y());
00277                         ivLeftLegStart.setTheta(tf::getYaw(transOdomLeft.getRotation()));
00278                         ivLeftLegStart.setLeg(footstep_planner::LEFT);
00279                         ivLeftLegCurrent = ivLeftLegStart;
00280                         ROS_INFO("Initialized left feet at %.3f %.3f %.3f", transOdomLeft.getOrigin().x(), transOdomLeft.getOrigin().y(), tf::getYaw(transOdomLeft.getRotation()));
00281 
00282                         tf_->waitForTransform(ivIdMapFrame, ivIdFootRight, now, ros::Duration(1.0));
00283                         tf_->lookupTransform(ivIdMapFrame, ivIdFootRight, now, transOdomRight);
00284                         ivRightLegStart.setX(transOdomRight.getOrigin().x());
00285                         ivRightLegStart.setY(transOdomRight.getOrigin().y());
00286                         ivRightLegStart.setTheta(tf::getYaw(transOdomRight.getRotation()));
00287                         ivRightLegStart.setLeg(footstep_planner::RIGHT);
00288                         ivRightLegCurrent = ivRightLegStart;
00289                         ROS_INFO("Initialized right feet at %.3f %.3f %.3f", transOdomRight.getOrigin().x(), transOdomRight.getOrigin().y(), tf::getYaw(transOdomRight.getRotation()));
00290                         
00291                         ivPlanSteps = false;
00292                         ivFootstepPlanningThread = new boost::thread(boost::bind(&BipedRobinPlannerROS::planningThread,this));
00293                         
00294                         ivSendStep = true;
00295 
00296                 } catch (tf::TransformException ex) {
00297                         ROS_ERROR("%s", ex.what());
00298                         return false;
00299                 }
00300                 return true;
00301         }
00302         
00303         void BipedRobinPlannerROS::planningThread(void) {
00304                 
00305                 ROS_INFO ("Footstep Planning Task has started");
00306                 
00307                 do {
00308                         if (ivPlanSteps) {
00309                                 
00310 
00311                                 
00312                                 {
00313                                         boost::mutex::scoped_lock lock(planner_mutex_);
00314                                         
00315                                         // update planner infos
00316                                         updateMapToPlanner();
00317                                         ivPlanner.setStart(ivLeftLegStart, ivRightLegStart);
00318                                         ivPlanner.setGoal(ivLocalGoalX, ivLocalGoalY, ivLocalGoalTheta);                                                
00319                                 }       
00320 
00321                                 // replan path
00322                                 ivPlanner.replan();
00323         
00324                                 // set interruption point
00325                                 try {
00326                                         boost::this_thread::interruption_point();
00327                                 } catch (const boost::thread_interrupted&){
00328                                         ROS_INFO("Planning Task was interrupted. Planner is starting again.");
00329                                         continue; //just start a replan
00330                                 }
00331                                 
00332                                 //check if new plan is realy a new plan
00333                                 footstep_planner::state_iter_t to_planned = ivPlanner.getPathBegin();
00334                                 footstep_planner::state_iter_t to_planned2 = ivPlanner.getPathBegin();
00335                                 if (to_planned == ivPlanner.getPathEnd()) {
00336                                         ROS_ERROR("No local plan found or steps are planned till goal");
00337                                         continue;  //just start a replan
00338                                 }
00339                                 
00340                                 {
00341                                         boost::mutex::scoped_lock lock(localPlan_mutex_);
00342                                         ROS_INFO(" 1234 current support leg: %f %f %f %d", to_planned.base()->getX(), to_planned.base()->getY(), to_planned.base()->getTheta(), to_planned.base()->getLeg());
00343                                                 
00344                                         
00345                                         //while(to_planned2!= ivPlanner.getPathEnd()) {
00346                                         //      ROS_INFO("                               local plan: %f %f %f %d", to_planned2.base()->getX(), to_planned2.base()->getY(), to_planned2.base()->getTheta(), to_planned2.base()->getLeg());
00347                                         //      to_planned2++;
00348                                         //}
00349                                         
00350                                         //add steps to the local plan
00351                                         while (ivStepsToStart.size() < 3 && to_planned!= ivPlanner.getPathEnd()) {
00352                                                 to_planned++;
00353                                                 ivStepsToStart.push_back(*to_planned.base());
00354                                                 
00355                                                 ROS_INFO(" 1234 step putted in buffer: %f %f %f %d", to_planned.base()->getX(), to_planned.base()->getY(), to_planned.base()->getTheta(), to_planned.base()->getLeg());
00356                                                 
00357                                                 if (to_planned->getLeg() == 0) ivRightLegStart = *to_planned.base();
00358                                                 else ivLeftLegStart = *to_planned.base();                       
00359                                         }
00360                                         //ROS_INFO("Listengroese reinschreiben %d", ivStepsToStart.size());
00361                                 }
00362                                 
00363                         }
00364                         ros::Duration(0.1).sleep();
00365                 } while(true) ;
00366                 
00367                 ROS_INFO ("Footstep Planning Task has terminated");
00368                 ivFootstepPlanningMutex.unlock();
00369         }       
00370 };