00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
00258
00259
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);
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
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
00433 std::string goal_frame_id = goal->initial_footstep.header.frame_id;
00434 if (use_pointcloud_model_) {
00435
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
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
00455
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
00508
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
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
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
00608
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
00644
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
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
00662 publishText(pub_text_,
00663 info_str,
00664 ERROR);
00665
00666 as_.setPreempted();
00667 return;
00668 }
00669
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
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
00739
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
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
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
00845
00846
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_) {
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
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);
00999 }
01000 }