2#include <base/samples/RigidBodyState.hpp>
3#include <sbpl_spline_primitives/SbplSplineMotionPrimitives.hpp>
5#include <trajectory_follower/SubTrajectory.hpp>
21 std::shared_ptr<EnvironmentXYZTheta>
env;
43 Planner(
const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig,
48 void updateMap(
const traversability_generator3d::TravMap3d &map)
50 std::shared_ptr<traversability_generator3d::TravMap3d> mapPtr = std::make_shared<traversability_generator3d::TravMap3d>(map);
58 env->updateMap(mapPtr);
99 PLANNING_RESULT plan(
const base::Time& maxTime,
const base::samples::RigidBodyState& start_pose,
100 const base::samples::RigidBodyState& end_pose, std::vector<trajectory_follower::SubTrajectory>& resultTrajectory2D,
101 std::vector<trajectory_follower::SubTrajectory>& resultTrajectory3D,
bool dumpOnError =
false,
bool dumpOnSuccess =
false);
103 void setTravConfig(
const traversability_generator3d::TraversabilityConfig& config);
107 const std::shared_ptr<const traversability_generator3d::TravMap3d>
getTraversabilityMap()
const;
109 const Eigen::Affine3d& ground2Body,
bool setZToZero);
112 bool calculateGoal(Eigen::Vector3d& goal_translation,
const double yaw)
noexcept;
113 bool tryGoal(
const Eigen::Vector3d& translation,
const double yaw)
noexcept;
Definition EnvironmentXYZTheta.hpp:24
Definition PlannerDump.hpp:17
Definition Planner.hpp:18
std::shared_ptr< ARAPlanner > planner
Definition Planner.hpp:22
const Mobility mobility
Definition Planner.hpp:25
traversability_generator3d::TraversabilityConfig traversabilityConfig
Definition Planner.hpp:26
void enablePathStatistics(bool enable)
Definition Planner.cpp:35
std::vector< int > solutionIds
Definition Planner.hpp:28
const std::shared_ptr< const traversability_generator3d::TravMap3d > getTraversabilityMap() const
Definition Planner.cpp:247
std::shared_ptr< EnvironmentXYZTheta > env
Definition Planner.hpp:21
std::vector< Eigen::Vector3d > previousStartPositions
Definition Planner.hpp:31
std::vector< Motion > getMotions() const
Definition Planner.cpp:242
PLANNING_RESULT
Definition Planner.hpp:34
@ INTERNAL_ERROR
Definition Planner.hpp:39
@ NO_MAP
Definition Planner.hpp:38
@ START_INVALID
Definition Planner.hpp:36
@ FOUND_SOLUTION
Definition Planner.hpp:40
@ GOAL_INVALID
Definition Planner.hpp:35
@ NO_SOLUTION
Definition Planner.hpp:37
void setTravConfig(const traversability_generator3d::TraversabilityConfig &config)
Definition Planner.cpp:270
PlannerConfig plannerConfig
Definition Planner.hpp:27
std::shared_ptr< trajectory_follower::SubTrajectory > findTrajectoryOutOfObstacle(const Eigen::Vector3d &start, double theta, const Eigen::Affine3d &ground2Body, bool setZToZero)
Definition Planner.cpp:252
PLANNING_RESULT plan(const base::Time &maxTime, const base::samples::RigidBodyState &start_pose, const base::samples::RigidBodyState &end_pose, std::vector< trajectory_follower::SubTrajectory > &resultTrajectory2D, std::vector< trajectory_follower::SubTrajectory > &resultTrajectory3D, bool dumpOnError=false, bool dumpOnSuccess=false)
Definition Planner.cpp:102
const sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig
Definition Planner.hpp:24
void updateMap(const traversability_generator3d::TravMap3d &map)
Definition Planner.hpp:48
void setPlannerConfig(const PlannerConfig &config)
Definition Planner.cpp:282
Definition Dijkstra.cpp:8
Definition Mobility.hpp:12
Definition PlannerConfig.hpp:8