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 {
00064 boost::mutex::scoped_lock lock(mutex_);
00065 if (!readSuccessors(nh)) {
00066 return;
00067 }
00068
00069 JSK_ROS_INFO("building graph");
00070 buildGraph();
00071 JSK_ROS_INFO("build graph done");
00072 }
00073 sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
00074 as_.start();
00075 }
00076
00077 void FootstepPlanner::pointcloudCallback(
00078 const sensor_msgs::PointCloud2::ConstPtr& msg)
00079 {
00080 boost::mutex::scoped_lock lock(mutex_);
00081 JSK_ROS_INFO("pointcloud model is updated");
00082 pointcloud_model_.reset(new pcl::PointCloud<pcl::PointNormal>);
00083 pcl::fromROSMsg(*msg, *pointcloud_model_);
00084 if (graph_ && use_pointcloud_model_) {
00085 graph_->setPointCloudModel(pointcloud_model_);
00086 }
00087 }
00088
00089 bool FootstepPlanner::projectFootPrint(
00090 const Eigen::Affine3f& center_pose,
00091 const Eigen::Affine3f& left_pose_trans,
00092 const Eigen::Affine3f& right_pose_trans,
00093 geometry_msgs::Pose& pose)
00094 {
00095 const Eigen::Vector3f resolution(resolution_x_,
00096 resolution_y_,
00097 resolution_theta_);
00098 const Eigen::Vector3f footstep_size(footstep_size_x_,
00099 footstep_size_y_,
00100 0.000001);
00101 Eigen::Affine3f left_pose = center_pose * left_pose_trans;
00102 Eigen::Affine3f right_pose = center_pose * right_pose_trans;
00103 FootstepState::Ptr left_input(new FootstepState(
00104 jsk_footstep_msgs::Footstep::LEFT,
00105 left_pose,
00106 footstep_size,
00107 resolution));
00108 FootstepState::Ptr right_input(new FootstepState(
00109 jsk_footstep_msgs::Footstep::RIGHT,
00110 right_pose,
00111 footstep_size,
00112 resolution));
00113 FootstepState::Ptr projected_left = graph_->projectFootstep(left_input);
00114 FootstepState::Ptr projected_right = graph_->projectFootstep(right_input);
00115 if (!projected_left || !projected_right) {
00116 return false;
00117 }
00118 Eigen::Affine3f projected_left_pose = projected_left->getPose();
00119 Eigen::Affine3f projected_right_pose = projected_right->getPose();
00120 Eigen::Quaternionf rot = Eigen::Quaternionf(projected_left_pose.rotation()).slerp(
00121 0.5, Eigen::Quaternionf(projected_right_pose.rotation()));
00122 Eigen::Vector3f pos = (Eigen::Vector3f(projected_right_pose.translation()) +
00123 Eigen::Vector3f(projected_left_pose.translation())) / 2.0;
00124 Eigen::Affine3f mid = Eigen::Translation3f(pos) * rot;
00125 tf::poseEigenToMsg(mid, pose);
00126 return true;
00127 }
00128
00129 bool FootstepPlanner::projectFootPrintWithLocalSearchService(
00130 jsk_interactive_marker::SnapFootPrint::Request& req,
00131 jsk_interactive_marker::SnapFootPrint::Response& res)
00132 {
00133 boost::mutex::scoped_lock lock(mutex_);
00134 if (!graph_ ) {
00135 return false;
00136 }
00137 if (!pointcloud_model_) {
00138 JSK_ROS_ERROR("No pointcloud model is yet available");
00139 publishText(pub_text_,
00140 "No pointcloud model is yet available",
00141 ERROR);
00142 return false;
00143 }
00144 Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
00145 std::vector<Eigen::Affine3f> center_poses;
00146 tf::poseMsgToEigen(req.lleg_pose, left_pose_trans);
00147 tf::poseMsgToEigen(req.rleg_pose, right_pose_trans);
00148 tf::poseMsgToEigen(req.input_pose.pose, center_pose);
00149 const double dx = 0.05;
00150 const double dy = 0.05;
00151 const double dtheta = pcl::deg2rad(5.0);
00152 for (int xi = 0; xi < 3; xi++) {
00153 for (int yi = 0; yi < 3; yi++) {
00154 for (int thetai = 0; thetai < 3; thetai++) {
00155 Eigen::Affine3f transppp = affineFromXYYaw(xi * dx, yi * dy, thetai * dtheta);
00156 Eigen::Affine3f transppm = affineFromXYYaw(xi * dx, yi * dy, - thetai * dtheta);
00157 Eigen::Affine3f transpmp = affineFromXYYaw(xi * dx, - yi * dy, thetai * dtheta);
00158 Eigen::Affine3f transpmm = affineFromXYYaw(xi * dx, - yi * dy, -thetai * dtheta);
00159 Eigen::Affine3f transmpp = affineFromXYYaw(- xi * dx, yi * dy, thetai * dtheta);
00160 Eigen::Affine3f transmpm = affineFromXYYaw(- xi * dx, yi * dy, - thetai * dtheta);
00161 Eigen::Affine3f transmmp = affineFromXYYaw(- xi * dx, - yi * dy, thetai * dtheta);
00162 Eigen::Affine3f transmmm = affineFromXYYaw(- xi * dx, - yi * dy, - thetai * dtheta);
00163 center_poses.push_back(center_pose * transppp);
00164 center_poses.push_back(center_pose * transppm);
00165 center_poses.push_back(center_pose * transpmp);
00166 center_poses.push_back(center_pose * transpmm);
00167 center_poses.push_back(center_pose * transmpp);
00168 center_poses.push_back(center_pose * transmpm);
00169 center_poses.push_back(center_pose * transmmp);
00170 center_poses.push_back(center_pose * transmmm);
00171 }
00172 }
00173 }
00174 for (size_t i = 0; i < center_poses.size(); i++) {
00175 if (projectFootPrint(center_poses[i], left_pose_trans, right_pose_trans,
00176 res.snapped_pose.pose)) {
00177 res.success = true;
00178 res.snapped_pose.header = req.input_pose.header;
00179 return true;
00180 }
00181 }
00182 JSK_ROS_ERROR("Failed to project footprint");
00183 publishText(pub_text_,
00184 "Failed to project goal",
00185 ERROR);
00186 return false;
00187 }
00188
00189 bool FootstepPlanner::projectFootPrintService(
00190 jsk_interactive_marker::SnapFootPrint::Request& req,
00191 jsk_interactive_marker::SnapFootPrint::Response& res)
00192 {
00193 boost::mutex::scoped_lock lock(mutex_);
00194 if (!graph_) {
00195 return false;
00196 }
00197 if (!pointcloud_model_) {
00198 JSK_ROS_ERROR("No pointcloud model is yet available");
00199 publishText(pub_text_,
00200 "No pointcloud model is yet available",
00201 ERROR);
00202 return false;
00203 }
00204 Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
00205 tf::poseMsgToEigen(req.lleg_pose, left_pose_trans);
00206 tf::poseMsgToEigen(req.rleg_pose, right_pose_trans);
00207 tf::poseMsgToEigen(req.input_pose.pose, center_pose);
00208 if (projectFootPrint(center_pose, left_pose_trans, right_pose_trans,
00209 res.snapped_pose.pose)) {
00210 res.success = true;
00211 res.snapped_pose.header = req.input_pose.header;
00212 return true;
00213 }
00214 else {
00215 JSK_ROS_ERROR("Failed to project footprint");
00216 publishText(pub_text_,
00217 "Failed to project goal",
00218 ERROR);
00219 return false;
00220 }
00221 }
00222
00223 void FootstepPlanner::publishText(ros::Publisher& pub,
00224 const std::string& text,
00225 PlanningStatus status)
00226 {
00227 std_msgs::ColorRGBA ok_color;
00228 ok_color.r = 0.3568627450980392;
00229 ok_color.g = 0.7529411764705882;
00230 ok_color.b = 0.8705882352941177;
00231 ok_color.a = 1.0;
00232 std_msgs::ColorRGBA warn_color;
00233 warn_color.r = 0.9411764705882353;
00234 warn_color.g = 0.6784313725490196;
00235 warn_color.b = 0.3058823529411765;
00236 warn_color.a = 1.0;
00237 std_msgs::ColorRGBA error_color;
00238 error_color.r = 0.8509803921568627;
00239 error_color.g = 0.3254901960784314;
00240 error_color.b = 0.30980392156862746;
00241 error_color.a = 1.0;
00242 std_msgs::ColorRGBA color;
00243 if (status == OK) {
00244 color = ok_color;
00245 }
00246 else if (status == WARNING) {
00247 color = warn_color;
00248 }
00249 else if (status == ERROR) {
00250 color = error_color;
00251 }
00252 jsk_rviz_plugins::OverlayText msg;
00253 msg.text = text;
00254 msg.width = 1000;
00255 msg.height = 1000;
00256 msg.top = 10;
00257 msg.left = 10;
00258 msg.bg_color.a = 0.0;
00259 msg.fg_color = color;
00260 msg.text_size = 24;
00261 pub.publish(msg);
00262 }
00263
00264 void FootstepPlanner::planCB(
00265 const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
00266 {
00267 boost::mutex::scoped_lock lock(mutex_);
00268 latest_header_ = goal->goal_footstep.header;
00269 JSK_ROS_INFO("planCB");
00270
00271 if (goal->initial_footstep.footsteps.size() == 0) {
00272 JSK_ROS_ERROR("no initial footstep is specified");
00273 as_.setPreempted();
00274 return;
00275 }
00276 if (goal->goal_footstep.footsteps.size() != 2) {
00277 JSK_ROS_ERROR("Need to specify 2 goal footsteps");
00278 as_.setPreempted();
00279 return;
00280 }
00281 if (use_pointcloud_model_ && !pointcloud_model_) {
00282 JSK_ROS_ERROR("No pointcloud model is yet available");
00283 as_.setPreempted();
00284 return;
00285 }
00286
00287 ros::WallDuration timeout(10.0);
00288 Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
00290
00291
00292 Eigen::Affine3f start_pose;
00293 tf::poseMsgToEigen(goal->initial_footstep.footsteps[0].pose, start_pose);
00294 FootstepState::Ptr start(FootstepState::fromROSMsg(
00295 goal->initial_footstep.footsteps[0],
00296 footstep_size,
00297 Eigen::Vector3f(resolution_x_,
00298 resolution_y_,
00299 resolution_theta_)));
00300 graph_->setStartState(start);
00301 if (project_start_state_) {
00302 if (!graph_->projectStart()) {
00303 JSK_ROS_ERROR("Failed to project start state");
00304 publishText(pub_text_,
00305 "Failed to project start",
00306 ERROR);
00307
00308 as_.setPreempted();
00309 return;
00310 }
00311 }
00312
00314
00315 jsk_footstep_msgs::Footstep left_goal, right_goal;
00316 for (size_t i = 0; i < goal->goal_footstep.footsteps.size(); i++) {
00317 FootstepState::Ptr goal_state(FootstepState::fromROSMsg(
00318 goal->goal_footstep.footsteps[i],
00319 footstep_size,
00320 Eigen::Vector3f(resolution_x_,
00321 resolution_y_,
00322 resolution_theta_)));
00323 if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00324 graph_->setLeftGoalState(goal_state);
00325 left_goal = goal->goal_footstep.footsteps[i];
00326 }
00327 else if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
00328 graph_->setRightGoalState(goal_state);
00329 right_goal = goal->goal_footstep.footsteps[i];
00330 }
00331 else {
00332 JSK_ROS_ERROR("unknown goal leg");
00333 as_.setPreempted();
00334 return;
00335 }
00336 }
00337 if (project_goal_state_) {
00338 if (!graph_->projectGoal()) {
00339 JSK_ROS_ERROR("Failed to project goal");
00340 as_.setPreempted();
00341 publishText(pub_text_,
00342 "Failed to project goal",
00343 ERROR);
00344 return;
00345 }
00346 }
00347
00348 if (use_transition_limit_) {
00349 graph_->setTransitionLimit(
00350 TransitionLimitXYZRPY::Ptr(new TransitionLimitXYZRPY(
00351 transition_limit_x_,
00352 transition_limit_y_,
00353 transition_limit_z_,
00354 transition_limit_roll_,
00355 transition_limit_pitch_,
00356 transition_limit_yaw_)));
00357 }
00358 else {
00359 graph_->setTransitionLimit(TransitionLimitXYZRPY::Ptr());
00360 }
00361 graph_->setLocalXMovement(local_move_x_);
00362 graph_->setLocalYMovement(local_move_y_);
00363 graph_->setLocalThetaMovement(local_move_theta_);
00364 graph_->setLocalXMovementNum(local_move_x_num_);
00365 graph_->setLocalYMovementNum(local_move_y_num_);
00366 graph_->setLocalThetaMovementNum(local_move_theta_num_);
00367 graph_->setPlaneEstimationMaxIterations(plane_estimation_max_iterations_);
00368 graph_->setPlaneEstimationMinInliers(plane_estimation_min_inliers_);
00369 graph_->setPlaneEstimationOutlierThreshold(plane_estimation_outlier_threshold_);
00370 graph_->setSupportCheckXSampling(support_check_x_sampling_);
00371 graph_->setSupportCheckYSampling(support_check_y_sampling_);
00372 graph_->setSupportCheckVertexNeighborThreshold(support_check_vertex_neighbor_threshold_);
00373
00374 FootstepAStarSolver<FootstepGraph> solver(graph_,
00375 close_list_x_num_,
00376 close_list_y_num_,
00377 close_list_theta_num_,
00378 profile_period_,
00379 cost_weight_,
00380 heuristic_weight_);
00381 if (heuristic_ == "step_cost") {
00382 solver.setHeuristic(boost::bind(&FootstepPlanner::stepCostHeuristic, this, _1, _2));
00383 }
00384 else if (heuristic_ == "zero") {
00385 solver.setHeuristic(boost::bind(&FootstepPlanner::zeroHeuristic, this, _1, _2));
00386 }
00387 else if (heuristic_ == "straight") {
00388 solver.setHeuristic(boost::bind(&FootstepPlanner::straightHeuristic, this, _1, _2));
00389 }
00390 else if (heuristic_ == "straight_rotation") {
00391 solver.setHeuristic(boost::bind(&FootstepPlanner::straightRotationHeuristic, this, _1, _2));
00392 }
00393 else {
00394 JSK_ROS_ERROR("Unknown heuristics");
00395 as_.setPreempted();
00396 return;
00397 }
00398 solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
00399 ros::WallTime start_time = ros::WallTime::now();
00400 std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
00401 ros::WallTime end_time = ros::WallTime::now();
00402 double planning_duration = (end_time - start_time).toSec();
00403 JSK_ROS_INFO_STREAM("took " << planning_duration << " sec");
00404 JSK_ROS_INFO_STREAM("path: " << path.size());
00405 if (path.size() == 0) {
00406 JSK_ROS_ERROR("Failed to plan path");
00407 publishText(pub_text_,
00408 "Failed to plan",
00409 ERROR);
00410 as_.setPreempted();
00411 return;
00412 }
00413
00414 jsk_footstep_msgs::FootstepArray ros_path;
00415 ros_path.header = goal->goal_footstep.header;
00416 for (size_t i = 0; i < path.size(); i++) {
00417 ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
00418 }
00419
00420 if (path[path.size() - 1]->getState()->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
00421 ros_path.footsteps.push_back(right_goal);
00422 ros_path.footsteps.push_back(left_goal);
00423 }
00424 else if (path[path.size() - 1]->getState()->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
00425 ros_path.footsteps.push_back(left_goal);
00426 ros_path.footsteps.push_back(right_goal);
00427 }
00428 result_.result = ros_path;
00429 as_.setSucceeded(result_);
00430
00431 pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
00432 solver.openListToPointCloud(open_list_cloud);
00433 solver.closeListToPointCloud(close_list_cloud);
00434 publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
00435 publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
00436 publishText(pub_text_,
00437 (boost::format("Planning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
00438 % planning_duration % path.size()
00439 % open_list_cloud.points.size()
00440 % close_list_cloud.points.size()).str(),
00441 OK);
00442 }
00443
00444 void FootstepPlanner::publishPointCloud(
00445 const pcl::PointCloud<pcl::PointNormal>& cloud,
00446 ros::Publisher& pub,
00447 const std_msgs::Header& header)
00448 {
00449 sensor_msgs::PointCloud2 ros_cloud;
00450 pcl::toROSMsg(cloud, ros_cloud);
00451 ros_cloud.header = header;
00452 pub.publish(ros_cloud);
00453 }
00454
00455 void FootstepPlanner::profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
00456 {
00457 JSK_ROS_INFO("open list: %lu", solver.getOpenList().size());
00458 JSK_ROS_INFO("close list: %lu", solver.getCloseList().size());
00459 publishText(pub_text_,
00460 (boost::format("open_list: %lu\nclose list:%lu")
00461 % (solver.getOpenList().size()) % (solver.getCloseList().size())).str(),
00462 OK);
00463 if (rich_profiling_) {
00464 pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
00465 solver.openListToPointCloud(open_list_cloud);
00466 solver.closeListToPointCloud(close_list_cloud);
00467 publishPointCloud(close_list_cloud, pub_close_list_, latest_header_);
00468 publishPointCloud(open_list_cloud, pub_open_list_, latest_header_);
00469 }
00470 }
00471
00472 double FootstepPlanner::stepCostHeuristic(
00473 SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00474 {
00475 return footstepHeuristicStepCost(node, graph, heuristic_first_rotation_weight_,
00476 heuristic_second_rotation_weight_);
00477 }
00478
00479 double FootstepPlanner::zeroHeuristic(
00480 SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00481 {
00482 return footstepHeuristicZero(node, graph);
00483 }
00484
00485 double FootstepPlanner::straightHeuristic(
00486 SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00487 {
00488 return footstepHeuristicStraight(node, graph);
00489 }
00490
00491 double FootstepPlanner::straightRotationHeuristic(
00492 SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
00493 {
00494 return footstepHeuristicStraightRotation(node, graph);
00495 }
00496
00508 bool FootstepPlanner::readSuccessors(ros::NodeHandle& nh)
00509 {
00510 successors_.clear();
00511 if (!nh.hasParam("successors")) {
00512 JSK_ROS_FATAL("no successors are specified");
00513 return false;
00514 }
00515
00516 XmlRpc::XmlRpcValue successors_xml;
00517 nh.param("successors", successors_xml, successors_xml);
00518 if (successors_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
00519 {
00520 JSK_ROS_FATAL("successors should be an array");
00521 return false;
00522 }
00523 for (size_t i_successors = 0; i_successors < successors_xml.size(); i_successors++) {
00524 XmlRpc::XmlRpcValue successor_xml;
00525 successor_xml = successors_xml[i_successors];
00526 if (successor_xml.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00527 JSK_ROS_FATAL("element of successors should be an dictionary");
00528 return false;
00529 }
00530 double x = 0;
00531 double y = 0;
00532 double theta = 0;
00533 if (successor_xml.hasMember("x")) {
00534 x = jsk_topic_tools::getXMLDoubleValue(successor_xml["x"]);
00535 }
00536 if (successor_xml.hasMember("y")) {
00537 y = jsk_topic_tools::getXMLDoubleValue(successor_xml["y"]);
00538 }
00539 if (successor_xml.hasMember("theta")) {
00540 theta = jsk_topic_tools::getXMLDoubleValue(successor_xml["theta"]);
00541 }
00542 Eigen::Affine3f successor = affineFromXYYaw(x, y, theta);
00543 successors_.push_back(successor);
00544 }
00545 JSK_ROS_INFO("%lu successors are defined", successors_.size());
00546 return true;
00547 }
00548
00549 void FootstepPlanner::configCallback(Config &config, uint32_t level)
00550 {
00551 boost::mutex::scoped_lock lock(mutex_);
00552 bool need_to_rebuild_graph = false;
00553 if (use_pointcloud_model_ != config.use_pointcloud_model) {
00554 use_pointcloud_model_ = config.use_pointcloud_model;
00555 need_to_rebuild_graph = true;
00556 }
00557 if (use_lazy_perception_ != config.use_lazy_perception) {
00558 use_lazy_perception_ = config.use_lazy_perception;
00559 need_to_rebuild_graph = true;
00560 }
00561 if (use_local_movement_ != config.use_local_movement) {
00562 use_local_movement_ = config.use_local_movement;
00563 need_to_rebuild_graph = true;
00564 }
00565 if (resolution_x_ != config.resolution_x) {
00566 resolution_x_ = config.resolution_x;
00567 need_to_rebuild_graph = true;
00568 }
00569 if (resolution_y_ != config.resolution_y) {
00570 resolution_y_ = config.resolution_y;
00571 need_to_rebuild_graph = true;
00572 }
00573 if (resolution_theta_ != config.resolution_theta) {
00574 resolution_theta_ = config.resolution_theta;
00575 need_to_rebuild_graph = true;
00576 }
00577 rich_profiling_ = config.rich_profiling;
00578 use_transition_limit_ = config.use_transition_limit;
00579 local_move_x_ = config.local_move_x;
00580 local_move_y_ = config.local_move_y;
00581 local_move_theta_ = config.local_move_theta;
00582 local_move_x_num_ = config.local_move_x_num;
00583 local_move_y_num_ = config.local_move_y_num;
00584 local_move_theta_num_ = config.local_move_theta_num;
00585 transition_limit_x_ = config.transition_limit_x;
00586 transition_limit_y_ = config.transition_limit_y;
00587 transition_limit_z_ = config.transition_limit_z;
00588 transition_limit_roll_ = config.transition_limit_roll;
00589 transition_limit_pitch_ = config.transition_limit_pitch;
00590 transition_limit_yaw_ = config.transition_limit_yaw;
00591 goal_pos_thr_ = config.goal_pos_thr;
00592 goal_rot_thr_ = config.goal_rot_thr;
00593 plane_estimation_max_iterations_ = config.plane_estimation_max_iterations;
00594 plane_estimation_min_inliers_ = config.plane_estimation_min_inliers;
00595 plane_estimation_outlier_threshold_ = config.plane_estimation_outlier_threshold;
00596 support_check_x_sampling_ = config.support_check_x_sampling;
00597 support_check_y_sampling_ = config.support_check_y_sampling;
00598 support_check_vertex_neighbor_threshold_ = config.support_check_vertex_neighbor_threshold;
00599 footstep_size_x_ = config.footstep_size_x;
00600 footstep_size_y_ = config.footstep_size_y;
00601 project_start_state_ = config.project_start_state;
00602 project_goal_state_ = config.project_goal_state;
00603 close_list_x_num_ = config.close_list_x_num;
00604 close_list_y_num_ = config.close_list_y_num;
00605 close_list_theta_num_ = config.close_list_theta_num;
00606 profile_period_ = config.profile_period;
00607 heuristic_ = config.heuristic;
00608 heuristic_first_rotation_weight_ = config.heuristic_first_rotation_weight;
00609 heuristic_second_rotation_weight_ = config.heuristic_second_rotation_weight;
00610 cost_weight_ = config.cost_weight;
00611 heuristic_weight_ = config.heuristic_weight;
00612 if (need_to_rebuild_graph) {
00613 if (graph_) {
00614 buildGraph();
00615 }
00616 }
00617 }
00618
00619 void FootstepPlanner::buildGraph()
00620 {
00621 graph_.reset(new FootstepGraph(Eigen::Vector3f(resolution_x_,
00622 resolution_y_,
00623 resolution_theta_),
00624 use_pointcloud_model_,
00625 use_lazy_perception_,
00626 use_local_movement_));
00627 if (use_pointcloud_model_ && pointcloud_model_) {
00628 graph_->setPointCloudModel(pointcloud_model_);
00629 }
00630 graph_->setBasicSuccessors(successors_);
00631 }
00632 }
00633