A class to control the interaction between ROS and the footstep planner. More...
#include <footstep_planner.h>
Public Types | |
typedef boost::shared_ptr < const FootstepPlanner > | ConstPtr |
typedef boost::function< void(msgs::PlanningFeedback)> | FeedbackCB |
typedef boost::function< void()> | PreemptCB |
typedef boost::shared_ptr < FootstepPlanner > | Ptr |
typedef boost::function< void(msgs::StepPlanRequestService::Response)> | ResultCB |
Public Member Functions | |
FootstepPlanner (ros::NodeHandle &nh) | |
bool | isPlanning () const |
void | preemptPlanning () |
stops thread running planning | |
bool | setParams (const vigir_generic_params::ParameterSet ¶ms) |
msgs::ErrorStatus | stepPlanRequest (msgs::StepPlanRequestService::Request &req, ResultCB result_cb=ResultCB(), FeedbackCB feedback_cb=FeedbackCB(), PreemptCB preempt_cb=PreemptCB()) |
bool | stepPlanRequestService (msgs::StepPlanRequestService::Request &req, msgs::StepPlanRequestService::Response &resp) |
Service handle to plan footsteps. | |
msgs::ErrorStatus | updateFeet (msgs::Feet &feet, uint8_t mode, bool transform=true) const |
msgs::ErrorStatus | updateFoot (msgs::Foot &foot, uint8_t mode, bool transform=true) const |
msgs::ErrorStatus | updateStepPlan (msgs::StepPlan &result, uint8_t mode, const std::string ¶m_set_name=std::string(), bool transform=true) const |
virtual | ~FootstepPlanner () |
Protected Member Functions | |
bool | checkRobotCollision (const State &left_foot, const State &right_foot, bool &left, bool &right) const |
void | doPlanning (msgs::StepPlanRequestService::Request &req) |
: method used in seperate thread | |
bool | extractPath (const std::vector< int > &state_ids) |
Extracts the path (list of foot poses) from a list of state IDs calculated by the SBPL. | |
msgs::ErrorStatus | finalizeAndAddStepToPlan (State &s, const msgs::PatternParameters &env_params, bool change_z=true) |
bool | finalizeStepPlan (msgs::StepPlanRequestService::Request &req, msgs::StepPlanRequestService::Response &resp, bool post_process) |
extracts step plan response from planning result | |
bool | findNearestValidState (State &s) const |
State | getFootPose (const State &robot, Leg leg, double dx, double dy, double dyaw) |
Returns the foot pose of a leg for a given robot pose. | |
State | getFootPose (const State &robot, Leg leg) |
size_t | getNumExpandedStates () const |
size_t | getNumFootPoses () const |
state_iter_t | getPathBegin () const |
double | getPathCosts () const |
state_iter_t | getPathEnd () const |
int | getPathSize () |
State | getStartFootLeft () |
State | getStartFootRight () |
bool | pathExists () |
bool | pathIsNew (const std::vector< int > &new_path) |
bool | plan (ReplanParams ¶ms) |
Starts the planning task in the underlying SBPL. | |
msgs::ErrorStatus | planPattern (msgs::StepPlanRequestService::Request &req) |
plans stepping | |
msgs::ErrorStatus | planSteps (msgs::StepPlanRequestService::Request &req) |
Start a planning task from scratch (will delete information of previous planning tasks). Map and start, goal poses need to be set beforehand. | |
void | reset () |
Reset the previous planning information. | |
void | resetTotally () |
Reset and reinitialize the environment. | |
bool | setGoal (const State &left_foot, const State &right_foot, bool ignore_collision=false) |
Sets the goal pose as position of left and right footsteps. | |
bool | setGoal (const msgs::StepPlanRequest &req, bool ignore_collision=false) |
Sets the goal pose. | |
void | setPlanner () |
Sets the planning algorithm used by SBPL. | |
bool | setStart (const State &left_foot, const State &right_foot, bool ignore_collision=false) |
Sets the start pose as position of left and right footsteps. | |
bool | setStart (const msgs::StepPlanRequest &req, bool ignore_collision=false) |
Sets the start pose. | |
void | startPlanning (msgs::StepPlanRequestService::Request &req) |
: starts planning in a new thread | |
Protected Attributes | |
EnvironmentParameters::Ptr | env_params |
FeedbackCB | feedback_cb |
FootPoseTransformer | foot_pose_transformer |
std::string | frame_id |
bool | goal_pose_set_up |
pcl::PointCloud < pcl::PointXYZI >::Ptr | ivCheckedFootContactSupport |
ros::Publisher | ivCheckedFootContactSupportPub |
State | ivGoalFootLeft |
State | ivGoalFootRight |
std::vector< State > | ivPath |
double | ivPathCost |
boost::shared_ptr < FootstepPlannerEnvironment > | ivPlannerEnvironmentPtr |
boost::shared_ptr< SBPLPlanner > | ivPlannerPtr |
std::vector< int > | ivPlanningStatesIds |
State | ivStartFootLeft |
State | ivStartFootRight |
double | max_number_steps |
double | max_path_length_ratio |
boost::recursive_mutex | planner_mutex |
boost::thread | planning_thread |
PreemptCB | preempt_cb |
ResultCB | result_cb |
unsigned int | start_foot_selection |
bool | start_pose_set_up |
unsigned int | step_plan_uid |
A class to control the interaction between ROS and the footstep planner.
Definition at line 92 of file footstep_planner.h.
typedef boost::shared_ptr<const FootstepPlanner> vigir_footstep_planning::FootstepPlanner::ConstPtr |
Definition at line 119 of file footstep_planner.h.
typedef boost::function<void (msgs::PlanningFeedback)> vigir_footstep_planning::FootstepPlanner::FeedbackCB |
Definition at line 96 of file footstep_planner.h.
typedef boost::function<void ()> vigir_footstep_planning::FootstepPlanner::PreemptCB |
Definition at line 97 of file footstep_planner.h.
typedef boost::shared_ptr<FootstepPlanner> vigir_footstep_planning::FootstepPlanner::Ptr |
Definition at line 118 of file footstep_planner.h.
typedef boost::function<void (msgs::StepPlanRequestService::Response)> vigir_footstep_planning::FootstepPlanner::ResultCB |
Definition at line 95 of file footstep_planner.h.
Definition at line 5 of file footstep_planner.cpp.
Definition at line 28 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::checkRobotCollision | ( | const State & | left_foot, |
const State & | right_foot, | ||
bool & | left, | ||
bool & | right | ||
) | const [protected] |
Definition at line 1161 of file footstep_planner.cpp.
void vigir_footstep_planning::FootstepPlanner::doPlanning | ( | msgs::StepPlanRequestService::Request & | req | ) | [protected] |
: method used in seperate thread
Definition at line 1048 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::extractPath | ( | const std::vector< int > & | state_ids | ) | [protected] |
Extracts the path (list of foot poses) from a list of state IDs calculated by the SBPL.
Definition at line 184 of file footstep_planner.cpp.
msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::finalizeAndAddStepToPlan | ( | State & | s, |
const msgs::PatternParameters & | env_params, | ||
bool | change_z = true |
||
) | [protected] |
Definition at line 795 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::finalizeStepPlan | ( | msgs::StepPlanRequestService::Request & | req, |
msgs::StepPlanRequestService::Response & | resp, | ||
bool | post_process | ||
) | [protected] |
extracts step plan response from planning result
Definition at line 817 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::findNearestValidState | ( | State & | s | ) | const [protected] |
Definition at line 1101 of file footstep_planner.cpp.
State vigir_footstep_planning::FootstepPlanner::getFootPose | ( | const State & | robot, |
Leg | leg, | ||
double | dx, | ||
double | dy, | ||
double | dyaw | ||
) | [protected] |
Returns the foot pose of a leg for a given robot pose.
Definition at line 1274 of file footstep_planner.cpp.
State vigir_footstep_planning::FootstepPlanner::getFootPose | ( | const State & | robot, |
Leg | leg | ||
) | [protected] |
Definition at line 1298 of file footstep_planner.cpp.
size_t vigir_footstep_planning::FootstepPlanner::getNumExpandedStates | ( | ) | const [inline, protected] |
Definition at line 179 of file footstep_planner.h.
size_t vigir_footstep_planning::FootstepPlanner::getNumFootPoses | ( | ) | const [inline, protected] |
Definition at line 185 of file footstep_planner.h.
state_iter_t vigir_footstep_planning::FootstepPlanner::getPathBegin | ( | ) | const [inline, protected] |
Definition at line 187 of file footstep_planner.h.
double vigir_footstep_planning::FootstepPlanner::getPathCosts | ( | ) | const [inline, protected] |
Definition at line 176 of file footstep_planner.h.
state_iter_t vigir_footstep_planning::FootstepPlanner::getPathEnd | ( | ) | const [inline, protected] |
Definition at line 188 of file footstep_planner.h.
int vigir_footstep_planning::FootstepPlanner::getPathSize | ( | ) | [inline, protected] |
Definition at line 191 of file footstep_planner.h.
State vigir_footstep_planning::FootstepPlanner::getStartFootLeft | ( | ) | [inline, protected] |
Definition at line 193 of file footstep_planner.h.
State vigir_footstep_planning::FootstepPlanner::getStartFootRight | ( | ) | [inline, protected] |
Definition at line 194 of file footstep_planner.h.
bool vigir_footstep_planning::FootstepPlanner::isPlanning | ( | ) | const |
Definition at line 85 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::pathExists | ( | ) | [inline, protected] |
Definition at line 203 of file footstep_planner.h.
bool vigir_footstep_planning::FootstepPlanner::pathIsNew | ( | const std::vector< int > & | new_path | ) | [protected] |
Definition at line 1303 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::plan | ( | ReplanParams & | params | ) | [protected] |
Starts the planning task in the underlying SBPL.
NOTE: Never call this directly. Always use either plan() or replan() to invoke this method.
: updateGoal adds goal states to list so planner may use it independend from costs...
Definition at line 90 of file footstep_planner.cpp.
msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::planPattern | ( | msgs::StepPlanRequestService::Request & | req | ) | [protected] |
plans stepping
Definition at line 465 of file footstep_planner.cpp.
msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::planSteps | ( | msgs::StepPlanRequestService::Request & | req | ) | [protected] |
Start a planning task from scratch (will delete information of previous planning tasks). Map and start, goal poses need to be set beforehand.
: Hack to disable collision check for start pose
Definition at line 439 of file footstep_planner.cpp.
stops thread running planning
Definition at line 1088 of file footstep_planner.cpp.
void vigir_footstep_planning::FootstepPlanner::reset | ( | ) | [protected] |
Reset the previous planning information.
Definition at line 395 of file footstep_planner.cpp.
void vigir_footstep_planning::FootstepPlanner::resetTotally | ( | ) | [protected] |
Reset and reinitialize the environment.
Definition at line 415 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::setGoal | ( | const State & | left_foot, |
const State & | right_foot, | ||
bool | ignore_collision = false |
||
) | [protected] |
Sets the goal pose as position of left and right footsteps.
Definition at line 1221 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::setGoal | ( | const msgs::StepPlanRequest & | req, |
bool | ignore_collision = false |
||
) | [protected] |
Sets the goal pose.
Definition at line 1252 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::setParams | ( | const vigir_generic_params::ParameterSet & | params | ) |
Definition at line 208 of file footstep_planner.cpp.
void vigir_footstep_planning::FootstepPlanner::setPlanner | ( | ) | [protected] |
Sets the planning algorithm used by SBPL.
Definition at line 36 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::setStart | ( | const State & | left_foot, |
const State & | right_foot, | ||
bool | ignore_collision = false |
||
) | [protected] |
Sets the start pose as position of left and right footsteps.
Definition at line 1175 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::setStart | ( | const msgs::StepPlanRequest & | req, |
bool | ignore_collision = false |
||
) | [protected] |
Sets the start pose.
Definition at line 1206 of file footstep_planner.cpp.
void vigir_footstep_planning::FootstepPlanner::startPlanning | ( | msgs::StepPlanRequestService::Request & | req | ) | [protected] |
: starts planning in a new thread
Definition at line 1042 of file footstep_planner.cpp.
msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::stepPlanRequest | ( | msgs::StepPlanRequestService::Request & | req, |
ResultCB | result_cb = ResultCB() , |
||
FeedbackCB | feedback_cb = FeedbackCB() , |
||
PreemptCB | preempt_cb = PreemptCB() |
||
) |
Definition at line 973 of file footstep_planner.cpp.
bool vigir_footstep_planning::FootstepPlanner::stepPlanRequestService | ( | msgs::StepPlanRequestService::Request & | req, |
msgs::StepPlanRequestService::Response & | resp | ||
) |
Service handle to plan footsteps.
Definition at line 1025 of file footstep_planner.cpp.
msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::updateFeet | ( | msgs::Feet & | feet, |
uint8_t | mode, | ||
bool | transform = true |
||
) | const |
Definition at line 291 of file footstep_planner.cpp.
msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::updateFoot | ( | msgs::Foot & | foot, |
uint8_t | mode, | ||
bool | transform = true |
||
) | const |
Definition at line 255 of file footstep_planner.cpp.
msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::updateStepPlan | ( | msgs::StepPlan & | result, |
uint8_t | mode, | ||
const std::string & | param_set_name = std::string() , |
||
bool | transform = true |
||
) | const |
Definition at line 316 of file footstep_planner.cpp.
Definition at line 257 of file footstep_planner.h.
Definition at line 239 of file footstep_planner.h.
Definition at line 253 of file footstep_planner.h.
std::string vigir_footstep_planning::FootstepPlanner::frame_id [protected] |
Definition at line 256 of file footstep_planner.h.
bool vigir_footstep_planning::FootstepPlanner::goal_pose_set_up [protected] |
Definition at line 259 of file footstep_planner.h.
pcl::PointCloud<pcl::PointXYZI>::Ptr vigir_footstep_planning::FootstepPlanner::ivCheckedFootContactSupport [protected] |
Definition at line 270 of file footstep_planner.h.
Definition at line 233 of file footstep_planner.h.
State vigir_footstep_planning::FootstepPlanner::ivGoalFootLeft [protected] |
Definition at line 249 of file footstep_planner.h.
State vigir_footstep_planning::FootstepPlanner::ivGoalFootRight [protected] |
Definition at line 250 of file footstep_planner.h.
std::vector<State> vigir_footstep_planning::FootstepPlanner::ivPath [protected] |
Definition at line 245 of file footstep_planner.h.
double vigir_footstep_planning::FootstepPlanner::ivPathCost [protected] |
Definition at line 266 of file footstep_planner.h.
boost::shared_ptr<FootstepPlannerEnvironment> vigir_footstep_planning::FootstepPlanner::ivPlannerEnvironmentPtr [protected] |
Definition at line 242 of file footstep_planner.h.
boost::shared_ptr<SBPLPlanner> vigir_footstep_planning::FootstepPlanner::ivPlannerPtr [protected] |
Definition at line 243 of file footstep_planner.h.
std::vector<int> vigir_footstep_planning::FootstepPlanner::ivPlanningStatesIds [protected] |
Definition at line 268 of file footstep_planner.h.
State vigir_footstep_planning::FootstepPlanner::ivStartFootLeft [protected] |
Definition at line 247 of file footstep_planner.h.
State vigir_footstep_planning::FootstepPlanner::ivStartFootRight [protected] |
Definition at line 248 of file footstep_planner.h.
double vigir_footstep_planning::FootstepPlanner::max_number_steps [protected] |
Definition at line 261 of file footstep_planner.h.
double vigir_footstep_planning::FootstepPlanner::max_path_length_ratio [protected] |
Definition at line 264 of file footstep_planner.h.
boost::recursive_mutex vigir_footstep_planning::FootstepPlanner::planner_mutex [mutable, protected] |
Definition at line 235 of file footstep_planner.h.
boost::thread vigir_footstep_planning::FootstepPlanner::planning_thread [protected] |
Definition at line 236 of file footstep_planner.h.
Definition at line 240 of file footstep_planner.h.
Definition at line 238 of file footstep_planner.h.
unsigned int vigir_footstep_planning::FootstepPlanner::start_foot_selection [protected] |
Definition at line 258 of file footstep_planner.h.
bool vigir_footstep_planning::FootstepPlanner::start_pose_set_up [protected] |
Definition at line 259 of file footstep_planner.h.
unsigned int vigir_footstep_planning::FootstepPlanner::step_plan_uid [protected] |
Definition at line 273 of file footstep_planner.h.