A class to control the interaction between ROS and the footstep planner. More...
#include <FootstepPlanner.h>
Public Member Functions | |
void | clearFootstepPathVis (unsigned num_footsteps=0) |
Clear the footstep path visualization from a previous planning task. | |
FootstepPlanner () | |
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 () |
void | goalPoseCallback (const geometry_msgs::PoseStampedConstPtr &goal_pose) |
Callback to set the goal pose as a robot pose centered between two feet. If the start pose has been set previously the planning is started. | |
void | mapCallback (const nav_msgs::OccupancyGridConstPtr &occupancy_map) |
Callback to set the map. | |
bool | pathExists () |
bool | plan () |
Start a planning task from scratch (will delete information of previous planning tasks). Map and start, goal poses need to be set beforehand. | |
bool | plan (const geometry_msgs::PoseStampedConstPtr start, const geometry_msgs::PoseStampedConstPtr goal) |
Sets start, goal poses and calls FootstepPlanner::plan(). | |
bool | plan (float start_x, float start_y, float start_theta, float goal_x, float goal_y, float goal_theta) |
Sets start, goal poses and calls FootstepPlanner::plan(). | |
bool | planService (humanoid_nav_msgs::PlanFootsteps::Request &req, humanoid_nav_msgs::PlanFootsteps::Response &resp) |
Service handle to plan footsteps. | |
bool | replan () |
Starts a planning task based on previous planning information (note that this method can also be used when no previous planning was performed). 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 geometry_msgs::PoseStampedConstPtr goal_pose) |
Sets the goal pose as a robot pose centered between two feet. | |
bool | setGoal (float x, float y, float theta) |
Sets the goal pose as a robot pose centered between two feet. | |
void | setMarkerNamespace (const std::string &ns) |
void | setMaxSearchTime (int search_time) |
Set the maximal search time. | |
bool | setStart (const geometry_msgs::PoseStampedConstPtr start_pose) |
Sets the start pose as a robot pose centered between two feet. | |
bool | setStart (float x, float y, float theta) |
Sets the start pose as a robot pose centered between two feet. | |
bool | setStart (const State &left_foot, const State &right_foot) |
Sets the start pose as position of left and right footsteps. | |
void | startPoseCallback (const geometry_msgs::PoseWithCovarianceStampedConstPtr &start_pose) |
Callback to set the start pose as a robot pose centered between two feet. If the goal pose has been set previously the planning is started. | |
bool | updateMap (const gridmap_2d::GridMap2DPtr map) |
Updates the map in the planning environment. | |
virtual | ~FootstepPlanner () |
Public Attributes | |
environment_params | ivEnvironmentParams |
Planning parameters. | |
Protected Member Functions | |
void | broadcastExpandedNodesVis () |
void | broadcastFootstepPathVis () |
void | broadcastHeuristicPathVis () |
void | broadcastPathVis () |
void | broadcastRandomNodesVis () |
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. | |
void | footPoseToMarker (const State &footstep, visualization_msgs::Marker *marker) |
Generates a visualization msgs for a foot pose. | |
State | getFootPose (const State &robot, Leg side) |
Returns the foot pose of a leg for a given robot pose. | |
bool | pathIsNew (const std::vector< int > &new_path) |
bool | run () |
Starts the planning task in the underlying SBPL. | |
void | setPlanner () |
Sets the planning algorithm used by SBPL. | |
void | updateEnvironment (const gridmap_2d::GridMap2DPtr old_map) |
Updates the environment in case of a changed map. | |
Protected Attributes | |
int | ivChangedCellsLimit |
If limit of changed cells is reached the planner starts a new task from the scratch. | |
int | ivCollisionCheckAccuracy |
ros::Publisher | ivExpandedStatesVisPub |
double | ivFootSeparation |
ros::Publisher | ivFootstepPathVisPub |
ros::ServiceServer | ivFootstepPlanService |
State | ivGoalFootLeft |
State | ivGoalFootRight |
bool | ivGoalPoseSetUp |
ros::Subscriber | ivGridMapSub |
ros::Publisher | ivHeuristicPathVisPub |
double | ivInitialEpsilon |
int | ivLastMarkerMsgSize |
gridmap_2d::GridMap2DPtr | ivMapPtr |
std::string | ivMarkerNamespace |
double | ivMaxSearchTime |
double | ivMaxStepWidth |
std::vector< State > | ivPath |
double | ivPathCost |
boost::shared_ptr< const PathCostHeuristic > | ivPathCostHeuristicPtr |
ros::Publisher | ivPathVisPub |
boost::shared_ptr < FootstepPlannerEnvironment > | ivPlannerEnvironmentPtr |
boost::shared_ptr< SBPLPlanner > | ivPlannerPtr |
std::string | ivPlannerType |
std::vector< int > | ivPlanningStatesIds |
ros::Publisher | ivRandomStatesVisPub |
bool | ivSearchUntilFirstSolution |
State | ivStartFootLeft |
State | ivStartFootRight |
bool | ivStartPoseSetUp |
ros::Publisher | ivStartPoseVisPub |
A class to control the interaction between ROS and the footstep planner.
Definition at line 59 of file FootstepPlanner.h.
Definition at line 34 of file FootstepPlanner.cpp.
footstep_planner::FootstepPlanner::~FootstepPlanner | ( | ) | [virtual] |
Definition at line 273 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::broadcastExpandedNodesVis | ( | ) | [protected] |
Definition at line 959 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::broadcastFootstepPathVis | ( | ) | [protected] |
Definition at line 991 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::broadcastHeuristicPathVis | ( | ) | [protected] |
void footstep_planner::FootstepPlanner::broadcastPathVis | ( | ) | [protected] |
Definition at line 1077 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::broadcastRandomNodesVis | ( | ) | [protected] |
Definition at line 1035 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::clearFootstepPathVis | ( | unsigned | num_footsteps = 0 | ) |
Clear the footstep path visualization from a previous planning task.
Definition at line 933 of file FootstepPlanner.cpp.
bool footstep_planner::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 416 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::footPoseToMarker | ( | const State & | footstep, |
visualization_msgs::Marker * | marker | ||
) | [protected] |
Generates a visualization msgs for a foot pose.
Definition at line 1105 of file FootstepPlanner.cpp.
State footstep_planner::FootstepPlanner::getFootPose | ( | const State & | robot, |
Leg | side | ||
) | [protected] |
Returns the foot pose of a leg for a given robot pose.
Definition at line 902 of file FootstepPlanner.cpp.
size_t footstep_planner::FootstepPlanner::getNumExpandedStates | ( | ) | const [inline] |
Definition at line 189 of file FootstepPlanner.h.
size_t footstep_planner::FootstepPlanner::getNumFootPoses | ( | ) | const [inline] |
Definition at line 195 of file FootstepPlanner.h.
state_iter_t footstep_planner::FootstepPlanner::getPathBegin | ( | ) | const [inline] |
Definition at line 197 of file FootstepPlanner.h.
double footstep_planner::FootstepPlanner::getPathCosts | ( | ) | const [inline] |
Definition at line 186 of file FootstepPlanner.h.
state_iter_t footstep_planner::FootstepPlanner::getPathEnd | ( | ) | const [inline] |
Definition at line 198 of file FootstepPlanner.h.
int footstep_planner::FootstepPlanner::getPathSize | ( | ) | [inline] |
Definition at line 201 of file FootstepPlanner.h.
State footstep_planner::FootstepPlanner::getStartFootLeft | ( | ) | [inline] |
Definition at line 203 of file FootstepPlanner.h.
Definition at line 204 of file FootstepPlanner.h.
void footstep_planner::FootstepPlanner::goalPoseCallback | ( | const geometry_msgs::PoseStampedConstPtr & | goal_pose | ) |
Callback to set the goal pose as a robot pose centered between two feet. If the start pose has been set previously the planning is started.
Subscribed to 'goal'.
Definition at line 608 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::mapCallback | ( | const nav_msgs::OccupancyGridConstPtr & | occupancy_map | ) |
bool footstep_planner::FootstepPlanner::pathExists | ( | ) | [inline] |
Definition at line 213 of file FootstepPlanner.h.
bool footstep_planner::FootstepPlanner::pathIsNew | ( | const std::vector< int > & | new_path | ) | [protected] |
Definition at line 919 of file FootstepPlanner.cpp.
Start a planning task from scratch (will delete information of previous planning tasks). Map and start, goal poses need to be set beforehand.
Definition at line 496 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::plan | ( | const geometry_msgs::PoseStampedConstPtr | start, |
const geometry_msgs::PoseStampedConstPtr | goal | ||
) |
Sets start, goal poses and calls FootstepPlanner::plan().
Definition at line 542 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::plan | ( | float | start_x, |
float | start_y, | ||
float | start_theta, | ||
float | goal_x, | ||
float | goal_y, | ||
float | goal_theta | ||
) |
Sets start, goal poses and calls FootstepPlanner::plan().
Definition at line 553 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::planService | ( | humanoid_nav_msgs::PlanFootsteps::Request & | req, |
humanoid_nav_msgs::PlanFootsteps::Response & | resp | ||
) |
Service handle to plan footsteps.
Definition at line 567 of file FootstepPlanner.cpp.
Starts a planning task based on previous planning information (note that this method can also be used when no previous planning was performed). Map and start, goal poses need to be set beforehand.
Definition at line 517 of file FootstepPlanner.cpp.
Reset the previous planning information.
Definition at line 467 of file FootstepPlanner.cpp.
Reset and reinitialize the environment.
Definition at line 483 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::run | ( | void | ) | [protected] |
Starts the planning task in the underlying SBPL.
NOTE: Never call this directly. Always use either plan() or replan() to invoke this method.
Definition at line 309 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::setGoal | ( | const geometry_msgs::PoseStampedConstPtr | goal_pose | ) |
Sets the goal pose as a robot pose centered between two feet.
Definition at line 665 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::setGoal | ( | float | x, |
float | y, | ||
float | theta | ||
) |
Sets the goal pose as a robot pose centered between two feet.
Definition at line 674 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::setMarkerNamespace | ( | const std::string & | ns | ) | [inline] |
Definition at line 138 of file FootstepPlanner.h.
void footstep_planner::FootstepPlanner::setMaxSearchTime | ( | int | search_time | ) | [inline] |
Set the maximal search time.
Definition at line 144 of file FootstepPlanner.h.
void footstep_planner::FootstepPlanner::setPlanner | ( | ) | [protected] |
Sets the planning algorithm used by SBPL.
Definition at line 278 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::setStart | ( | const geometry_msgs::PoseStampedConstPtr | start_pose | ) |
Sets the start pose as a robot pose centered between two feet.
Definition at line 704 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::setStart | ( | float | x, |
float | y, | ||
float | theta | ||
) |
Sets the start pose as a robot pose centered between two feet.
Definition at line 731 of file FootstepPlanner.cpp.
bool footstep_planner::FootstepPlanner::setStart | ( | const State & | left_foot, |
const State & | right_foot | ||
) |
Sets the start pose as position of left and right footsteps.
Definition at line 713 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::startPoseCallback | ( | const geometry_msgs::PoseWithCovarianceStampedConstPtr & | start_pose | ) |
Callback to set the start pose as a robot pose centered between two feet. If the goal pose has been set previously the planning is started.
Subscribed to 'initialpose'.
Definition at line 628 of file FootstepPlanner.cpp.
void footstep_planner::FootstepPlanner::updateEnvironment | ( | const gridmap_2d::GridMap2DPtr | old_map | ) | [protected] |
Updates the environment in case of a changed map.
Definition at line 786 of file FootstepPlanner.cpp.
Updates the map in the planning environment.
Definition at line 764 of file FootstepPlanner.cpp.
int footstep_planner::FootstepPlanner::ivChangedCellsLimit [protected] |
If limit of changed cells is reached the planner starts a new task from the scratch.
Definition at line 295 of file FootstepPlanner.h.
int footstep_planner::FootstepPlanner::ivCollisionCheckAccuracy [protected] |
Definition at line 282 of file FootstepPlanner.h.
Planning parameters.
Definition at line 213 of file FootstepPlanner.h.
Definition at line 271 of file FootstepPlanner.h.
double footstep_planner::FootstepPlanner::ivFootSeparation [protected] |
Definition at line 280 of file FootstepPlanner.h.
Definition at line 272 of file FootstepPlanner.h.
Definition at line 278 of file FootstepPlanner.h.
Definition at line 268 of file FootstepPlanner.h.
Definition at line 269 of file FootstepPlanner.h.
Definition at line 284 of file FootstepPlanner.h.
Definition at line 274 of file FootstepPlanner.h.
Definition at line 275 of file FootstepPlanner.h.
double footstep_planner::FootstepPlanner::ivInitialEpsilon [protected] |
Definition at line 289 of file FootstepPlanner.h.
int footstep_planner::FootstepPlanner::ivLastMarkerMsgSize [protected] |
Definition at line 285 of file FootstepPlanner.h.
Definition at line 259 of file FootstepPlanner.h.
std::string footstep_planner::FootstepPlanner::ivMarkerNamespace [protected] |
Definition at line 298 of file FootstepPlanner.h.
double footstep_planner::FootstepPlanner::ivMaxSearchTime [protected] |
Definition at line 288 of file FootstepPlanner.h.
double footstep_planner::FootstepPlanner::ivMaxStepWidth [protected] |
Definition at line 281 of file FootstepPlanner.h.
std::vector<State> footstep_planner::FootstepPlanner::ivPath [protected] |
Definition at line 264 of file FootstepPlanner.h.
double footstep_planner::FootstepPlanner::ivPathCost [protected] |
Definition at line 286 of file FootstepPlanner.h.
boost::shared_ptr<const PathCostHeuristic> footstep_planner::FootstepPlanner::ivPathCostHeuristicPtr [protected] |
Definition at line 262 of file FootstepPlanner.h.
Definition at line 276 of file FootstepPlanner.h.
boost::shared_ptr<FootstepPlannerEnvironment> footstep_planner::FootstepPlanner::ivPlannerEnvironmentPtr [protected] |
Definition at line 258 of file FootstepPlanner.h.
boost::shared_ptr<SBPLPlanner> footstep_planner::FootstepPlanner::ivPlannerPtr [protected] |
Definition at line 260 of file FootstepPlanner.h.
std::string footstep_planner::FootstepPlanner::ivPlannerType [protected] |
Definition at line 297 of file FootstepPlanner.h.
std::vector<int> footstep_planner::FootstepPlanner::ivPlanningStatesIds [protected] |
Definition at line 300 of file FootstepPlanner.h.
Definition at line 273 of file FootstepPlanner.h.
Definition at line 287 of file FootstepPlanner.h.
Definition at line 266 of file FootstepPlanner.h.
Definition at line 267 of file FootstepPlanner.h.
Definition at line 284 of file FootstepPlanner.h.
Definition at line 277 of file FootstepPlanner.h.