#include <footstep_graph.h>
Public Types | |
typedef boost::function< double(StatePtr, StatePtr, double) > | PathCostFunction |
typedef boost::shared_ptr< FootstepGraph > | Ptr |
typedef boost::function< bool(StatePtr target_state, std::vector< StatePtr > &) > | SuccessorFunction |
Public Types inherited from jsk_footstep_planner::Graph< FootstepState > | |
typedef boost::shared_ptr< Graph > | Ptr |
typedef boost::shared_ptr< StateT > | StatePtr |
typedef FootstepState | StateT |
Public Member Functions | |
virtual void | clearPerceptionDuration () |
bool | finalizeSteps (const StatePtr &last_1_Step, const StatePtr &lastStep, std::vector< StatePtr > &finalizeSteps) |
FootstepGraph (const Eigen::Vector3f &resolution, const bool use_pointcloud_model=false, const bool lazy_projection=true, const bool local_movement=false, const bool use_obstacle_model=false) | |
virtual TransitionLimit::Ptr | getGlobalTransitionLimit () |
virtual FootstepState::Ptr | getGoal (int leg) |
virtual ros::WallDuration | getPerceptionDuration () |
virtual pcl::PointIndices::Ptr | getPointIndicesCollidingSphere (const Eigen::Affine3f &c) |
virtual Eigen::Affine3f | getRobotCoords (StatePtr current_state, StatePtr previous_state) const |
Compute robot coords from current footstep and previous footstep. R_robot = midcoords(F_current, F_previous) * R_offset;. More... | |
virtual TransitionLimit::Ptr | getTransitionLimit () |
virtual std::string | infoString () const |
return string about graph information. More... | |
virtual bool | isColliding (StatePtr current_state, StatePtr previous_state) |
return True if current_state collides with obstacle. More... | |
virtual bool | isCollidingBox (const Eigen::Affine3f &c, pcl::PointIndices::Ptr candidates) const |
virtual bool | isGoal (StatePtr state) |
virtual bool | isSuccessable (StatePtr current_state, StatePtr previous_state) |
virtual bool | lazyProjection () const |
virtual std::vector< FootstepState::Ptr > | localMoveFootstepState (FootstepState::Ptr in) |
virtual bool | localMovement () const |
virtual double | maxSuccessorDistance () |
virtual double | maxSuccessorRotation () |
double | path_cost_original (StatePtr from, StatePtr to, double prev_cost) |
virtual double | pathCost (StatePtr from, StatePtr to, double prev_cost) |
virtual FootstepState::Ptr | projectFootstep (FootstepState::Ptr in) |
virtual FootstepState::Ptr | projectFootstep (FootstepState::Ptr in, unsigned int &state) |
virtual bool | projectGoal () |
virtual bool | projectStart () |
virtual void | setBasicSuccessors (std::vector< Eigen::Affine3f > left_to_right_successors) |
virtual void | setCollisionBBoxOffset (const Eigen::Affine3f &offset) |
virtual void | setCollisionBBoxSize (const Eigen::Vector3f &size) |
virtual void | setGlobalTransitionLimit (TransitionLimit::Ptr limit) |
virtual void | setGoalState (FootstepState::Ptr left, FootstepState::Ptr right) |
void | setHeuristicPathLine (jsk_recognition_utils::PolyLine &path_line) |
virtual void | setLeftGoalState (FootstepState::Ptr goal) |
virtual void | setObstacleModel (pcl::PointCloud< pcl::PointXYZ >::Ptr model) |
virtual void | setParameters (FootstepParameters &p) |
virtual void | setPathCostFunction (PathCostFunction p) |
virtual void | setPointCloudModel (pcl::PointCloud< pcl::PointNormal >::Ptr model) |
virtual void | setProgressPublisher (ros::NodeHandle &nh, std::string topic) |
virtual void | setRightGoalState (FootstepState::Ptr goal) |
virtual void | setSuccessorFunction (SuccessorFunction s) |
virtual void | setTransitionLimit (TransitionLimit::Ptr limit) |
virtual std::vector< StatePtr > | successors (StatePtr target_state) |
bool | successors_original (StatePtr target_state, std::vector< FootstepGraph::StatePtr > &ret) |
virtual bool | useObstacleModel () const |
virtual bool | usePointCloudModel () const |
Public Member Functions inherited from jsk_footstep_planner::Graph< FootstepState > | |
virtual void | addNode (StatePtr state) |
virtual StatePtr | getGoalState () |
virtual StatePtr | getStartState () |
Graph () | |
virtual size_t | numNodes () |
virtual void | setGoalState (StatePtr goal) |
virtual void | setStartState (StatePtr start) |
Private Attributes | |
PathCostFunction | path_cost_func_ |
SuccessorFunction | successor_func_ |
Friends | |
double | footstepHeuristicFollowPathLine (SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph) |
Definition at line 86 of file footstep_graph.h.
typedef boost::function< double(StatePtr, StatePtr, double) > jsk_footstep_planner::FootstepGraph::PathCostFunction |
Definition at line 123 of file footstep_graph.h.
Definition at line 121 of file footstep_graph.h.
typedef boost::function< bool(StatePtr target_state, std::vector<StatePtr> &) > jsk_footstep_planner::FootstepGraph::SuccessorFunction |
Definition at line 122 of file footstep_graph.h.
|
inline |
Definition at line 127 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 275 of file footstep_graph.h.
bool jsk_footstep_planner::FootstepGraph::finalizeSteps | ( | const StatePtr & | last_1_Step, |
const StatePtr & | lastStep, | ||
std::vector< StatePtr > & | finalizeSteps | ||
) |
Definition at line 186 of file footstep_graph.cpp.
|
inlinevirtual |
Definition at line 271 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 207 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 274 of file footstep_graph.h.
|
virtual |
Definition at line 139 of file footstep_graph.cpp.
|
virtual |
Compute robot coords from current footstep and previous footstep. R_robot = midcoords(F_current, F_previous) * R_offset;.
Definition at line 133 of file footstep_graph.cpp.
|
inlinevirtual |
Definition at line 269 of file footstep_graph.h.
|
virtual |
return string about graph information.
Definition at line 200 of file footstep_graph.cpp.
|
virtual |
return True if current_state collides with obstacle.
Definition at line 169 of file footstep_graph.cpp.
|
virtual |
Definition at line 150 of file footstep_graph.cpp.
|
virtual |
Implements jsk_footstep_planner::Graph< FootstepState >.
Definition at line 103 of file footstep_graph.cpp.
|
virtual |
Definition at line 246 of file footstep_graph.cpp.
|
inlinevirtual |
Definition at line 264 of file footstep_graph.h.
|
virtual |
Definition at line 265 of file footstep_graph.cpp.
|
inlinevirtual |
Definition at line 265 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 220 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 224 of file footstep_graph.h.
|
inline |
Definition at line 287 of file footstep_graph.h.
|
inlinevirtual |
Reimplemented from jsk_footstep_planner::Graph< FootstepState >.
Definition at line 160 of file footstep_graph.h.
|
virtual |
Definition at line 356 of file footstep_graph.cpp.
|
virtual |
Definition at line 362 of file footstep_graph.cpp.
|
virtual |
Definition at line 382 of file footstep_graph.cpp.
|
virtual |
Definition at line 404 of file footstep_graph.cpp.
|
virtual |
Definition at line 73 of file footstep_graph.cpp.
|
inlinevirtual |
Definition at line 277 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 278 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 270 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 181 of file footstep_graph.h.
|
inline |
Definition at line 291 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 188 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 253 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 267 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 283 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 235 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 229 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 193 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 280 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 268 of file footstep_graph.h.
|
inlinevirtual |
Implements jsk_footstep_planner::Graph< FootstepState >.
Definition at line 152 of file footstep_graph.h.
bool jsk_footstep_planner::FootstepGraph::successors_original | ( | StatePtr | target_state, |
std::vector< FootstepGraph::StatePtr > & | ret | ||
) |
Definition at line 306 of file footstep_graph.cpp.
|
inlinevirtual |
Definition at line 262 of file footstep_graph.h.
|
inlinevirtual |
Definition at line 263 of file footstep_graph.h.
|
friend |
|
protected |
Definition at line 313 of file footstep_graph.h.
|
protected |
Definition at line 314 of file footstep_graph.h.
|
protected |
Definition at line 327 of file footstep_graph.h.
|
protected |
Definition at line 303 of file footstep_graph.h.
|
protected |
Definition at line 333 of file footstep_graph.h.
|
protected |
Definition at line 321 of file footstep_graph.h.
|
protected |
Definition at line 306 of file footstep_graph.h.
|
protected |
Definition at line 322 of file footstep_graph.h.
|
protected |
Definition at line 315 of file footstep_graph.h.
|
protected |
Definition at line 316 of file footstep_graph.h.
|
protected |
Definition at line 297 of file footstep_graph.h.
|
protected |
Definition at line 300 of file footstep_graph.h.
|
protected |
Definition at line 328 of file footstep_graph.h.
|
private |
Definition at line 336 of file footstep_graph.h.
|
protected |
Definition at line 330 of file footstep_graph.h.
|
protected |
Definition at line 296 of file footstep_graph.h.
|
protected |
Definition at line 298 of file footstep_graph.h.
|
protected |
Definition at line 318 of file footstep_graph.h.
|
protected |
Definition at line 317 of file footstep_graph.h.
|
protected |
Definition at line 324 of file footstep_graph.h.
|
protected |
Definition at line 307 of file footstep_graph.h.
|
private |
Definition at line 335 of file footstep_graph.h.
|
protected |
Definition at line 304 of file footstep_graph.h.
|
protected |
Definition at line 305 of file footstep_graph.h.
|
protected |
Definition at line 326 of file footstep_graph.h.
|
protected |
Definition at line 299 of file footstep_graph.h.
|
protected |
Definition at line 302 of file footstep_graph.h.
|
protected |
Definition at line 323 of file footstep_graph.h.
|
protected |
Definition at line 320 of file footstep_graph.h.
|
protected |
zero_state is used only for global transition limit
Definition at line 312 of file footstep_graph.h.