footstep_planner.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_footstep_planner/footstep_planner.h"
00037 #include <jsk_topic_tools/log_utils.h>
00038 #include <jsk_topic_tools/rosparam_utils.h>
00039 #include <jsk_recognition_utils/pcl_conversion_util.h>
00040 #include <pcl/common/angles.h>
00041 #include <boost/format.hpp>
00042 
00043 namespace jsk_footstep_planner
00044 {
00045   FootstepPlanner::FootstepPlanner(ros::NodeHandle& nh):
00046     as_(nh, nh.getNamespace(),
00047         boost::bind(&FootstepPlanner::planCB, this, _1), false)
00048   {
00049     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
00050     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00051       boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
00052     srv_->setCallback (f);
00053     pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
00054       "text", 1, true);
00055     pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
00056       "close_list", 1, true);
00057     pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
00058       "open_list", 1, true);
00059     srv_project_footprint_ = nh.advertiseService(
00060       "project_footprint", &FootstepPlanner::projectFootPrintService, this);
00061     srv_project_footprint_with_local_search_ = nh.advertiseService(
00062       "project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
00063     srv_collision_bounding_box_info_ = nh.advertiseService(
00064       "collision_bounding_box_info", &FootstepPlanner::collisionBoundingBoxInfoService, this);
00065     srv_project_footstep_ = nh.advertiseService(
00066       "project_footstep", &FootstepPlanner::projectFootstepService, this);
00067     srv_set_heuristic_path_ = nh.advertiseService(
00068       "set_heuristic_path", &FootstepPlanner::setHeuristicPathService, this);
00069     std::vector<double> lleg_footstep_offset, rleg_footstep_offset;
00070     if (jsk_topic_tools::readVectorParameter(nh, "lleg_footstep_offset", lleg_footstep_offset)) {
00071       inv_lleg_footstep_offset_ = Eigen::Vector3f(- lleg_footstep_offset[0],
00072                                                   - lleg_footstep_offset[1],
00073                                                   - lleg_footstep_offset[2]);
00074     } else {
00075       inv_lleg_footstep_offset_ = Eigen::Vector3f(0, 0, 0);
00076     }
00077     if (jsk_topic_tools::readVectorParameter(nh, "rleg_footstep_offset", rleg_footstep_offset)) {
00078       inv_rleg_footstep_offset_ = Eigen::Vector3f(- rleg_footstep_offset[0],
00079                                                   - rleg_footstep_offset[1],
00080                                                   - rleg_footstep_offset[2]);
00081     } else {
00082       inv_rleg_footstep_offset_ = Eigen::Vector3f(0, 0, 0);
00083     }
00084     {
00085       boost::mutex::scoped_lock lock(mutex_);
00086       if (!readSuccessors(nh)) {
00087         return;
00088       }
00089 
00090       ROS_INFO("building graph");
00091       buildGraph();
00092       ROS_INFO("build graph done");
00093     }
00094     sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
00095     sub_obstacle_model_ = nh.subscribe("obstacle_model", 1, &FootstepPlanner::obstacleCallback, this);
00096     std::vector<double> collision_bbox_size, collision_bbox_offset;
00097     if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_size", collision_bbox_size)) {
00098       collision_bbox_size_[0] = collision_bbox_size[0];
00099       collision_bbox_size_[1] = collision_bbox_size[1];
00100       collision_bbox_size_[2] = collision_bbox_size[2];
00101     }
00102     if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_offset", collision_bbox_offset)) {
00103       collision_bbox_offset_ = Eigen::Affine3f::Identity() * Eigen::Translation3f(collision_bbox_offset[0],
00104                                                                                   collision_bbox_offset[1],
00105                                                                                   collision_bbox_offset[2]);
00106     }
00107     as_.start();
00108   }
00109 
00110   void FootstepPlanner::obstacleCallback(
00111     const sensor_msgs::PointCloud2::ConstPtr& msg)
00112   {
00113     boost::mutex::scoped_lock lock(mutex_);
00114     obstacle_model_.reset(new pcl::PointCloud<pcl::PointXYZ>);
00115     pcl::fromROSMsg(*msg, *obstacle_model_);
00116     obstacle_model_frame_id_ = msg->header.frame_id;
00117     if (graph_ && use_obstacle_model_) {
00118       graph_->setObstacleModel(obstacle_model_);
00119     }
00120   }
00121   
00122   void FootstepPlanner::pointcloudCallback(
00123     const sensor_msgs::PointCloud2::ConstPtr& msg)
00124   {
00125     boost::mutex::scoped_lock lock(mutex_);
00126     ROS_DEBUG("pointcloud model is updated");
00127     pointcloud_model_.reset(new pcl::PointCloud<pcl::PointNormal>);
00128     pcl::fromROSMsg(*msg, *pointcloud_model_);
00129     pointcloud_model_frame_id_ = msg->header.frame_id;
00130     if (graph_ && use_pointcloud_model_) {
00131       graph_->setPointCloudModel(pointcloud_model_);
00132     }
00133   }
00134 
00135   bool FootstepPlanner::projectFootPrint(
00136     const Eigen::Affine3f& center_pose,
00137     const Eigen::Affine3f& left_pose_trans,
00138     const Eigen::Affine3f& right_pose_trans,
00139     geometry_msgs::Pose& pose)
00140   {
00141     const Eigen::Vector3f resolution(resolution_x_,
00142                                      resolution_y_,
00143                                      resolution_theta_);
00144     const Eigen::Vector3f footstep_size(footstep_size_x_,
00145                                         footstep_size_y_,
00146                                         0.000001);
00147     Eigen::Affine3f left_pose = center_pose * left_pose_trans;
00148     Eigen::Affine3f right_pose = center_pose * right_pose_trans;
00149     FootstepState::Ptr left_input(new FootstepState(
00150                                     jsk_footstep_msgs::Footstep::LEFT,
00151                                     left_pose,
00152                                     footstep_size,
00153                                     resolution));
00154     FootstepState::Ptr right_input(new FootstepState(
00155                                     jsk_footstep_msgs::Footstep::RIGHT,
00156                                     right_pose,
00157                                     footstep_size,
00158                                     resolution));
00159     FootstepState::Ptr projected_left = graph_->projectFootstep(left_input);
00160     FootstepState::Ptr projected_right = graph_->projectFootstep(right_input);
00161     if (!projected_left || !projected_right) {
00162       return false;
00163     }
00164     Eigen::Affine3f projected_left_pose = projected_left->getPose();
00165     Eigen::Affine3f projected_right_pose = projected_right->getPose();
00166     Eigen::Quaternionf rot = Eigen::Quaternionf(projected_left_pose.rotation()).slerp(
00167       0.5, Eigen::Quaternionf(projected_right_pose.rotation()));
00168     Eigen::Vector3f pos = (Eigen::Vector3f(projected_right_pose.translation()) +
00169                            Eigen::Vector3f(projected_left_pose.translation())) / 2.0;
00170     Eigen::Affine3f mid = Eigen::Translation3f(pos) * rot;
00171     tf::poseEigenToMsg(mid, pose);
00172     return true;
00173   }
00174   
00175   bool FootstepPlanner::projectFootPrintWithLocalSearchService(
00176     jsk_interactive_marker::SnapFootPrint::Request& req,
00177     jsk_interactive_marker::SnapFootPrint::Response& res)
00178   {
00179     boost::mutex::scoped_lock lock(mutex_);
00180     if (!graph_ ) {
00181       return false;
00182     }
00183     if (use_pointcloud_model_ && !pointcloud_model_) {
00184       ROS_ERROR("No pointcloud model is yet available");
00185       publishText(pub_text_,
00186                   "No pointcloud model is yet available",
00187                   ERROR);
00188       return false;
00189     }
00190     Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
00191     std::vector<Eigen::Affine3f> center_poses;
00192     tf::poseMsgToEigen(req.lleg_pose, left_pose_trans);
00193     tf::poseMsgToEigen(req.rleg_pose, right_pose_trans);
00194     tf::poseMsgToEigen(req.input_pose.pose, center_pose);
00195     const double dx = 0.05;
00196     const double dy = 0.05;
00197     const double dtheta = pcl::deg2rad(5.0);
00198     for (int xi = 0; xi < 3; xi++) {
00199       for (int yi = 0; yi < 3; yi++) {
00200         for (int thetai = 0; thetai < 3; thetai++) {
00201           Eigen::Affine3f transppp = affineFromXYYaw(xi * dx, yi * dy, thetai * dtheta);
00202           Eigen::Affine3f transppm = affineFromXYYaw(xi * dx, yi * dy, - thetai * dtheta);
00203           Eigen::Affine3f transpmp = affineFromXYYaw(xi * dx, - yi * dy, thetai * dtheta);
00204           Eigen::Affine3f transpmm = affineFromXYYaw(xi * dx, - yi * dy, -thetai * dtheta);
00205           Eigen::Affine3f transmpp = affineFromXYYaw(- xi * dx, yi * dy, thetai * dtheta);
00206           Eigen::Affine3f transmpm = affineFromXYYaw(- xi * dx, yi * dy, - thetai * dtheta);
00207           Eigen::Affine3f transmmp = affineFromXYYaw(- xi * dx, - yi * dy, thetai * dtheta);
00208           Eigen::Affine3f transmmm = affineFromXYYaw(- xi * dx, - yi * dy, - thetai * dtheta);
00209           center_poses.push_back(center_pose * transppp);
00210           center_poses.push_back(center_pose * transppm);
00211           center_poses.push_back(center_pose * transpmp);
00212           center_poses.push_back(center_pose * transpmm);
00213           center_poses.push_back(center_pose * transmpp);
00214           center_poses.push_back(center_pose * transmpm);
00215           center_poses.push_back(center_pose * transmmp);
00216           center_poses.push_back(center_pose * transmmm);
00217         }
00218       }
00219     }
00220     for (size_t i = 0; i < center_poses.size(); i++) {
00221       if (projectFootPrint(center_poses[i], left_pose_trans, right_pose_trans,
00222                            res.snapped_pose.pose)) {
00223         res.success = true;
00224         res.snapped_pose.header = req.input_pose.header;
00225         return true;
00226       }
00227     }
00228     ROS_ERROR("Failed to project footprint");
00229     publishText(pub_text_,
00230                 "Failed to project goal",
00231                 ERROR);
00232     return false;
00233   }
00234 
00235   bool FootstepPlanner::collisionBoundingBoxInfoService(
00236       jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
00237       jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res)
00238   {
00239     boost::mutex::scoped_lock lock(mutex_);
00240     res.box_dimensions.x = collision_bbox_size_[0];
00241     res.box_dimensions.y = collision_bbox_size_[1];
00242     res.box_dimensions.z = collision_bbox_size_[2];
00243     tf::poseEigenToMsg(collision_bbox_offset_, res.box_offset);
00244     return true;
00245   }
00246 
00247   bool FootstepPlanner::projectFootstepService(
00248     jsk_footstep_planner::ProjectFootstep::Request& req,
00249     jsk_footstep_planner::ProjectFootstep::Response& res)
00250   {
00251     boost::mutex::scoped_lock lock(mutex_);
00252     if (!graph_) {
00253       return false;
00254     }
00255     if (!pointcloud_model_) {
00256       ROS_ERROR("No pointcloud model is yet available");
00257       //publishText(pub_text_,
00258       //"No pointcloud model is yet available",
00259       //ERROR);
00260       return false;
00261     }
00262 
00263     const Eigen::Vector3f resolution(resolution_x_,
00264                                      resolution_y_,
00265                                      resolution_theta_);
00266     const Eigen::Vector3f footstep_size(footstep_size_x_,
00267                                         footstep_size_y_,
00268                                         0.000001);
00269 
00270     for (std::vector<jsk_footstep_msgs::Footstep>::iterator it = req.input.footsteps.begin();
00271          it != req.input.footsteps.end(); it++) {
00272       if (it->offset.x == 0.0 &&
00273           it->offset.y == 0.0 &&
00274           it->offset.z == 0.0 ) {
00275         if (it->leg == jsk_footstep_msgs::Footstep::LEFT) {
00276           it->offset.x = - inv_lleg_footstep_offset_[0];
00277           it->offset.y = - inv_lleg_footstep_offset_[1];
00278           it->offset.z = - inv_lleg_footstep_offset_[2];
00279         } else {
00280           it->offset.x = - inv_rleg_footstep_offset_[0];
00281           it->offset.y = - inv_rleg_footstep_offset_[1];
00282           it->offset.z = - inv_rleg_footstep_offset_[2];
00283         }
00284       }
00285       if(it->dimensions.x == 0 &&
00286          it->dimensions.y == 0 &&
00287          it->dimensions.z == 0 ) {
00288         it->dimensions.x = footstep_size_x_;
00289         it->dimensions.y = footstep_size_y_;
00290         it->dimensions.z = 0.000001;
00291       }
00292       FootstepState::Ptr step = FootstepState::fromROSMsg(*it, footstep_size, resolution);
00293       FootstepState::Ptr projected = graph_->projectFootstep(step);
00294       if(!!projected) {
00295         res.success.push_back(true);
00296         jsk_footstep_msgs::Footstep::Ptr p;
00297         if (it->leg == jsk_footstep_msgs::Footstep::LEFT) {
00298           p = projected->toROSMsg(inv_lleg_footstep_offset_);
00299         } else if (it->leg == jsk_footstep_msgs::Footstep::RIGHT) {
00300           p = projected->toROSMsg(inv_rleg_footstep_offset_);
00301         } else {
00302           p = projected->toROSMsg();
00303         }
00304         res.result.footsteps.push_back(*p);
00305       } else {
00306         res.success.push_back(false);
00307         res.result.footsteps.push_back(*it); // return the same step as in input
00308       }
00309     }
00310     res.result.header = req.input.header;
00311 
00312     return true;
00313   }
00314 
00315   bool FootstepPlanner::projectFootPrintService(
00316     jsk_interactive_marker::SnapFootPrint::Request& req,
00317     jsk_interactive_marker::SnapFootPrint::Response& res)
00318   {
00319     boost::mutex::scoped_lock lock(mutex_);
00320     if (!graph_) {
00321       return false;
00322     }
00323     if (!pointcloud_model_) {
00324       ROS_ERROR("No pointcloud model is yet available");
00325       publishText(pub_text_,
00326                   "No pointcloud model is yet available",
00327                   ERROR);
00328       return false;
00329     }
00330     Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
00331     tf::poseMsgToEigen(req.lleg_pose, left_pose_trans);
00332     tf::poseMsgToEigen(req.rleg_pose, right_pose_trans);
00333     tf::poseMsgToEigen(req.input_pose.pose, center_pose);
00334     if (projectFootPrint(center_pose, left_pose_trans, right_pose_trans,
00335                          res.snapped_pose.pose)) {
00336       res.success = true;
00337       res.snapped_pose.header = req.input_pose.header;
00338       return true;
00339     }
00340     else {
00341       ROS_ERROR("Failed to project footprint");
00342       publishText(pub_text_,
00343                   "Failed to project goal",
00344                   ERROR);
00345       return false;
00346     }
00347   }
00348   bool FootstepPlanner::setHeuristicPathService (
00349     jsk_footstep_planner::SetHeuristicPath::Request& req,
00350     jsk_footstep_planner::SetHeuristicPath::Response& res)
00351   {
00352     boost::mutex::scoped_lock lock(mutex_);
00353     if (!graph_) {
00354       return false;
00355     }
00356     std::vector<Eigen::Vector3f> points;
00357     for(int i = 0; i < req.segments.size(); i++) {
00358       Eigen::Vector3f p(req.segments[i].x,
00359                         req.segments[i].y,
00360                         req.segments[i].z);
00361       points.push_back(p);
00362     }
00363     jsk_recognition_utils::PolyLine pl(points);
00364     setHeuristicPathLine(pl);
00365 
00366     return true;
00367   }
00368 
00369   void FootstepPlanner::publishText(ros::Publisher& pub,
00370                                     const std::string& text,
00371                                     PlanningStatus status)
00372   {
00373     std_msgs::ColorRGBA ok_color;
00374     ok_color.r = 0.3568627450980392;
00375     ok_color.g = 0.7529411764705882;
00376     ok_color.b = 0.8705882352941177;
00377     ok_color.a = 1.0;
00378     std_msgs::ColorRGBA warn_color;
00379     warn_color.r = 0.9411764705882353;
00380     warn_color.g = 0.6784313725490196;
00381     warn_color.b = 0.3058823529411765;
00382     warn_color.a = 1.0;
00383     std_msgs::ColorRGBA error_color;
00384     error_color.r = 0.8509803921568627;
00385     error_color.g = 0.3254901960784314;
00386     error_color.b = 0.30980392156862746;
00387     error_color.a = 1.0;
00388     std_msgs::ColorRGBA color;
00389     if (status == OK) {
00390       color = ok_color;
00391     }
00392     else if (status == WARNING) {
00393       color = warn_color;
00394     }
00395     else if (status == ERROR) {
00396       color = error_color;
00397     }
00398     jsk_rviz_plugins::OverlayText msg;
00399     msg.text = text;
00400     msg.width = 1000;
00401     msg.height = 1000;
00402     msg.top = 10;
00403     msg.left = 10;
00404     msg.bg_color.a = 0.0;
00405     msg.fg_color = color;
00406     msg.text_size = 24;
00407     pub.publish(msg);
00408   }
00409 
00410   void FootstepPlanner::planCB(
00411     const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
00412   {
00413     boost::mutex::scoped_lock lock(mutex_);
00414     latest_header_ = goal->goal_footstep.header;
00415     ROS_INFO("planCB");
00416     // check message sanity
00417     if (goal->initial_footstep.footsteps.size() == 0) {
00418       ROS_ERROR("no initial footstep is specified");
00419       as_.setPreempted();
00420       return;
00421     }
00422     if (goal->goal_footstep.footsteps.size() != 2) {
00423       ROS_ERROR("Need to specify 2 goal footsteps");
00424       as_.setPreempted();
00425       return;
00426     }
00427     if (use_pointcloud_model_ && !pointcloud_model_) {
00428       ROS_ERROR("No pointcloud model is yet available");
00429       as_.setPreempted();
00430       return;
00431     }
00432     // check frame_id sanity
00433     std::string goal_frame_id = goal->initial_footstep.header.frame_id;
00434     if (use_pointcloud_model_) {
00435       // check perception cloud header
00436       if (goal_frame_id != pointcloud_model_frame_id_) {
00437         ROS_ERROR("frame_id of goal and pointcloud do not match. goal: %s, pointcloud: %s.",
00438                       goal_frame_id.c_str(), pointcloud_model_frame_id_.c_str());
00439         as_.setPreempted();
00440         return;
00441       }
00442     }
00443     if (use_obstacle_model_) {
00444       // check perception cloud header
00445       if (goal_frame_id != obstacle_model_frame_id_) {
00446         ROS_ERROR("frame_id of goal and obstacle pointcloud do not match. goal: %s, obstacle: %s.",
00447                       goal_frame_id.c_str(), obstacle_model_frame_id_.c_str());
00448         as_.setPreempted();
00449         return;
00450       }
00451     }
00452     Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
00453     Eigen::Vector3f search_resolution(resolution_x_, resolution_y_, resolution_theta_);
00454     // check goal is whether collision free
00455     // conevrt goal footstep into FootstepState::Ptr instance.
00456     if (goal->goal_footstep.footsteps.size() != 2) {
00457       ROS_ERROR("goal footstep should be a pair of footsteps");
00458       as_.setPreempted();
00459       return;
00460     }
00461     std::vector<jsk_footstep_msgs::Footstep > goal_ros;
00462     goal_ros.push_back(goal->goal_footstep.footsteps[0]);
00463     goal_ros.push_back(goal->goal_footstep.footsteps[1]);
00464     for (int i = 0; i < 2; i++) {
00465       if (goal_ros[i].offset.x == 0.0 &&
00466           goal_ros[i].offset.y == 0.0 &&
00467           goal_ros[i].offset.z == 0.0 ) {
00468         if (goal_ros[i].leg == jsk_footstep_msgs::Footstep::LEFT) {
00469           goal_ros[i].offset.x = - inv_lleg_footstep_offset_[0];
00470           goal_ros[i].offset.y = - inv_lleg_footstep_offset_[1];
00471           goal_ros[i].offset.z = - inv_lleg_footstep_offset_[2];
00472         } else {
00473           goal_ros[i].offset.x = - inv_rleg_footstep_offset_[0];
00474           goal_ros[i].offset.y = - inv_rleg_footstep_offset_[1];
00475           goal_ros[i].offset.z = - inv_rleg_footstep_offset_[2];
00476         }
00477       }
00478       if(goal_ros[i].dimensions.x == 0 &&
00479          goal_ros[i].dimensions.y == 0 &&
00480          goal_ros[i].dimensions.z == 0 ) {
00481         goal_ros[i].dimensions.x = footstep_size_x_;
00482         goal_ros[i].dimensions.y = footstep_size_y_;
00483         goal_ros[i].dimensions.z = 0.000001;
00484       }
00485     }
00486 
00487     FootstepState::Ptr first_goal = FootstepState::fromROSMsg(goal_ros[0],
00488                                                               footstep_size,
00489                                                               search_resolution);
00490     FootstepState::Ptr second_goal = FootstepState::fromROSMsg(goal_ros[1],
00491                                                                footstep_size,
00492                                                                search_resolution);
00493     if (!graph_->isSuccessable(second_goal, first_goal)) {
00494       ROS_ERROR("goal is non-realistic");
00495       as_.setPreempted();
00496       return;
00497     }
00498     ros::WallDuration timeout;
00499     if(goal->timeout.toSec() == 0.0) {
00500       timeout = ros::WallDuration(planning_timeout_);
00501     } else {
00502       timeout = ros::WallDuration(goal->timeout.toSec());
00503     }
00504 
00505 
00507     // set start state
00508     // 0 is always start
00509     jsk_footstep_msgs::Footstep start_ros = goal->initial_footstep.footsteps[0];
00510     if (start_ros.offset.x == 0.0 &&
00511         start_ros.offset.y == 0.0 &&
00512         start_ros.offset.z == 0.0 ) {
00513       if (start_ros.leg == jsk_footstep_msgs::Footstep::LEFT) {
00514         start_ros.offset.x = - inv_lleg_footstep_offset_[0];
00515         start_ros.offset.y = - inv_lleg_footstep_offset_[1];
00516         start_ros.offset.z = - inv_lleg_footstep_offset_[2];
00517       } else {
00518         start_ros.offset.x = - inv_rleg_footstep_offset_[0];
00519         start_ros.offset.y = - inv_rleg_footstep_offset_[1];
00520         start_ros.offset.z = - inv_rleg_footstep_offset_[2];
00521       }
00522     }
00523     FootstepState::Ptr start(FootstepState::fromROSMsg(
00524                                start_ros,
00525                                footstep_size,
00526                                search_resolution));
00527     graph_->setStartState(start);
00528     if (project_start_state_) {
00529       if (!graph_->projectStart()) {
00530         ROS_ERROR("Failed to project start state");
00531         publishText(pub_text_,
00532                     "Failed to project start",
00533                     ERROR);
00534 
00535         as_.setPreempted();
00536         return;
00537       }
00538     }
00539 
00541     // set goal state
00542     jsk_footstep_msgs::Footstep left_goal, right_goal;
00543     for (size_t i = 0; i < goal_ros.size(); i++) {
00544       FootstepState::Ptr goal_state(FootstepState::fromROSMsg(
00545                                       goal_ros[i],
00546                                       footstep_size,
00547                                       Eigen::Vector3f(resolution_x_,
00548                                                       resolution_y_,
00549                                                       resolution_theta_)));
00550       if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00551         graph_->setLeftGoalState(goal_state);
00552         left_goal = goal_ros[i];
00553       }
00554       else if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
00555         graph_->setRightGoalState(goal_state);
00556         right_goal = goal_ros[i];
00557       }
00558       else {
00559         ROS_ERROR("unknown goal leg");
00560         as_.setPreempted();
00561         return;
00562       }
00563     }
00564     if (project_goal_state_) {
00565       if (!graph_->projectGoal()) {
00566         ROS_ERROR("Failed to project goal");
00567         as_.setPreempted();
00568         publishText(pub_text_,
00569                     "Failed to project goal",
00570                     ERROR);
00571         return;
00572       }
00573     }
00574     // set parameters
00575     if (parameters_.use_transition_limit) {
00576       graph_->setTransitionLimit(
00577         TransitionLimitXYZRPY::Ptr(new TransitionLimitXYZRPY(
00578                                      parameters_.transition_limit_x,
00579                                      parameters_.transition_limit_y,
00580                                      parameters_.transition_limit_z,
00581                                      parameters_.transition_limit_roll,
00582                                      parameters_.transition_limit_pitch,
00583                                      parameters_.transition_limit_yaw)));
00584     }
00585     else {
00586       graph_->setTransitionLimit(TransitionLimitXYZRPY::Ptr());
00587     }
00588     if (use_obstacle_model_) {
00589       graph_->setCollisionBBoxSize(collision_bbox_size_);
00590       graph_->setCollisionBBoxOffset(collision_bbox_offset_);
00591     }
00592     if (parameters_.use_global_transition_limit) {
00593       graph_->setGlobalTransitionLimit(
00594         TransitionLimitRP::Ptr(new TransitionLimitRP(
00595                                      parameters_.global_transition_limit_roll,
00596                                      parameters_.global_transition_limit_pitch)));
00597 
00598     }
00599     else {
00600       graph_->setGlobalTransitionLimit(TransitionLimitRP::Ptr());
00601     }
00602     graph_->setParameters(parameters_);
00603 
00604     graph_->setSuccessorFunction(boost::bind(&FootstepGraph::successors_original, graph_, _1, _2));
00605     graph_->setPathCostFunction(boost::bind(&FootstepGraph::path_cost_original, graph_, _1, _2, _3));
00606 
00607     //ROS_INFO_STREAM(graph_->infoString());
00608     // Solver setup
00609     FootstepAStarSolver<FootstepGraph> solver(graph_,
00610                                               close_list_x_num_,
00611                                               close_list_y_num_,
00612                                               close_list_theta_num_,
00613                                               profile_period_,
00614                                               cost_weight_,
00615                                               heuristic_weight_);
00616     if (heuristic_ == "step_cost") {
00617       solver.setHeuristic(boost::bind(&FootstepPlanner::stepCostHeuristic, this, _1, _2));
00618     }
00619     else if (heuristic_ == "zero") {
00620       solver.setHeuristic(boost::bind(&FootstepPlanner::zeroHeuristic, this, _1, _2));
00621     }
00622     else if (heuristic_ == "straight") {
00623       solver.setHeuristic(boost::bind(&FootstepPlanner::straightHeuristic, this, _1, _2));
00624     }
00625     else if (heuristic_ == "straight_rotation") {
00626       solver.setHeuristic(boost::bind(&FootstepPlanner::straightRotationHeuristic, this, _1, _2));
00627     }
00628     else if (heuristic_ == "follow_path") {
00629       solver.setHeuristic(boost::bind(&FootstepPlanner::followPathLineHeuristic, this, _1, _2));
00630     }
00631     else {
00632       ROS_ERROR("Unknown heuristics");
00633       as_.setPreempted();
00634       return;
00635     }
00636     graph_->clearPerceptionDuration();
00637     solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
00638     ROS_INFO("start_solver timeout: %f", timeout.toSec());
00639     ros::WallTime start_time = ros::WallTime::now();
00640     std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
00641     ros::WallTime end_time = ros::WallTime::now();
00642     double planning_duration = (end_time - start_time).toSec();
00643     //ROS_INFO_STREAM("took " << planning_duration << " sec");
00644     //ROS_INFO_STREAM("path: " << path.size());
00645     if (path.size() == 0) {
00646       pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
00647       solver.openListToPointCloud(open_list_cloud);
00648       solver.closeListToPointCloud(close_list_cloud);
00649       std::string info_str
00650         = (boost::format("Failed to plan path / Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
00651            % planning_duration
00652            % graph_->getPerceptionDuration().toSec()
00653            % (planning_duration - graph_->getPerceptionDuration().toSec())
00654            % path.size()
00655            % open_list_cloud.points.size()
00656            % close_list_cloud.points.size()).str();
00657       ROS_ERROR_STREAM(info_str);
00658       // pub open/close list
00659       publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
00660       publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
00661       // pub text
00662       publishText(pub_text_,
00663                   info_str,
00664                   ERROR);
00665       //
00666       as_.setPreempted();
00667       return;
00668     }
00669     // finalize in graph
00670     std::vector <FootstepState::Ptr> finalizeSteps;
00671     if (! (graph_->finalizeSteps((path.size() >1 ? path[path.size()-2]->getState() : FootstepState::Ptr()),
00672                                  path[path.size()-1]->getState(),
00673                                  finalizeSteps))) {
00674       ROS_ERROR("Failed to finalize path");
00675       publishText(pub_text_,
00676                   "Failed to finalize path",
00677                   ERROR);
00678       as_.setPreempted();
00679       return;
00680     }
00681     // Convert path to FootstepArray
00682     jsk_footstep_msgs::FootstepArray ros_path;
00683     ros_path.header = goal->goal_footstep.header;
00684     for (size_t i = 0; i < path.size(); i++) {
00685       const FootstepState::Ptr st = path[i]->getState();
00686       if (st->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00687         ros_path.footsteps.push_back(*(st->toROSMsg(inv_lleg_footstep_offset_)));
00688       } else {
00689         ros_path.footsteps.push_back(*(st->toROSMsg(inv_rleg_footstep_offset_)));
00690       }
00691     }
00692     for (size_t i = 0; i < finalizeSteps.size(); i++) {
00693       const FootstepState::Ptr st = finalizeSteps[i];
00694       if (st->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00695         ros_path.footsteps.push_back(*(st->toROSMsg(inv_lleg_footstep_offset_)));
00696       } else {
00697         ros_path.footsteps.push_back(*(st->toROSMsg(inv_rleg_footstep_offset_)));
00698       }
00699     }
00700     result_.result = ros_path;
00701     as_.setSucceeded(result_);
00702 
00703     pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
00704     solver.openListToPointCloud(open_list_cloud);
00705     solver.closeListToPointCloud(close_list_cloud);
00706     publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
00707     publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
00708     std::string info_str
00709       = (boost::format("Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
00710          % planning_duration
00711          % graph_->getPerceptionDuration().toSec()
00712          % (planning_duration - graph_->getPerceptionDuration().toSec())
00713          % path.size()
00714          % open_list_cloud.points.size()
00715          % close_list_cloud.points.size()).str();
00716     ROS_INFO_STREAM(info_str);
00717     publishText(pub_text_, info_str, OK);
00718     ROS_INFO_STREAM("use_obstacle_model: " << graph_->useObstacleModel());
00719   }
00720 
00721   void FootstepPlanner::publishPointCloud(
00722     const pcl::PointCloud<pcl::PointNormal>& cloud,
00723     ros::Publisher& pub,
00724     const std_msgs::Header& header)
00725   {
00726     sensor_msgs::PointCloud2 ros_cloud;
00727     pcl::toROSMsg(cloud, ros_cloud);
00728     ros_cloud.header = header;
00729     pub.publish(ros_cloud);
00730   }
00731 
00732   void FootstepPlanner::profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
00733   {
00734     if (as_.isPreemptRequested()) {
00735       solver.cancelSolve();
00736       ROS_WARN("cancelled!");
00737     }
00738     // ROS_INFO("open list: %lu", solver.getOpenList().size());
00739     // ROS_INFO("close list: %lu", solver.getCloseList().size());
00740     publishText(pub_text_,
00741                 (boost::format("open_list: %lu\nclose list:%lu")
00742                  % (solver.getOpenList().size()) % (solver.getCloseList().size())).str(),
00743                 OK);
00744     if (rich_profiling_) {
00745       pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
00746       solver.openListToPointCloud(open_list_cloud);
00747       solver.closeListToPointCloud(close_list_cloud);
00748       publishPointCloud(close_list_cloud, pub_close_list_, latest_header_);
00749       publishPointCloud(open_list_cloud, pub_open_list_, latest_header_);
00750     }
00751   }
00752   
00753   double FootstepPlanner::stepCostHeuristic(
00754     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00755   {
00756     return footstepHeuristicStepCost(node, graph, heuristic_first_rotation_weight_,
00757                                      heuristic_second_rotation_weight_);
00758   }
00759 
00760   double FootstepPlanner::zeroHeuristic(
00761     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00762   {
00763     return footstepHeuristicZero(node, graph);
00764   }
00765 
00766   double FootstepPlanner::straightHeuristic(
00767     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00768   {
00769     return footstepHeuristicStraight(node, graph);
00770   }
00771 
00772   double FootstepPlanner::straightRotationHeuristic(
00773     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00774   {
00775     return footstepHeuristicStraightRotation(node, graph);
00776   }
00777 
00778   double FootstepPlanner::followPathLineHeuristic(
00779     SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00780   {
00781     return footstepHeuristicFollowPathLine(node, graph);
00782   }
00783 
00795   bool FootstepPlanner::readSuccessors(ros::NodeHandle& nh)
00796   {
00797     successors_.clear();
00798     if (!nh.hasParam("successors")) {
00799       ROS_FATAL("no successors are specified");
00800       return false;
00801     }
00802     // read default translation from right foot to left foot
00803     double default_x   = 0.0;
00804     double default_y   = 0.0;
00805     double default_theta = 0.0;
00806     if (nh.hasParam("default_lfoot_to_rfoot_offset")) {
00807       std::vector<double> default_offset;
00808       if (jsk_topic_tools::readVectorParameter(nh, "default_lfoot_to_rfoot_offset", default_offset)) {
00809         default_x =     default_offset[0];
00810         default_y =     default_offset[1];
00811         default_theta = default_offset[2];
00812       }
00813     }
00814     // read successors
00815     XmlRpc::XmlRpcValue successors_xml;
00816     nh.param("successors", successors_xml, successors_xml);
00817     if (successors_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
00818     {
00819       ROS_FATAL("successors should be an array");
00820       return false;
00821     }
00822     for (size_t i_successors = 0; i_successors < successors_xml.size(); i_successors++) {
00823       XmlRpc::XmlRpcValue successor_xml;
00824       successor_xml = successors_xml[i_successors];
00825       if (successor_xml.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00826         ROS_FATAL("element of successors should be an dictionary");
00827         return false;
00828       }
00829       double x = 0;
00830       double y = 0;
00831       double theta = 0;
00832       if (successor_xml.hasMember("x")) {
00833         x = jsk_topic_tools::getXMLDoubleValue(successor_xml["x"]);
00834         x += default_x;
00835       }
00836       if (successor_xml.hasMember("y")) {
00837         y = jsk_topic_tools::getXMLDoubleValue(successor_xml["y"]);
00838         y += default_y;
00839       }
00840       if (successor_xml.hasMember("theta")) {
00841         theta = jsk_topic_tools::getXMLDoubleValue(successor_xml["theta"]);
00842         theta += default_theta;
00843       }
00844       // successors written in parameters should not include offset
00845       // successors using in graph include offset(between the center of the cube and the end-coords)
00846       // Footstep returned to a client, its pose indicates end-coords
00847       Eigen::Affine3f successor =
00848         Eigen::Translation3f(inv_lleg_footstep_offset_[0],
00849                              inv_lleg_footstep_offset_[1],
00850                              inv_lleg_footstep_offset_[2]) *
00851         affineFromXYYaw(x, y, theta) *
00852         Eigen::Translation3f(-inv_rleg_footstep_offset_[0],
00853                              -inv_rleg_footstep_offset_[1],
00854                              -inv_rleg_footstep_offset_[2]);
00855       successors_.push_back(successor);
00856     }
00857     ROS_INFO("%lu successors are defined", successors_.size());
00858     if ((default_x != 0.0) || (default_y != 0.0) || (default_theta != 0.0)) {
00859       ROS_INFO("default_offset: #f(%f %f %f)", default_x, default_y, default_theta);
00860     }
00861     if ((inv_lleg_footstep_offset_[0] != 0) ||
00862         (inv_lleg_footstep_offset_[1] != 0) ||
00863         (inv_lleg_footstep_offset_[2] != 0) ) {
00864       ROS_INFO("left_leg_offset: #f(%f %f %f)",
00865                - inv_lleg_footstep_offset_[0],
00866                - inv_lleg_footstep_offset_[1],
00867                - inv_lleg_footstep_offset_[2]);
00868     }
00869     if ((inv_rleg_footstep_offset_[0] != 0) ||
00870         (inv_rleg_footstep_offset_[1] != 0) ||
00871         (inv_rleg_footstep_offset_[2] != 0) ) {
00872       ROS_INFO("right_leg_offset: #f(%f %f %f)",
00873                - inv_rleg_footstep_offset_[0],
00874                - inv_rleg_footstep_offset_[1],
00875                - inv_rleg_footstep_offset_[2]);
00876     }
00877     for (size_t i = 0; i < successors_.size(); i++) {
00878       Eigen::Vector3f tr = successors_[i].translation();
00879       float roll, pitch, yaw;
00880       pcl::getEulerAngles(successors_[i], roll, pitch, yaw);
00881       ROS_INFO("successor_%2.2d: (make-coords :pos (scale 1000 #f(%f %f 0)) :rpy (list %f 0 0))", i, tr[0], tr[1], yaw);
00882     }
00883     return true;
00884   }
00885 
00886   void FootstepPlanner::configCallback(Config &config, uint32_t level)
00887   {
00888     boost::mutex::scoped_lock lock(mutex_);
00889     bool need_to_rebuild_graph = false;
00890     if (use_pointcloud_model_ != config.use_pointcloud_model) {
00891       use_pointcloud_model_ = config.use_pointcloud_model;
00892       need_to_rebuild_graph = true;
00893     }
00894     if (use_lazy_perception_ != config.use_lazy_perception) {
00895       use_lazy_perception_ = config.use_lazy_perception;
00896       need_to_rebuild_graph = true;
00897     }
00898     if (use_local_movement_ != config.use_local_movement) {
00899       use_local_movement_ = config.use_local_movement;
00900       need_to_rebuild_graph = true;
00901     }
00902     if (resolution_x_ != config.resolution_x) {
00903       resolution_x_ = config.resolution_x;
00904       need_to_rebuild_graph = true;
00905     }
00906     if (resolution_y_ != config.resolution_y) {
00907       resolution_y_ = config.resolution_y;
00908       need_to_rebuild_graph = true;
00909     }
00910     if (resolution_theta_ != config.resolution_theta) {
00911       resolution_theta_ = config.resolution_theta;
00912       need_to_rebuild_graph = true;
00913     }
00914     planning_timeout_ = config.planning_timeout;
00915     rich_profiling_ = config.rich_profiling;
00916     parameters_.use_transition_limit = config.use_transition_limit;
00917     parameters_.use_global_transition_limit = config.use_global_transition_limit;
00918     parameters_.local_move_x = config.local_move_x;
00919     parameters_.local_move_y = config.local_move_y;
00920     parameters_.local_move_theta = config.local_move_theta;
00921     parameters_.local_move_x_num = config.local_move_x_num;
00922     parameters_.local_move_y_num = config.local_move_y_num;
00923     parameters_.local_move_theta_num = config.local_move_theta_num;
00924     parameters_.local_move_x_offset = config.local_move_x_offset;
00925     parameters_.local_move_y_offset = config.local_move_y_offset;
00926     parameters_.local_move_theta_offset = config.local_move_theta_offset;
00927     parameters_.transition_limit_x = config.transition_limit_x;
00928     parameters_.transition_limit_y = config.transition_limit_y;
00929     parameters_.transition_limit_z = config.transition_limit_z;
00930     parameters_.transition_limit_roll = config.transition_limit_roll;
00931     parameters_.transition_limit_pitch = config.transition_limit_pitch;
00932     parameters_.transition_limit_yaw = config.transition_limit_yaw;
00933     parameters_.global_transition_limit_roll = config.global_transition_limit_roll;
00934     parameters_.global_transition_limit_pitch = config.global_transition_limit_pitch;
00935     parameters_.goal_pos_thr = config.goal_pos_thr;
00936     parameters_.goal_rot_thr = config.goal_rot_thr;
00937     parameters_.plane_estimation_use_normal              = config.plane_estimation_use_normal;
00938     parameters_.plane_estimation_normal_distance_weight  = config.plane_estimation_normal_distance_weight;
00939     parameters_.plane_estimation_normal_opening_angle    = config.plane_estimation_normal_opening_angle;
00940     parameters_.plane_estimation_min_ratio_of_inliers    = config.plane_estimation_min_ratio_of_inliers;
00941     parameters_.plane_estimation_max_iterations = config.plane_estimation_max_iterations;
00942     parameters_.plane_estimation_min_inliers = config.plane_estimation_min_inliers;
00943     parameters_.plane_estimation_outlier_threshold = config.plane_estimation_outlier_threshold;
00944     parameters_.support_check_x_sampling = config.support_check_x_sampling;
00945     parameters_.support_check_y_sampling = config.support_check_y_sampling;
00946     parameters_.support_check_vertex_neighbor_threshold = config.support_check_vertex_neighbor_threshold;
00947     parameters_.support_padding_x = config.support_padding_x;
00948     parameters_.support_padding_y = config.support_padding_y;
00949     parameters_.skip_cropping = config.skip_cropping;
00950     footstep_size_x_ = config.footstep_size_x;
00951     footstep_size_y_ = config.footstep_size_y;
00952     project_start_state_ = config.project_start_state;
00953     project_goal_state_ = config.project_goal_state;
00954     close_list_x_num_ = config.close_list_x_num;
00955     close_list_y_num_ = config.close_list_y_num;
00956     close_list_theta_num_ = config.close_list_theta_num;
00957     profile_period_ = config.profile_period;
00958     heuristic_ = config.heuristic;
00959     heuristic_first_rotation_weight_ = config.heuristic_first_rotation_weight;
00960     heuristic_second_rotation_weight_ = config.heuristic_second_rotation_weight;
00961     cost_weight_ = config.cost_weight;
00962     heuristic_weight_ = config.heuristic_weight;
00963     if (use_obstacle_model_ != config.use_obstacle_model) {
00964       use_obstacle_model_ = config.use_obstacle_model;
00965       need_to_rebuild_graph = true;
00966     }
00967     parameters_.obstacle_resolution = config.obstacle_resolution;
00968     if (need_to_rebuild_graph) {
00969       if (graph_) {             // In order to skip first initialization
00970         ROS_INFO("re-building graph");
00971         buildGraph();
00972       }
00973     }
00974   }
00975   
00976   void FootstepPlanner::buildGraph()
00977   {
00978     graph_.reset(new FootstepGraph(Eigen::Vector3f(resolution_x_,
00979                                                    resolution_y_,
00980                                                    resolution_theta_),
00981                                    use_pointcloud_model_,
00982                                    use_lazy_perception_,
00983                                    use_local_movement_,
00984                                    use_obstacle_model_));
00985     if (use_pointcloud_model_ && pointcloud_model_) {
00986       graph_->setPointCloudModel(pointcloud_model_);
00987     }
00988     if (use_obstacle_model_ && obstacle_model_) {
00989       graph_->setObstacleModel(obstacle_model_);
00990     }
00991     //graph_->setObstacleResolution(parameters_.obstacle_resolution);
00992     graph_->setParameters(parameters_);
00993     graph_->setBasicSuccessors(successors_);
00994   }
00995 
00996   void FootstepPlanner::setHeuristicPathLine(jsk_recognition_utils::PolyLine &path_line)
00997   {
00998     graph_->setHeuristicPathLine(path_line); // copy ???
00999   }
01000 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:28