7 , start_foot_selection(msgs::StepPlanRequest::AUTO)
8 , start_pose_set_up(false)
9 , goal_pose_set_up(false)
10 , max_number_steps(0.0)
11 , max_path_length_ratio(0.0)
13 , ivCheckedFootContactSupport(new
pcl::PointCloud<
pcl::PointXYZI>())
22 if (!ParameterManager::empty())
25 ROS_ERROR(
"[FootstepPlanner] Can't initialize environment due to missing parameters!");
38 if (
env_params->ivPlannerType ==
"ARAPlanner" ||
64 else if (
env_params->ivPlannerType ==
"ADPlanner")
70 else if (
env_params->ivPlannerType ==
"RSTARPlanner")
92 bool path_existed = (bool)
ivPath.size();
95 std::vector<int> solution_state_ids;
111 std::vector<int> changed_edges;
112 changed_edges.push_back(mdp_config.startstateid);
116 ad_planner->update_preds_of_changededges(&changed_edges);
120 if (
ivPlannerPtr->set_start(mdp_config.startstateid) == 0)
125 if (
ivPlannerPtr->set_goal(mdp_config.goalstateid) == 0)
135 ROS_INFO(
"Start planning (max time: %f, eps_0: %.2f, d_eps: %.2f, h(start, goal): %.3f (l) - %.3f (r))",
136 params.max_time, params.initial_eps, params.dec_eps,
139 ret =
ivPlannerPtr->replan(&solution_state_ids, params, &path_cost);
141 catch (
const SBPL_Exception* e)
143 ROS_ERROR(
"SBPL planning failed (%s)", e->what());
148 bool path_is_new =
pathIsNew(solution_state_ids);
149 if (ret && solution_state_ids.size() > 0 && path_is_new)
151 ROS_INFO(
"Solution of size %zu found after %f s",
152 solution_state_ids.size(),
157 ROS_INFO(
"Expanded states: %i total / %i new",
172 else if (!path_is_new)
174 ROS_ERROR(
"Solution found by SBPL is the same as the old solution. Replanning failed.");
188 for(std::vector<int>::const_iterator state_ids_iter = state_ids.begin(); state_ids_iter != state_ids.end(); ++state_ids_iter)
200 if (
ivPath.back().getLeg() == RIGHT)
215 std::string plugin_set;
216 if (params.getParam(
"plugin_set", plugin_set))
217 vigir_pluginlib::PluginManager::loadPluginSet(plugin_set);
219 ROS_WARN(
"[FootstepPlanner] setParams: No plugin set was given by parameter set '%s'", params.getName().c_str());
222 StepPlanMsgPlugin::Ptr plugin;
223 if (vigir_pluginlib::PluginManager::getPlugin(plugin))
224 plugin->loadParams(params);
227 StateGenerator::mutableInstance().loadPlugins();
228 StateGenerator::mutableInstance().loadParams(params);
231 RobotModel::mutableInstance().loadPlugins();
232 RobotModel::mutableInstance().loadParams(params);
235 PostProcessor::mutableInstance().loadPlugins();
236 PostProcessor::mutableInstance().loadParams(params);
239 WorldModel::mutableInstance().loadPlugins();
240 WorldModel::mutableInstance().loadParams(params);
243 StepCostEstimator::mutableInstance().loadPlugins();
244 StepCostEstimator::mutableInstance().loadParams(params);
247 Heuristic::mutableInstance().loadPlugins();
248 Heuristic::mutableInstance().loadParams(params);
257 msgs::ErrorStatus status;
263 if (mode & msgs::UpdateMode::UPDATE_MODE_MOVE_TO_VALID)
269 status += ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootstepPlanner",
"updateFoot: Couldn't determine valid position!");
271 else if (mode & msgs::UpdateMode::UPDATE_MODE_3D)
273 if (!WorldModel::instance().isTerrainModelAvailable() || !WorldModel::instance().getTerrainModel()->update3DData(foot.pose))
274 status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FootstepPlanner",
"updateFoot: Couldn't update 3D data.",
false);
276 else if (mode & msgs::UpdateMode::UPDATE_MODE_Z)
278 if (!WorldModel::instance().isTerrainModelAvailable() || !WorldModel::instance().getTerrainModel()->getHeight(foot.pose.position.x, foot.pose.position.y, foot.pose.position.z))
279 status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FootstepPlanner",
"updateFoot: Couldn't update z.",
false);
293 msgs::ErrorStatus status;
295 if (mode & msgs::UpdateMode::UPDATE_MODE_MOVE_TO_VALID)
297 State left(feet.left);
298 State right(feet.right);
301 left.getFoot(feet.left);
302 right.getFoot(feet.right);
305 status += ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootstepPlanner",
"updateFeet: Couldn't determine valid position!");
307 mode -= msgs::UpdateMode::UPDATE_MODE_MOVE_TO_VALID;
310 status +=
updateFoot(feet.left, mode, transform);
311 status +=
updateFoot(feet.right, mode, transform);
318 if (step_plan.steps.empty())
319 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"[FootstepPlanner]",
"updateStepPlan: Can't process empty step plan!");
320 if (step_plan.start.header.frame_id.empty())
321 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"[FootstepPlanner]",
"updateStepPlan: Can't process step plan due to missing start feet poses!");
330 msgs::ErrorStatus status;
332 if (mode & msgs::UpdateMode::UPDATE_MODE_REPLAN)
334 ROS_WARN(
"UPDATE_MODE_REPLAN isn't implemented yet!");
338 std::vector<msgs::Step>::iterator itr = step_plan.steps.begin();
340 State left_state(step_plan.start.left);
341 State right_state(step_plan.start.right);
342 State prev_state(*itr);
345 for (; itr != step_plan.steps.end(); itr++)
347 msgs::Step& cur_step = *itr;
348 State cur_state = State(cur_step);
351 status +=
updateFoot(cur_step.foot, mode,
false);
354 if (mode & msgs::UpdateMode::UPDATE_MODE_CHECK_VALIDITY)
356 cur_step.valid = RobotModel::instance().isReachable(left_state, right_state, cur_state);
360 if (mode & msgs::UpdateMode::UPDATE_MODE_CHECK_COLLISION)
362 cur_step.colliding = !WorldModel::instance().isAccessible(cur_state, prev_state);
366 if (mode & msgs::UpdateMode::UPDATE_MODE_COST)
369 if (StepCostEstimator::instance().getCost(left_state, right_state, cur_state, c, r))
375 status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FootstepPlanner",
"updateStepPlan: Couldn't determine cost for step " + boost::lexical_cast<std::string>(cur_step.step_index) +
"!");
379 if (cur_state.getLeg() == LEFT)
380 left_state = cur_state;
382 right_state = cur_state;
384 prev_state = cur_state;
392 return msgs::ErrorStatus();
399 Heuristic::mutableInstance().resetPlugins();
420 StepPlanMsgPlugin::Ptr plugin;
421 if (vigir_pluginlib::PluginManager::getPlugin(plugin))
424 StateGenerator::mutableInstance().resetPlugins();
425 RobotModel::mutableInstance().resetPlugins();
426 PostProcessor::mutableInstance().resetPlugins();
427 StepCostEstimator::mutableInstance().resetPlugins();
428 Heuristic::mutableInstance().resetPlugins();
442 if (!
setStart(req.plan_request,
true))
443 return ErrorStatusError(msgs::ErrorStatus::ERR_INVALID_START,
"FootstepPlanner",
"planSteps: Couldn't set start pose! Please check if poses are set!");
446 if (!
setGoal(req.plan_request))
447 return ErrorStatusError(msgs::ErrorStatus::ERR_INVALID_GOAL,
"FootstepPlanner",
"planSteps: Couldn't set goal pose! Please check if poses are set!");
451 ReplanParams params(req.plan_request.max_planning_time > 0 ? req.plan_request.max_planning_time :
env_params->max_planning_time);
453 params.final_eps = 1.0;
455 params.return_first_solution =
env_params->ivSearchUntilFirstSolution;
456 params.repair_time = -1;
460 return ErrorStatusError(msgs::ErrorStatus::ERR_NO_SOLUTION,
"FootstepPlanner",
"planSteps: No solution found!");
462 return msgs::ErrorStatus();
467 double cell_size = 0.0001;
468 int num_angle_bins = 2048;
469 double angle_bin_size = (2.0*M_PI / num_angle_bins);
472 if (!
setStart(req.plan_request,
true))
473 return ErrorStatusError(msgs::ErrorStatus::ERR_INVALID_START,
"FootstepPlanner",
"planPattern: Couldn't set start pose! Please check if poses are set!");
479 unsigned int num_steps = req.plan_request.pattern_parameters.steps;
480 bool close_step = req.plan_request.pattern_parameters.close_step && num_steps > 0;
481 bool single_step_mode =
false;
483 bool change_z =
true;
484 double step_up_height = std::abs(req.plan_request.pattern_parameters.dz);
485 double extra_seperation_factor = req.plan_request.pattern_parameters.extra_seperation ? 1.25 : 1.0;
487 ROS_INFO(
"Start planning stepping (mode: %u, steps: %u)\n", req.plan_request.pattern_parameters.mode, num_steps);
489 State previous_state;
492 msgs::ErrorStatus status;
494 switch (req.plan_request.pattern_parameters.mode)
496 case msgs::PatternParameters::STEP_UP:
497 case msgs::PatternParameters::STEP_DOWN:
498 case msgs::PatternParameters::STEP_OVER:
500 case msgs::PatternParameters::FORWARD:
503 new Footstep(req.plan_request.pattern_parameters.step_distance_forward,
env_params->foot_seperation, 0.0,
512 case msgs::PatternParameters::BACKWARD:
515 new Footstep(-req.plan_request.pattern_parameters.step_distance_forward,
env_params->foot_seperation, 0.0,
524 case msgs::PatternParameters::STRAFE_LEFT:
527 new Footstep(0.0,
env_params->foot_seperation+req.plan_request.pattern_parameters.step_distance_sideward, 0.0,
534 single_step_mode =
true;
537 case msgs::PatternParameters::STRAFE_RIGHT:
540 new Footstep(0.0,
env_params->foot_seperation+req.plan_request.pattern_parameters.step_distance_sideward, 0.0,
547 single_step_mode =
true;
550 case msgs::PatternParameters::ROTATE_LEFT:
553 new Footstep(-sin(req.plan_request.pattern_parameters.turn_angle)*(
env_params->foot_seperation*extra_seperation_factor)/2,
554 cos(req.plan_request.pattern_parameters.turn_angle)*(
env_params->foot_seperation*extra_seperation_factor)/2+(
env_params->foot_seperation*extra_seperation_factor)/2,
555 req.plan_request.pattern_parameters.turn_angle,
562 single_step_mode =
true;
565 case msgs::PatternParameters::ROTATE_RIGHT:
568 new Footstep(-sin(req.plan_request.pattern_parameters.turn_angle)*(
env_params->foot_seperation*extra_seperation_factor)/2,
569 cos(req.plan_request.pattern_parameters.turn_angle)*(
env_params->foot_seperation*extra_seperation_factor)/2+(
env_params->foot_seperation*extra_seperation_factor)/2,
570 req.plan_request.pattern_parameters.turn_angle,
577 single_step_mode =
true;
580 case msgs::PatternParameters::SAMPLING:
583 unsigned int start_foot;
585 if (req.plan_request.start_foot_selection != msgs::StepPlanRequest::AUTO)
586 start_foot = req.plan_request.start_foot_selection;
589 if (req.plan_request.pattern_parameters.step_distance_sideward < 0.0 || (req.plan_request.pattern_parameters.step_distance_sideward == 0.0 && req.plan_request.pattern_parameters.turn_angle < 0.0))
590 start_foot = msgs::StepPlanRequest::RIGHT;
592 start_foot = msgs::StepPlanRequest::LEFT;
596 if (req.plan_request.pattern_parameters.step_distance_sideward == 0.0 && req.plan_request.pattern_parameters.turn_angle == 0.0)
599 new Footstep(req.plan_request.pattern_parameters.step_distance_forward,
env_params->foot_seperation+req.plan_request.pattern_parameters.step_distance_sideward, req.plan_request.pattern_parameters.turn_angle,
605 new Footstep(req.plan_request.pattern_parameters.step_distance_forward,
env_params->foot_seperation-req.plan_request.pattern_parameters.step_distance_sideward, -req.plan_request.pattern_parameters.turn_angle,
611 else if (start_foot == msgs::StepPlanRequest::LEFT)
614 new Footstep(req.plan_request.pattern_parameters.step_distance_forward,
env_params->foot_seperation+req.plan_request.pattern_parameters.step_distance_sideward, req.plan_request.pattern_parameters.turn_angle,
620 new Footstep(req.plan_request.pattern_parameters.step_distance_forward,
env_params->foot_seperation, 0.0,
629 new Footstep(req.plan_request.pattern_parameters.step_distance_forward,
env_params->foot_seperation, 0.0,
635 new Footstep(req.plan_request.pattern_parameters.step_distance_forward,
env_params->foot_seperation-req.plan_request.pattern_parameters.step_distance_sideward, -req.plan_request.pattern_parameters.turn_angle,
647 case msgs::PatternParameters::FEET_REALIGN_ON_CENTER:
661 if (hasError(status))
665 if (req.plan_request.start_foot_selection == msgs::StepPlanRequest::RIGHT)
684 case msgs::PatternParameters::FEET_REALIGN_ON_LEFT:
692 case msgs::PatternParameters::FEET_REALIGN_ON_RIGHT:
700 case msgs::PatternParameters::WIDE_STANCE:
714 if (hasError(status))
718 if (req.plan_request.start_foot_selection == msgs::StepPlanRequest::RIGHT)
720 State temp =
getFootPose(feet_center, RIGHT, req.plan_request.pattern_parameters.step_distance_forward, req.plan_request.pattern_parameters.step_distance_sideward, req.plan_request.pattern_parameters.turn_angle);
723 temp =
getFootPose(feet_center, LEFT, req.plan_request.pattern_parameters.step_distance_forward, req.plan_request.pattern_parameters.step_distance_sideward, req.plan_request.pattern_parameters.turn_angle);
728 State temp =
getFootPose(feet_center, LEFT, req.plan_request.pattern_parameters.step_distance_forward, req.plan_request.pattern_parameters.step_distance_sideward, req.plan_request.pattern_parameters.turn_angle);
731 temp =
getFootPose(feet_center, RIGHT, req.plan_request.pattern_parameters.step_distance_forward, req.plan_request.pattern_parameters.step_distance_sideward, req.plan_request.pattern_parameters.turn_angle);
738 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootstepPlanner",
"planPattern: Unknown walk mode (" + boost::lexical_cast<std::string>(req.plan_request.pattern_parameters.mode) +
") set!");
742 Footstep parallel_step(0.0,
env_params->foot_seperation, 0.0, 0.0, cell_size, num_angle_bins,
env_params->hash_table_size);
743 Footstep parallel_step_extra(0.0,
env_params->foot_seperation*extra_seperation_factor, 0.0, 0.0, cell_size, num_angle_bins,
env_params->hash_table_size);
748 PlanningState previous_pstate(previous_state, cell_size, angle_bin_size,
env_params->hash_table_size);
749 PlanningState current_pstate(current_state, cell_size, angle_bin_size,
env_params->hash_table_size, &previous_pstate);
753 if (hasError(status))
757 if (req.plan_request.pattern_parameters.mode == msgs::PatternParameters::STEP_UP || req.plan_request.pattern_parameters.mode == msgs::PatternParameters::STEP_OVER)
760 PlanningState temp = step_up.performMeOnThisState(current_pstate);
761 previous_pstate = current_pstate;
762 current_pstate = temp;
764 current_pstate.getState().setZ(current_pstate.getState().getZ() + step_up_height);
768 if (hasError(status))
773 for (
unsigned int i = 0; i < num_steps; i++)
776 if (req.plan_request.pattern_parameters.mode == msgs::PatternParameters::SAMPLING)
778 if (current_pstate.getState().getLeg() == LEFT)
779 footstep = footstep_right;
780 else if (current_pstate.getState().getLeg() == RIGHT)
781 footstep = footstep_left;
785 PlanningState temp = footstep->performMeOnThisState(current_pstate);
786 previous_pstate = current_pstate;
787 current_pstate = temp;
791 if (hasError(status))
795 if (single_step_mode && (!req.plan_request.pattern_parameters.close_step || i < (num_steps-1)))
798 PlanningState temp = parallel_step_extra.performMeOnThisState(current_pstate);
799 previous_pstate = current_pstate;
800 current_pstate = temp;
804 if (hasError(status))
811 if (req.plan_request.pattern_parameters.mode == msgs::PatternParameters::STEP_DOWN || req.plan_request.pattern_parameters.mode == msgs::PatternParameters::STEP_OVER)
814 PlanningState temp = step_down.performMeOnThisState(current_pstate);
815 previous_pstate = current_pstate;
816 current_pstate = temp;
818 current_pstate.getState().setZ(current_pstate.getState().getZ() - step_up_height);
822 if (hasError(status))
830 PlanningState temp = parallel_step.performMeOnThisState(current_pstate);
831 previous_pstate = current_pstate;
832 current_pstate = temp;
836 if (hasError(status))
846 s.setZ(s.getZ() + params.dz);
851 temp.setRoll(params.roll);
852 temp.setPitch(params.pitch);
854 return msgs::ErrorStatus();
859 return msgs::ErrorStatus();
862 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootstepPlanner",
"addStepping: Something went wrong...");
867 resp.step_plan.header.frame_id =
frame_id;
868 resp.step_plan.header.stamp = req.plan_request.header.stamp;
875 StepPlanMsgPlugin::Ptr plugin;
876 if (vigir_pluginlib::PluginManager::getPlugin(plugin))
878 plugin->initMsg(resp.step_plan);
879 plugin->initMsg(step);
882 step.header = resp.step_plan.header;
883 step.foot.header = resp.step_plan.header;
885 step.colliding =
false;
890 int step_index = req.plan_request.start_step_index;
894 State swing_foot = *path_iter;
895 const State& stand_foot = swing_foot.getLeg() == LEFT ? right_foot : left_foot;
899 (path_iter !=
getPathBegin() && req.plan_request.planning_mode == msgs::StepPlanRequest::PLANNING_MODE_PATTERN))
902 PostProcessor::instance().postProcessForward(left_foot, right_foot, swing_foot);
904 PostProcessor::instance().postProcessBackward(left_foot, right_foot, swing_foot);
908 swing_foot.getStep(step);
909 if (swing_foot.getLeg() == LEFT)
910 step.foot.foot_index = msgs::Foot::LEFT;
911 else if (swing_foot.getLeg() == RIGHT)
912 step.foot.foot_index = msgs::Foot::RIGHT;
915 ROS_ERROR(
"Footstep pose at (%.3f, %.3f, %.3f, %.3f) is set to NOLEG!", swing_foot.getX(), swing_foot.getY(), swing_foot.getZ(), swing_foot.getYaw());
918 step.step_index = step_index++;
928 if (std::abs(stand_foot.getZ() - swing_foot.getZ()) > 0.18)
929 resp.status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FootstepPlanner",
"finalizeStepPlan: Plan contains large lift heights");
932 resp.step_plan.steps.push_back(step);
935 if (swing_foot.getLeg() == LEFT)
936 left_foot = swing_foot;
937 else if (swing_foot.getLeg() == RIGHT)
938 right_foot = swing_foot;
942 PostProcessor::instance().postProcess(
resp.step_plan);
945 resp.step_plan.start.header =
resp.step_plan.header;
948 resp.step_plan.start.left.foot_index = msgs::Foot::LEFT;
949 resp.step_plan.start.left.header =
resp.step_plan.header;
952 resp.step_plan.start.right.foot_index = msgs::Foot::RIGHT;
953 resp.step_plan.start.right.header =
resp.step_plan.header;
955 resp.step_plan.goal.header =
resp.step_plan.header;
958 resp.step_plan.goal.left.foot_index = msgs::Foot::LEFT;
959 resp.step_plan.goal.left.header =
resp.step_plan.header;
962 resp.step_plan.goal.right.foot_index = msgs::Foot::RIGHT;
963 resp.step_plan.goal.right.header =
resp.step_plan.header;
966 if (req.plan_request.planning_mode == msgs::StepPlanRequest::PLANNING_MODE_PATTERN)
967 updateStepPlan(
resp.step_plan, msgs::UpdateMode::UPDATE_MODE_CHECK_VALIDITY | msgs::UpdateMode::UPDATE_MODE_CHECK_COLLISION | msgs::UpdateMode::UPDATE_MODE_COST, std::string(),
false);
969 updateStepPlan(
resp.step_plan, msgs::UpdateMode::UPDATE_MODE_COST, std::string(),
false);
974 if (
resp.status.error == msgs::ErrorStatus::NO_ERROR &&
resp.final_eps > 1.8)
975 resp.status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FootstepPlanner",
"stepPlanRequestService: Suboptimal plan (eps: " + boost::lexical_cast<std::string>(
resp.final_eps) +
")!");
978 double total_cost = 0.0;
979 double last_cost = 0.0;
980 for (std::vector<msgs::Step>::const_iterator itr =
resp.step_plan.steps.begin(); itr !=
resp.step_plan.steps.end(); itr++)
984 geometry_msgs::Vector3 n;
985 quaternionToNormal(
step.foot.pose.orientation, n);
987 ROS_INFO(
"[%i] n: %.3f/%.3f/%.3f",
step.step_index, n.x, n.y, n.z);
988 ROS_INFO(
"[%i] swing height: %.3f, sway duration: %.3f, step duration: %.3f",
step.step_index,
step.swing_height,
step.sway_duration,
step.step_duration);
989 ROS_INFO(
"[%i] valid: %s, colliding: %s",
step.step_index,
step.valid ?
"y" :
"n",
step.colliding ?
"y" :
"n");
992 total_cost +=
step.cost;
993 last_cost =
step.cost;
995 if (WorldModel::instance().isTerrainModelAvailable())
997 double support = 0.0;
999 ROS_INFO(
"[%i] Ground contact support: %.3f",
step.step_index, support);
1002 ROS_INFO(
"-------------------------------------");
1004 ROS_INFO(
"Total path cost: %f (%f)", total_cost, total_cost-last_cost);
1008 sensor_msgs::PointCloud2 msg;
1032 std::string request_frame_id = strip_const(req.plan_request.header.frame_id,
'/');
1033 std::string start_frame_id = strip_const(req.plan_request.start.header.frame_id,
'/');
1034 std::string goal_frame_id = strip_const(req.plan_request.goal.header.frame_id,
'/');
1036 if (request_frame_id != start_frame_id)
1037 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootstepPlanner",
"stepPlanRequest: Frame IDs of plan request ('" + request_frame_id +
") and start pose ('" + start_frame_id +
"') do not match!");
1039 if (req.plan_request.planning_mode != msgs::StepPlanRequest::PLANNING_MODE_PATTERN && request_frame_id != goal_frame_id)
1040 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootstepPlanner",
"stepPlanRequest: Frame IDs of plan request ('" + request_frame_id +
"') and goal pose ('" + goal_frame_id +
"') do not match!");
1043 if (!req.plan_request.parameter_set_name.data.empty())
1045 if (!ParameterManager::setActive(req.plan_request.parameter_set_name.data))
1046 return ErrorStatusError(msgs::ErrorStatus::ERR_INVALID_PARAMETERS,
"FootstepPlanner",
"stepPlanRequest: Can't find parameter set named '" + req.plan_request.parameter_set_name.data +
"'!");
1048 setParams(ParameterManager::getActive());
1064 frame_id = req.plan_request.header.frame_id;
1070 return msgs::ErrorStatus();
1078 if (hasError(resp.status))
1101 msgs::StepPlanRequestService::Response
resp;
1104 if (req.plan_request.planning_mode == msgs::StepPlanRequest::PLANNING_MODE_3D)
1105 WorldModel::mutableInstance().useTerrainModel(
true);
1106 else if (req.plan_request.planning_mode == msgs::StepPlanRequest::PLANNING_MODE_PATTERN)
1107 WorldModel::mutableInstance().useTerrainModel(req.plan_request.pattern_parameters.use_terrain_model);
1109 WorldModel::mutableInstance().useTerrainModel(
false);
1112 switch (req.plan_request.planning_mode)
1114 case msgs::StepPlanRequest::PLANNING_MODE_2D:
1115 case msgs::StepPlanRequest::PLANNING_MODE_3D:
1118 case msgs::StepPlanRequest::PLANNING_MODE_PATTERN:
1122 resp.status = ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootstepPlanner",
"stepPlanRequest: A not supported planning mode '" + boost::lexical_cast<std::string>(req.plan_request.planning_mode) +
"' was given!");
1130 if (!hasError(resp.status))
1151 if (WorldModel::instance().isAccessible(s))
1154 State current_state = s;
1155 State best_state = s;
1157 double pos_diff = FLT_MAX;
1158 double yaw_diff = FLT_MAX;
1159 bool solution_found =
false;
1164 t.
setBasis(s.getPose().getBasis());
1170 for (
double yaw = -0.2; yaw <= 0.4; yaw+=
env_params->angle_bin_size)
1172 current_state.setYaw(s.getYaw() + (s.getLeg() == LEFT ? yaw : -yaw));
1173 for (
double y = -0.05; y <= 0.2; y+=
env_params->cell_size)
1175 orig_pos.
setY(s.getLeg() == LEFT ? y : -y);
1176 for (
double x = -0.15; x <= 0.15; x+=
env_params->cell_size)
1180 trans_pos = t * orig_pos;
1182 current_state.
setX(trans_pos.
getX());
1183 current_state.setY(trans_pos.
getY());
1185 if (!WorldModel::instance().update3DData(current_state))
1188 if (!WorldModel::instance().isAccessible(current_state))
1191 double dist = std::sqrt(x*x + y*y);
1192 if (pos_diff >= dist && yaw_diff >= std::abs(yaw))
1194 best_state = current_state;
1196 yaw_diff = std::abs(yaw);
1197 solution_found =
true;
1206 return solution_found;
1211 left = !WorldModel::instance().isAccessible(left_foot);
1212 right = !WorldModel::instance().isAccessible(right_foot);
1214 if (!left && !right && !WorldModel::instance().isAccessible(left_foot, right_foot))
1220 return left || right;
1226 if (std::isnan(left_foot.getRoll()))
return false;
1227 if (std::isnan(left_foot.getPitch()))
return false;
1228 if (std::isnan(left_foot.getYaw()))
return false;
1229 if (std::isnan(right_foot.getRoll()))
return false;
1230 if (std::isnan(right_foot.getPitch()))
return false;
1231 if (std::isnan(right_foot.getYaw()))
return false;
1233 bool left_collision =
false;
1234 bool right_collision =
false;
1236 if (!ignore_collision &&
checkRobotCollision(left_foot, right_foot, left_collision, right_collision))
1247 ROS_INFO(
"Start foot poses set to (left: %f %f %f %f) and (right: %f %f %f %f)",
1256 State left_foot(req.start.left);
1257 State right_foot(req.start.right);
1259 if (req.planning_mode == msgs::StepPlanRequest::PLANNING_MODE_2D)
1261 double z = 0.5*(req.start.left.pose.position.z + req.start.right.pose.position.z);
1266 return setStart(left_foot, right_foot, ignore_collision);
1272 if (std::isnan(left_foot.getRoll()))
return false;
1273 if (std::isnan(left_foot.getPitch()))
return false;
1274 if (std::isnan(left_foot.getYaw()))
return false;
1275 if (std::isnan(right_foot.getRoll()))
return false;
1276 if (std::isnan(right_foot.getPitch()))
return false;
1277 if (std::isnan(right_foot.getYaw()))
return false;
1279 bool left_collision =
false;
1280 bool right_collision =
false;
1282 if (!ignore_collision &&
checkRobotCollision(left_foot, right_foot, left_collision, right_collision))
1293 ROS_INFO(
"Goal foot poses set to (left: %f %f %f %f) and (right: %f %f %f %f)",
1302 State left_foot(req.goal.left);
1303 State right_foot(req.goal.right);
1305 if (req.planning_mode == msgs::StepPlanRequest::PLANNING_MODE_2D)
1307 double z = 0.5*(req.start.left.pose.position.z + req.start.right.pose.position.z);
1308 geometry_msgs::Vector3 n;
1311 quaternionToNormal(req.start.left.pose.orientation, n);
1312 left_foot.setNormal(n);
1315 quaternionToNormal(req.start.right.pose.orientation, n);
1316 right_foot.setNormal(n);
1319 return setGoal(left_foot, right_foot, ignore_collision);
1328 double cos_theta = cos(robot.getYaw());
1329 double sin_theta = sin(robot.getYaw());
1330 double shift_x = cos_theta * sign * dx - sin_theta * (0.5 *
env_params->foot_seperation + dy);
1331 double shift_y = sin_theta * sign * dx + cos_theta * (0.5 *
env_params->foot_seperation + dy);
1333 State foot(robot.getX() + sign * shift_x,
1334 robot.getY() + sign * shift_y,
1338 robot.getYaw() + sign * dyaw,
1341 WorldModel::instance().update3DData(foot);
1356 bool unequal =
true;
1357 for (
unsigned i = 0; i < new_path.size(); ++i)
void publish(const boost::shared_ptr< M > &message) const
TFSIMD_FORCE_INLINE void setX(tfScalar x)
TFSIMD_FORCE_INLINE void setY(tfScalar y)
static double getYaw(const Quaternion &bt_q)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
TFSIMD_FORCE_INLINE const tfScalar & z() const
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
TFSIMD_FORCE_INLINE const tfScalar & getX() const
#define ROS_ERROR_STREAM(args)