Classes | Public Types | Public Member Functions | Private Member Functions | Private Attributes | List of all members
planner_cspace::planner_3d::StartPosePredictor Class Reference

#include <start_pose_predictor.h>

Classes

struct  Config
 

Public Types

using Astar = GridAstar< 3, 2 >
 

Public Member Functions

void clear ()
 
const nav_msgs::Path & getPreservedPath () const
 
const double getPreservedPathLength () const
 
bool process (const geometry_msgs::Pose &robot_pose, const GridAstar< 3, 2 >::Gridmap< char, 0x40 > &cm, const costmap_cspace_msgs::MapMetaData3D &map_info, const nav_msgs::Path &previous_path_msg, Astar::Vec &result_start_grid)
 
void setConfig (const Config &config)
 

Private Member Functions

bool buildResults (const trajectory_tracker::Path2D::ConstIterator &expected_start_pose_it, const geometry_msgs::Pose &start_metric, const GridAstar< 3, 2 >::Gridmap< char, 0x40 > &cm, Astar::Vec &start_grid)
 
trajectory_tracker::Path2D::ConstIterator getExpectedPose (const std::vector< double > &etas) const
 
double getInitialETA (const geometry_msgs::Pose &robot_pose, const trajectory_tracker::Pose2D &initial_path_pose) const
 
trajectory_tracker::Path2D::ConstIterator getSwitchBack (const std::vector< double > &etas) const
 
bool isGridCenter (const trajectory_tracker::Pose2D &pose) const
 
bool isPathColliding (const trajectory_tracker::Path2D::ConstIterator &begin, const trajectory_tracker::Path2D::ConstIterator &end, const GridAstar< 3, 2 >::Gridmap< char, 0x40 > &cm)
 
bool removeAlreadyPassed (const geometry_msgs::Pose &robot_pose)
 

Private Attributes

Config config_
 
costmap_cspace_msgs::MapMetaData3D map_info_
 
nav_msgs::Path preserved_path_
 
double preserved_path_length_ = 0.0
 
trajectory_tracker::Path2D previous_path_2d_
 

Detailed Description

Definition at line 47 of file start_pose_predictor.h.

Member Typedef Documentation

◆ Astar

Definition at line 50 of file start_pose_predictor.h.

Member Function Documentation

◆ buildResults()

bool planner_cspace::planner_3d::StartPosePredictor::buildResults ( const trajectory_tracker::Path2D::ConstIterator expected_start_pose_it,
const geometry_msgs::Pose start_metric,
const GridAstar< 3, 2 >::Gridmap< char, 0x40 > &  cm,
Astar::Vec start_grid 
)
private

Definition at line 132 of file start_pose_predictor.cpp.

◆ clear()

void planner_cspace::planner_3d::StartPosePredictor::clear ( )

Definition at line 76 of file start_pose_predictor.cpp.

◆ getExpectedPose()

trajectory_tracker::Path2D::ConstIterator planner_cspace::planner_3d::StartPosePredictor::getExpectedPose ( const std::vector< double > &  etas) const
private

Definition at line 230 of file start_pose_predictor.cpp.

◆ getInitialETA()

double planner_cspace::planner_3d::StartPosePredictor::getInitialETA ( const geometry_msgs::Pose robot_pose,
const trajectory_tracker::Pose2D initial_path_pose 
) const
private

Definition at line 122 of file start_pose_predictor.cpp.

◆ getPreservedPath()

const nav_msgs::Path& planner_cspace::planner_3d::StartPosePredictor::getPreservedPath ( ) const
inline

Definition at line 71 of file start_pose_predictor.h.

◆ getPreservedPathLength()

const double planner_cspace::planner_3d::StartPosePredictor::getPreservedPathLength ( ) const
inline

Definition at line 75 of file start_pose_predictor.h.

◆ getSwitchBack()

trajectory_tracker::Path2D::ConstIterator planner_cspace::planner_3d::StartPosePredictor::getSwitchBack ( const std::vector< double > &  etas) const
private

Definition at line 194 of file start_pose_predictor.cpp.

◆ isGridCenter()

bool planner_cspace::planner_3d::StartPosePredictor::isGridCenter ( const trajectory_tracker::Pose2D pose) const
private

Definition at line 165 of file start_pose_predictor.cpp.

◆ isPathColliding()

bool planner_cspace::planner_3d::StartPosePredictor::isPathColliding ( const trajectory_tracker::Path2D::ConstIterator begin,
const trajectory_tracker::Path2D::ConstIterator end,
const GridAstar< 3, 2 >::Gridmap< char, 0x40 > &  cm 
)
private

Definition at line 176 of file start_pose_predictor.cpp.

◆ process()

bool planner_cspace::planner_3d::StartPosePredictor::process ( const geometry_msgs::Pose robot_pose,
const GridAstar< 3, 2 >::Gridmap< char, 0x40 > &  cm,
const costmap_cspace_msgs::MapMetaData3D &  map_info,
const nav_msgs::Path &  previous_path_msg,
Astar::Vec result_start_grid 
)

Definition at line 42 of file start_pose_predictor.cpp.

◆ removeAlreadyPassed()

bool planner_cspace::planner_3d::StartPosePredictor::removeAlreadyPassed ( const geometry_msgs::Pose robot_pose)
private

Definition at line 83 of file start_pose_predictor.cpp.

◆ setConfig()

void planner_cspace::planner_3d::StartPosePredictor::setConfig ( const Config config)
inline

Definition at line 60 of file start_pose_predictor.h.

Member Data Documentation

◆ config_

Config planner_cspace::planner_3d::StartPosePredictor::config_
private

Definition at line 95 of file start_pose_predictor.h.

◆ map_info_

costmap_cspace_msgs::MapMetaData3D planner_cspace::planner_3d::StartPosePredictor::map_info_
private

Definition at line 96 of file start_pose_predictor.h.

◆ preserved_path_

nav_msgs::Path planner_cspace::planner_3d::StartPosePredictor::preserved_path_
private

Definition at line 98 of file start_pose_predictor.h.

◆ preserved_path_length_

double planner_cspace::planner_3d::StartPosePredictor::preserved_path_length_ = 0.0
private

Definition at line 99 of file start_pose_predictor.h.

◆ previous_path_2d_

trajectory_tracker::Path2D planner_cspace::planner_3d::StartPosePredictor::previous_path_2d_
private

Definition at line 97 of file start_pose_predictor.h.


The documentation for this class was generated from the following files:


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Fri May 16 2025 02:15:23