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
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
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
00067 if(prune_plan_) {
00068 base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
00069 }
00070
00071
00072 costmap_ros_->clearRobotFootprint();
00073
00074
00075 if(transformed_plan.empty()) {
00076 return false;
00077 }
00078
00079
00080
00081
00082
00083
00084
00085
00086 {
00087 boost::mutex::scoped_lock lock(localPlan_mutex_);
00088
00089
00090
00091
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()) ;
00110
00111
00112 ivPlanSteps = true;
00113
00114
00115
00116
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
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
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
00179 }
00180
00181
00182 return false;
00183 }
00184
00185 void BipedRobinPlannerROS::stepsLeftCallback(const std_msgs::UInt8 stepsLeftInBuffer) {
00186 if(stepsLeftInBuffer.data == 0){
00187
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;
00198
00199
00200
00201
00202
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
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
00226 global_plan_.clear();
00227 global_plan_ = orig_global_plan;
00228
00229
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);
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
00316 updateMapToPlanner();
00317 ivPlanner.setStart(ivLeftLegStart, ivRightLegStart);
00318 ivPlanner.setGoal(ivLocalGoalX, ivLocalGoalY, ivLocalGoalTheta);
00319 }
00320
00321
00322 ivPlanner.replan();
00323
00324
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;
00330 }
00331
00332
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;
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
00346
00347
00348
00349
00350
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
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 };