forked from ohlsont/control-waypoint_navigation
-
Notifications
You must be signed in to change notification settings - Fork 1
/
WaypointNavigation.hpp
143 lines (115 loc) · 4.24 KB
/
WaypointNavigation.hpp
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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
/****************************************************************
*
* Copyright (c) 2016
*
* European Space Technology and Research Center
* ESTEC - European Space Agency
*
* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* Description: Library for pure-pursuit based path following
*
* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* Author: Jan Filip, email:jan.filip@esa.int, jan.filip2@gmail.com
* Supervised by: Martin Azkarate, email:martin.azkarate@esa.int
*
* Date of creation: Dec 2016
*
* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
#pragma once
#include <Eigen/Geometry>
#include "Waypoint.hpp"
#include "MotionCommand.h"
#include "BasePose.hpp"
#include "Time.hpp"
#include <vector>
namespace waypoint_navigation_lib
{
enum NavigationState
{
DRIVING = 0, // 0
ALIGNING, // 1
TARGET_REACHED, // 2
OUT_OF_BOUNDARIES, // 3
NO_TRAJECTORY, // 4
NO_POSE // 5
};
class WaypointNavigation
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
WaypointNavigation();
// Set positon and orientation, where to drive to
void setLookaheadPoint(base::Waypoint& pose);
// Set current orientation and position,
// with a validity check (to avoid NaNs from Vicon)
// If invalid pose is received, it is discarded and false is returned
bool setPose(base::Pose& pose);
// Sets the trajecory the robot should follow
// and calculates the distances between consecutive waypoints
void setTrajectory(std::vector<base::Waypoint*>& t);
// Returns the trajectory
const std::vector<base::Waypoint*>& getTrajectory() const { return trajectory; }
NavigationState getNavigationState();
void setNavigationState(NavigationState state);
const base::Waypoint* getLookaheadPoint();
bool update(proxy_library::MotionCommand& mc);
bool configure(double minR, double tv, double rv, double cr, double lad, bool backward);
bool configurePD(double P, double D, double saturation);
bool configureAlignment(double d_deadband, double d_saturation, double d_rotVec);
bool configureTol(double TolPos, double TolHeading);
// Calculates a motion command (Ackermann or Point turn)
// given the robot pose and DRIVING mode
void getMovementCommand(proxy_library::MotionCommand& mc);
bool getProgressOnSegment(int segmentNumber,
double& progress,
double& distAlong,
double& distPerpend);
double getLookaheadDistance();
void setCurrentSegment(int segmentNumber); // TESTING ONLY - TODO Remove
int getCurrentSegment();
double getTargetHeading();
double getHeadingError();
private:
NavigationState mNavigationState;
bool aligning;
bool targetSet;
bool poseSet;
bool newWaypoint;
bool finalPhase;
bool backwardPerimtted;
double minTurnRadius; // [m]
double maxDisplacementAckermannTurn;
double translationalVelocity;
double rotationalVelocity;
double corridor; // Allowed Distance perpendicular to path segment
double lookaheadDistance;
double distanceToPath;
double targetHeading;
// Alignment tolerances
double defaultTolHeading, defaultTolPos;
// Alignment controller
double alignment_deadband, alignment_saturation;
double headingErr, alignment_P, alignment_D;
bool pd_initialized;
base::Time tprev;
base::Pose curPose;
base::Waypoint targetPose;
std::vector<double>* distanceToNext;
std::vector<base::Waypoint*> trajectory;
base::Waypoint lookaheadPoint;
int currentSegment;
base::Vector2d w1, w2, l1, l2, xr;
// Helper function for setting values of base::Vector2d with X, Y of a trajectory waypoint
bool setSegmentWaypoint(base::Vector2d& waypoint, int indexSegment);
// Helper function for finding the closes point on the path segment
// from the current position of the robot
base::Vector2d getClosestPointOnPath();
void initilalizeCurrentSegment();
bool isInsideBoundaries(double& distAlong, double& distPerpend);
inline void wrapAngle(double& angle);
inline void saturation(double& value, double limit);
};
} // namespace waypoint_navigation_lib