forked from arpg/CarPlanner
-
Notifications
You must be signed in to change notification settings - Fork 0
/
CarController.h
87 lines (62 loc) · 2.78 KB
/
CarController.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
#ifndef _CAR_CONTROLLER_H_
#define _CAR_CONTROLLER_H_
#include "CarPlannerCommon.h"
#include "CVarHelpers.h"
#include "LocalPlanner.h"
typedef std::list<ControlPlan*> PlanPtrList;
///////////////////////////////////////////////////////////////////////////////
class CarController
{
public:
CarController();
void Init(std::vector<MotionSample> &segmentSamples, LocalPlanner *pPlanner, BulletCarModel *pModel, double dt) ;
void Reset();
void GetCurrentCommands(const double time, ControlCommand &command);
VehicleState GetCurrentPose();
void SetCurrentPoseFromCarModel(BulletCarModel* pModel, int nWorldId);
void SetCurrentPose(VehicleState pose, CommandList* pCommandList = NULL);
void GetCurrentCommands(const double time,
ControlCommand& command,
Eigen::Vector3d& targetVel,
Sophus::SE3d &dT_target);
float* GetMaxControlPlanTimePtr(){ return &m_dMaxControlPlanTime; }
float* GetLookaheadTimePtr(){ return &m_dLookaheadTime; }
double GetLastPlanStartTime();
bool PlanControl(double dPlanStartTime, ControlPlan*& pPlanOut);
static double AdjustStartingSample(const std::vector<MotionSample>& segmentSamples,
VehicleState& state,
int& segmentIndex,
int& sampleIndex,
int lowerLimit = 100,
int upperLimit = 100);
static void PrepareLookaheadTrajectory(const std::vector<MotionSample>& vSegmentSamples,
ControlPlan *pPlan, VelocityProfile &trajectoryProfile, MotionSample &trajectorySample, const double dLookaheadTime);
private:
bool _SampleControlPlan(ControlPlan* pPlan,LocalProblem& problem);
bool _SolveControlPlan(const ControlPlan* pPlan, LocalProblem& problem, const MotionSample &trajectory);
void _GetCurrentPlanIndex(double currentTime, PlanPtrList::iterator& planIndex, int& sampleIndex, double& interpolationAmount);
int m_nLastCurrentPlanIndex;
VehicleState m_CurrentState;
BulletCarModel* m_pModel;
LocalPlanner* m_pPlanner;
ControlCommand m_LastCommand;
double m_dStartTime;
double m_dt;
bool m_bStarted;
bool m_bStopping;
bool m_bPoseUpdated;
bool m_bFirstPose;
float& m_dMaxControlPlanTime;
float& m_dLookaheadTime;
boost::thread* m_pControlPlannerThread;
PlanPtrList m_lControlPlans;
std::vector<MotionSample> m_vSegmentSamples;
CommandList m_lCurrentCommands;
MotionSample m_MotionSample2dOnly;
boost::mutex m_PlanMutex;
boost::mutex m_PoseMutex;
Eigen::Vector5d m_dLastDelta;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#endif