ugv_nav4d
Planner.hpp
Go to the documentation of this file.
1#pragma once
2#include <base/samples/RigidBodyState.hpp>
3#include <sbpl_spline_primitives/SbplSplineMotionPrimitives.hpp>
5#include <trajectory_follower/SubTrajectory.hpp>
6#include "PlannerConfig.hpp"
7
8#include <memory>
9
10class ARAPlanner;
11
12namespace ugv_nav4d
13{
14
15class PlannerDump;
16
18{
19protected:
20 friend class PlannerDump;
21 std::shared_ptr<EnvironmentXYZTheta> env;
22 std::shared_ptr<ARAPlanner> planner;
23
24 const sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig;
26 traversability_generator3d::TraversabilityConfig traversabilityConfig;
28 std::vector<int> solutionIds;
29
31 std::vector<Eigen::Vector3d> previousStartPositions;
32
33public:
42
43 Planner(const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig,
44 const traversability_generator3d::TraversabilityConfig &traversabilityConfig,
45 const Mobility& mobility,
47
48 void updateMap(const traversability_generator3d::TravMap3d &map)
49 {
50 std::shared_ptr<traversability_generator3d::TravMap3d> mapPtr = std::make_shared<traversability_generator3d::TravMap3d>(map);
51
52 if(!env)
53 {
55 }
56 else
57 {
58 env->updateMap(mapPtr);
59 }
60 }
61
62 void enablePathStatistics(bool enable);
63
64 std::vector<Motion> getMotions() const;
65
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);
102
103 void setTravConfig(const traversability_generator3d::TraversabilityConfig& config);
104
105 void setPlannerConfig(const PlannerConfig& config);
106
107 const std::shared_ptr<const traversability_generator3d::TravMap3d> getTraversabilityMap() const;
108 std::shared_ptr<trajectory_follower::SubTrajectory> findTrajectoryOutOfObstacle(const Eigen::Vector3d& start, double theta,
109 const Eigen::Affine3d& ground2Body, bool setZToZero);
110
111 private:
112 bool calculateGoal(Eigen::Vector3d& goal_translation, const double yaw) noexcept;
113 bool tryGoal(const Eigen::Vector3d& translation, const double yaw) noexcept;
114
115};
116
117}
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