Skip to content

Commit 7c3efbd

Browse files
HeenneHeenne
committed
Implemented minimal curvation #14
- Made scaling of tangent dynamic - Implemented function that increases the tangent to probide mininmal curves
1 parent cefb2fd commit 7c3efbd

File tree

6 files changed

+194
-28
lines changed

6 files changed

+194
-28
lines changed

‎fpp_launch/config/global_planner_params.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ FormationPathPlanner:
5555
end_straight_distance: 1.0
5656
# Every nth cell the cell of the RelaxedAStar planner will be used to generate splines
5757
# Every nth cell will act as a control point
58-
control_point_distance: 50
58+
control_point_distance: 80
5959
# This value defines how many points should be inserted into the plan from one spline.
6060
# F.e. control_point_distance = 50. With 0.05m per cell the control points are spaced every 2.5m.
6161
# With a planning_points_per_spline value of 20 this would generate 30 points into the plan that are in between two control points.

‎fpp_ros/include/fpp_ros/path_planner/cubic_bezier_splines.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,14 @@ namespace path_planner
4747
Eigen::Vector2f getStartPose();
4848
Eigen::Vector2f getEndPose();
4949
Eigen::Vector2f getStartTangent();
50+
Eigen::Vector2f getMultipliedStartTangent();
5051
Eigen::Vector2f getEndTangent();
52+
Eigen::Vector2f getMultipliedEndTangent();
53+
54+
void setStartTangentMagnitude(float start_tangent_magnitude);
55+
void setEndTangentMagnitude(float end_tangent_magnitude);
56+
float getStartTangentMagnitude();
57+
float getEndTangentMagnitude();
5158

5259
private:
5360
float calcStartToEndLength();
@@ -79,7 +86,13 @@ namespace path_planner
7986
Eigen::Vector2f cp1_;
8087
Eigen::Vector2f cp2_;
8188

89+
//! Stores the default value for the start tangent without any factor applied.
8290
Eigen::Vector2f start_tangent_;
91+
//! Stores the default value for the end tangent without any factor applied
8392
Eigen::Vector2f end_tangent_;
93+
//! Stores the dynamically changable parameter for changing the length of the start tangent
94+
float start_tangent_magnitude_;
95+
//! Stores the dynamically changable parameter for changing the length of the end tangent
96+
float end_tangent_magnitude_;
8497
};
8598
}

‎fpp_ros/include/fpp_ros/path_planner/quintic_bezier_splines.h‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,10 +29,12 @@ namespace path_planner
2929

3030
void setStartTangent(tf::Quaternion robot_orientation);
3131
void setStartTangent(Eigen::Vector2f start_tangent);
32+
// void setMultipliedStartTangent(Eigen::Vector2f start_tangent, float start_tangent_magnitude);
3233

3334
void setEndTangent(tf::Quaternion robot_end_orientation);
3435
void setEndTangent(Eigen::Vector2f end_tangent);
3536
void setEndTangentByNextPose(Eigen::Vector2f next_pose);
37+
// void setMultipliedEndTangent(Eigen::Vector2f end_tangent, float end_tangent_magnitude);
3638

3739
void calcControlPoints();
3840
Eigen::Vector2f calcPointOnBezierSpline(float iterator);
@@ -48,6 +50,7 @@ namespace path_planner
4850
*
4951
* @param iterator
5052
* @param min_curve_radius
53+
* @param point_of_failure Iterator index where the curve is to tight
5154
* @return true Curve Radius is greater or equal to min curve radius. Curve is fine.
5255
* @return false Curve radius is smaller than min curve radius. Curve is not finde.
5356
*/
@@ -57,10 +60,21 @@ namespace path_planner
5760
*
5861
* @param resolution
5962
* @param min_curve_radius
63+
* @param point_of_failure Iterator index where the curve is to tight
6064
* @return true Curve Radius is greater or equal to min curve radius. Curve is fine.
6165
* @return false Curve radius is smaller than min curve radius. Curve is not finde.
6266
*/
6367
bool checkMinCurveRadiusOnSpline(int resolution, float min_curve_radius);
68+
/**
69+
* @brief
70+
*
71+
* @param resolution
72+
* @param min_curve_radius
73+
* @param point_of_failure Iterator index where the curve is to tight
74+
* @return true Curve Radius is greater or equal to min curve radius. Curve is fine.
75+
* @return false Curve radius is smaller than min curve radius. Curve is not finde.
76+
*/
77+
bool checkMinCurveRadiusOnSpline(int resolution, float min_curve_radius, int &point_of_failure);
6478

6579
void visualizeData();
6680
void addStartEndPointToVisuHelper();
@@ -71,7 +85,14 @@ namespace path_planner
7185
Eigen::Vector2f getStartPose();
7286
Eigen::Vector2f getEndPose();
7387
Eigen::Vector2f getStartTangent();
88+
Eigen::Vector2f getMultipliedStartTangent();
7489
Eigen::Vector2f getEndTangent();
90+
Eigen::Vector2f getMultipliedEndTangent();
91+
92+
void setStartTangentMagnitude(float start_tangent_magnitude);
93+
void setEndTangentMagnitude(float end_tangent_magnitude);
94+
float getStartTangentMagnitude();
95+
float getEndTangentMagnitude();
7596

7697
std::shared_ptr<path_planner::CubicBezierSplines> convertToCubicBezierSpline();
7798

@@ -107,7 +128,13 @@ namespace path_planner
107128
Eigen::Vector2f cp3_;
108129
Eigen::Vector2f cp4_;
109130

131+
//! Stores the default value for the start tangent without any factor applied.
110132
Eigen::Vector2f start_tangent_;
133+
//! Stores the default value for the end tangent without any factor applied
111134
Eigen::Vector2f end_tangent_;
135+
//! Stores the dynamically changable parameter for changing the length of the start tangent
136+
float start_tangent_magnitude_;
137+
//! Stores the dynamically changable parameter for changing the length of the end tangent
138+
float end_tangent_magnitude_;
112139
};
113140
}

‎fpp_ros/src/path_planner/cubic_bezier_splines.cpp‎

Lines changed: 40 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ namespace path_planner
3535

3636
tf::Vector3 direction_vector(1, 0, 0);
3737
tf::Vector3 rotated_vector = tf::quatRotate(robot_orientation, direction_vector);
38-
rotated_vector = 0.5 * rotated_vector * start_to_end_length; // see setEndTangent for 0.5 explanation
38+
rotated_vector = rotated_vector * start_to_end_length; // see setEndTangent for 0.5 explanation
3939
this->start_tangent_ << rotated_vector[0], rotated_vector[1];
4040
}
4141

@@ -75,7 +75,7 @@ namespace path_planner
7575
Eigen::Matrix<float, 2, 1> angular_bisector = normalized_diff_vector_end_to_next + normalized_diff_vector_start_to_end;
7676
angular_bisector.normalize();
7777
float length_diff_vector_end_to_next = diff_vector_end_to_next.norm(); // Use length of end point and next point to calculate the length of the tangent
78-
this->end_tangent_ = 2.0 * length_diff_vector_end_to_next * angular_bisector; // This 0.5 value is taken from the paper (p. 32 then links to Linear Geometry with Computer Graphics page 318)
78+
this->end_tangent_ = length_diff_vector_end_to_next * angular_bisector; // This 0.5 value is taken from the paper (p. 32 then links to Linear Geometry with Computer Graphics page 318)
7979
}
8080

8181
void CubicBezierSplines::visualizeData()
@@ -89,26 +89,56 @@ namespace path_planner
8989
ros::Duration(0.1).sleep(); // Wait for markers to be shown, maybe this helps to visualize them every time
9090
}
9191

92-
Eigen::Matrix<float, 2, 1> CubicBezierSplines::getStartPose()
92+
Eigen::Vector2f CubicBezierSplines::getStartPose()
9393
{
9494
return this->start_pose_;
9595
}
9696

97-
Eigen::Matrix<float, 2, 1> CubicBezierSplines::getEndPose()
97+
Eigen::Vector2f CubicBezierSplines::getEndPose()
9898
{
9999
return this->end_pose_;
100100
}
101101

102-
Eigen::Matrix<float, 2, 1> CubicBezierSplines::getStartTangent()
102+
Eigen::Vector2f CubicBezierSplines::getStartTangent()
103103
{
104104
return this->start_tangent_;
105105
}
106106

107-
Eigen::Matrix<float, 2, 1> CubicBezierSplines::getEndTangent()
107+
Eigen::Vector2f CubicBezierSplines::getMultipliedStartTangent()
108+
{
109+
return this->start_tangent_magnitude_ * this->start_tangent_;
110+
}
111+
112+
Eigen::Vector2f CubicBezierSplines::getEndTangent()
108113
{
109114
return this->end_tangent_;
110115
}
111116

117+
Eigen::Vector2f CubicBezierSplines::getMultipliedEndTangent()
118+
{
119+
return this->end_tangent_magnitude_ * this->end_tangent_;
120+
}
121+
122+
void CubicBezierSplines::setStartTangentMagnitude(float start_tangent_magnitude)
123+
{
124+
this->start_tangent_magnitude_ = start_tangent_magnitude;
125+
}
126+
127+
void CubicBezierSplines::setEndTangentMagnitude(float end_tangent_magnitude)
128+
{
129+
this->end_tangent_magnitude_ = end_tangent_magnitude;
130+
}
131+
132+
float CubicBezierSplines::getStartTangentMagnitude()
133+
{
134+
return this->start_tangent_magnitude_;
135+
}
136+
137+
float CubicBezierSplines::getEndTangentMagnitude()
138+
{
139+
return this->end_tangent_magnitude_;
140+
}
141+
112142
float CubicBezierSplines::calcStartToEndLength()
113143
{
114144
return std::sqrt(std::pow((this->end_pose_[0] - this->start_pose_[0]), 2) + std::pow((this->end_pose_[1] - this->start_pose_[1]), 2));
@@ -117,8 +147,8 @@ namespace path_planner
117147
void CubicBezierSplines::calcControlPoints()
118148
{
119149
// Ich glaube den Bruch hier kann ich anpassen um die Controlpoints weiter zu strecken
120-
this->cp1_ = (1.0 / 5.0) * this->start_tangent_ + this->start_pose_;
121-
this->cp2_ = -(1.0 / 5.0) * this->end_tangent_ + this->end_pose_;
150+
this->cp1_ = (1.0 / 5.0) * this->getMultipliedStartTangent() + this->start_pose_;
151+
this->cp2_ = -(1.0 / 5.0) * this->getMultipliedEndTangent() + this->end_pose_;
122152
}
123153

124154
Eigen::Matrix<float, 2, 1> CubicBezierSplines::calcPointOnBezierSpline(float iterator)
@@ -303,8 +333,8 @@ namespace path_planner
303333

304334
void CubicBezierSplines::addTangentsToVisuHelper()
305335
{
306-
this->addTangentToVisuHelper(this->start_pose_, this->start_tangent_);
307-
this->addTangentToVisuHelper(this->end_pose_, this->end_tangent_);
336+
this->addTangentToVisuHelper(this->start_pose_, this->getMultipliedStartTangent());
337+
this->addTangentToVisuHelper(this->end_pose_, this->getMultipliedEndTangent());
308338
}
309339

310340
void CubicBezierSplines::addTangentToVisuHelper(Eigen::Matrix<float, 2, 1> start_point, Eigen::Matrix<float, 2, 1> tangent)

‎fpp_ros/src/path_planner/quintic_bezier_splines.cpp‎

Lines changed: 70 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
namespace path_planner
44
{
55
QuinticBezierSplines::QuinticBezierSplines(visualization_helper::VisualizationHelper *visu_helper)
6-
: previous_spline_(nullptr), next_spline_(nullptr)
6+
: previous_spline_(nullptr), next_spline_(nullptr), start_tangent_magnitude_(0.25), end_tangent_magnitude_(0.25)
77
{
88
this->visu_helper_ = visu_helper;
99
this->initVisuHelper();
@@ -22,6 +22,7 @@ namespace path_planner
2222
{
2323
this->previous_spline_ = previous_spline;
2424
this->setStartTangent(this->previous_spline_->getEndTangent());
25+
this->setStartTangentMagnitude(this->previous_spline_->getEndTangentMagnitude());
2526
}
2627

2728
void QuinticBezierSplines::setNextSpline(const std::shared_ptr<QuinticBezierSplines> &next_spline)
@@ -36,7 +37,10 @@ namespace path_planner
3637

3738
tf::Vector3 direction_vector(1, 0, 0);
3839
tf::Vector3 rotated_vector = tf::quatRotate(robot_orientation, direction_vector);
39-
rotated_vector = 0.5 * rotated_vector * start_to_end_length; // see setEndTangent for 0.5 explanation
40+
// In the paper a factor of 0.5 was used to shorten the tangent. This will no longer be applied directly.
41+
// The factor will now be applied through the Poperty "getMultipliedStartTangent"
42+
// (p. 32 then links to Linear Geometry with Computer Graphics page 318)
43+
rotated_vector = rotated_vector * start_to_end_length;
4044
this->start_tangent_ << rotated_vector[0], rotated_vector[1];
4145

4246
ROS_INFO_STREAM("start_pose_tangent:" << this->start_tangent_[0] << " | " << this->start_tangent_[1]);
@@ -47,13 +51,19 @@ namespace path_planner
4751
this->start_tangent_ = start_tangent;
4852
}
4953

54+
// void QuinticBezierSplines::setMultipliedStartTangent(Eigen::Vector2f start_tangent, float start_tangent_magnitude)
55+
// {
56+
// this->start_tangent_ = start_tangent;
57+
// this->start_tangent_magnitude_ = start_tangent_magnitude;
58+
// }
59+
5060
void QuinticBezierSplines::setEndTangent(tf::Quaternion robot_end_orientation)
5161
{
5262
float start_to_end_length = this->calcStartToEndLength();
5363

5464
tf::Vector3 direction_vector(1, 0, 0);
5565
tf::Vector3 rotated_vector = tf::quatRotate(robot_end_orientation, direction_vector);
56-
rotated_vector = 0.5 * rotated_vector * start_to_end_length;
66+
rotated_vector = rotated_vector * start_to_end_length;
5767
this->end_tangent_ << rotated_vector[0], rotated_vector[1];
5868

5969
ROS_INFO_STREAM("end_pose_tangent:" << this->end_tangent_[0] << " | " << this->end_tangent_[1]);
@@ -80,9 +90,18 @@ namespace path_planner
8090
Eigen::Vector2f angular_bisector = normalized_diff_vector_end_to_next + normalized_diff_vector_start_to_end;
8191
angular_bisector.normalize();
8292
float length_diff_vector_end_to_next = diff_vector_end_to_next.norm(); // Use length of end point and next point to calculate the length of the tangent
83-
this->end_tangent_ = 0.5 * length_diff_vector_end_to_next * angular_bisector; // This 0.5 value is taken from the paper (p. 32 then links to Linear Geometry with Computer Graphics page 318)
93+
// In the paper a factor of 0.5 was used to shorten the tangent. This will no longer be applied directly.
94+
// The factor will now be applied through the Poperty "getMultipliedEndTangent"
95+
// (p. 32 then links to Linear Geometry with Computer Graphics page 318)
96+
this->end_tangent_ = length_diff_vector_end_to_next * angular_bisector;
8497
}
8598

99+
// void QuinticBezierSplines::setMultipliedEndTangent(Eigen::Vector2f end_tangent, float end_tangent_magnitude)
100+
// {
101+
// this->end_tangent_ = end_tangent;
102+
// this->end_tangent_magnitude_ = end_tangent_magnitude;
103+
// }
104+
86105
void QuinticBezierSplines::visualizeData()
87106
{
88107
this->visu_helper_->visualizeMarkerArray(this->start_end_marker_identificator_);
@@ -109,11 +128,41 @@ namespace path_planner
109128
return this->start_tangent_;
110129
}
111130

131+
Eigen::Vector2f QuinticBezierSplines::getMultipliedStartTangent()
132+
{
133+
return this->start_tangent_magnitude_ * this->start_tangent_;
134+
}
135+
112136
Eigen::Vector2f QuinticBezierSplines::getEndTangent()
113137
{
114138
return this->end_tangent_;
115139
}
116140

141+
Eigen::Vector2f QuinticBezierSplines::getMultipliedEndTangent()
142+
{
143+
return this->end_tangent_magnitude_ * this->end_tangent_;
144+
}
145+
146+
void QuinticBezierSplines::setStartTangentMagnitude(float start_tangent_magnitude)
147+
{
148+
this->start_tangent_magnitude_ = start_tangent_magnitude;
149+
}
150+
151+
void QuinticBezierSplines::setEndTangentMagnitude(float end_tangent_magnitude)
152+
{
153+
this->end_tangent_magnitude_ = end_tangent_magnitude;
154+
}
155+
156+
float QuinticBezierSplines::getStartTangentMagnitude()
157+
{
158+
return this->start_tangent_magnitude_;
159+
}
160+
161+
float QuinticBezierSplines::getEndTangentMagnitude()
162+
{
163+
return this->end_tangent_magnitude_;
164+
}
165+
117166
float QuinticBezierSplines::calcStartToEndLength()
118167
{
119168
return std::sqrt(std::pow((this->end_pose_[0] - this->start_pose_[0]), 2) + std::pow((this->end_pose_[1] - this->start_pose_[1]), 2));
@@ -122,8 +171,8 @@ namespace path_planner
122171
void QuinticBezierSplines::calcControlPoints()
123172
{
124173
// This formula comes from S'(0) and S'(1). Has to be (1/5)
125-
this->cp1_ = (1.0 / 5.0) * this->start_tangent_ + this->start_pose_;
126-
this->cp4_ = -(1.0 / 5.0) * this->end_tangent_ + this->end_pose_;
174+
this->cp1_ = (1.0 / 5.0) * this->getMultipliedStartTangent() + this->start_pose_;
175+
this->cp4_ = -(1.0 / 5.0) * this->getMultipliedEndTangent() + this->end_pose_;
127176

128177
std::shared_ptr<path_planner::CubicBezierSplines> previous_spline = nullptr;
129178
std::shared_ptr<path_planner::CubicBezierSplines> current_spline = nullptr;
@@ -144,6 +193,7 @@ namespace path_planner
144193
previous_spline->setStartTangent(this->previous_spline_->getStartTangent());
145194
previous_spline->setNextSpline(current_spline);
146195
previous_spline->calcControlPoints(); // Everything is set for the previous spline. Control points can be calculated
196+
147197
current_spline->setPreviousSpline(previous_spline);
148198
}
149199
else
@@ -226,14 +276,7 @@ namespace path_planner
226276
result_vector << 0, 0;
227277
for(int counter = 0; counter < point_matrix.size(); counter++)
228278
{
229-
// ROS_INFO_STREAM("ITERATION");
230-
// ROS_INFO_STREAM(bernstein_polynom_vector[counter]);
231-
// ROS_INFO_STREAM(result_vector);
232-
// ROS_INFO_STREAM(point_matrix[counter]);
233-
// ROS_INFO_STREAM((bernstein_polynom_vector[counter] * point_matrix[counter]));
234-
// ROS_INFO_STREAM(result_vector);
235-
236-
result_vector = result_vector + (bernstein_polynom_vector[counter] * point_matrix[counter]);
279+
result_vector = result_vector + (bernstein_polynom_vector[counter] * point_matrix[counter]);
237280
}
238281
return result_vector;
239282
}
@@ -242,6 +285,11 @@ namespace path_planner
242285
{
243286
std::shared_ptr<path_planner::CubicBezierSplines> cubic_spline =
244287
std::make_shared<path_planner::CubicBezierSplines>(this->visu_helper_, this->start_pose_, this->end_pose_);
288+
289+
// Set the magnitudes as these are settings that affect the splines deeply
290+
cubic_spline->setStartTangentMagnitude(this->start_tangent_magnitude_);
291+
cubic_spline->setEndTangentMagnitude(this->end_tangent_magnitude_);
292+
245293
return cubic_spline;
246294
}
247295

@@ -311,13 +359,19 @@ namespace path_planner
311359

312360
bool QuinticBezierSplines::checkMinCurveRadiusOnSpline(int resolution, float min_curve_radius)
313361
{
314-
for(int counter = 0; counter <= resolution; counter++)
362+
int point_of_failure_dummy = 0;
363+
return this->checkMinCurveRadiusOnSpline(resolution, min_curve_radius, point_of_failure_dummy);
364+
}
365+
366+
bool QuinticBezierSplines::checkMinCurveRadiusOnSpline(int resolution, float min_curve_radius, int &point_of_failure)
367+
{
368+
for(point_of_failure = 0; point_of_failure <= resolution; point_of_failure++)
315369
{
316370
// ROS_INFO_STREAM("iterator: " << counter << " | " << float(counter) / float(resolution));
317371
// Eigen::Vector2f point = this->calcPointOnBezierSpline(float(counter) / float(resolution));
318372
// ROS_INFO_STREAM("x: " << point[0] << " y: " << point[1]);
319373

320-
if(!this->checkMinCurveRadiusAtPoint((float(counter) / float(resolution)), min_curve_radius))
374+
if(!this->checkMinCurveRadiusAtPoint((float(point_of_failure) / float(resolution)), min_curve_radius))
321375
{
322376
return false;
323377
}

0 commit comments

Comments
 (0)