Skip to content

Commit 5273d32

Browse files
papertowel123papertowel123
committed
Merge pull request #75 from logivations/improve_velocity_polygon_params
improve param read in velocity polygon
1 parent ab1a209 commit 5273d32

File tree

5 files changed

+154
-2
lines changed

5 files changed

+154
-2
lines changed

‎nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,12 @@ class Polygon
119119
* @return Time before collision in seconds
120120
*/
121121
double getTimeBeforeCollision() const;
122+
/**
123+
* @brief Obtains time step for robot movement simulation for current polygon.
124+
* Applicable for APPROACH model.
125+
* @return Simulation time step in seconds
126+
*/
127+
double getSimulationTimeStep() const;
122128

123129
/**
124130
* @brief Obtains minimum velocity before completly stopping.

‎nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ class VelocityPolygon : public Polygon
7070

7171
protected:
7272
/**
73-
* @brief Custom struc to store the parameters of the sub-polygon
73+
* @brief Custom struct to store the parameters of the sub-polygon
7474
* @param poly_ The points of the sub-polygon
7575
* @param velocity_polygon_name_ The name of the sub-polygon
7676
* @param linear_min_ The minimum linear velocity
@@ -79,6 +79,12 @@ class VelocityPolygon : public Polygon
7979
* @param theta_max_ The maximum angular velocity
8080
* @param direction_end_angle_ The end angle of the direction(For holonomic robot only)
8181
* @param direction_start_angle_ The start angle of the direction(For holonomic robot only)
82+
* @param slowdown_ratio_ Robot slowdown (share of its actual speed)
83+
* @param linear_limit_ Robot linear limit
84+
* @param angular_limit_ Robot angular limit
85+
* @param time_before_collision_ Time before collision in seconds
86+
* @param simulation_time_step_ Time step for robot movement simulation
87+
* @param min_vel_before_stop_ Minimum velocity before we fully stop
8288
*/
8389
struct SubPolygonParameter
8490
{
@@ -90,6 +96,12 @@ class VelocityPolygon : public Polygon
9096
double theta_max_;
9197
double direction_end_angle_;
9298
double direction_start_angle_;
99+
double slowdown_ratio_;
100+
double linear_limit_;
101+
double angular_limit_;
102+
double time_before_collision_;
103+
double simulation_time_step_;
104+
double min_vel_before_stop_;
93105
};
94106

95107
/**

‎nav2_collision_monitor/src/polygon.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -173,6 +173,11 @@ double Polygon::getTimeBeforeCollision() const
173173
return time_before_collision_;
174174
}
175175

176+
double Polygon::getSimulationTimeStep() const
177+
{
178+
return simulation_time_step_;
179+
}
180+
176181
double Polygon::getMinVelBeforeStop() const
177182
{
178183
return min_vel_before_stop_;

‎nav2_collision_monitor/src/velocity_polygon.cpp‎

Lines changed: 56 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -123,9 +123,56 @@ bool VelocityPolygon::getParameters(
123123
.as_double();
124124
}
125125

126+
double slowdown_ratio = 0.0;
127+
if (action_type_ == SLOWDOWN) {
128+
nav2_util::declare_parameter_if_not_declared(
129+
node, polygon_name_ + "." + velocity_polygon_name + ".slowdown_ratio", rclcpp::ParameterValue(
130+
0.5));
131+
slowdown_ratio = node->get_parameter(
132+
polygon_name_ + "." + velocity_polygon_name + ".slowdown_ratio").as_double();
133+
}
134+
135+
double linear_limit = 0.0;
136+
double angular_limit = 0.0;
137+
if (action_type_ == LIMIT) {
138+
nav2_util::declare_parameter_if_not_declared(
139+
node, polygon_name_ + "." + velocity_polygon_name + ".linear_limit", rclcpp::ParameterValue(
140+
0.5));
141+
linear_limit = node->get_parameter(
142+
polygon_name_ + "." + velocity_polygon_name + ".linear_limit").as_double();
143+
nav2_util::declare_parameter_if_not_declared(
144+
node, polygon_name_ + "." + velocity_polygon_name + ".angular_limit", rclcpp::ParameterValue(
145+
0.5));
146+
angular_limit = node->get_parameter(
147+
polygon_name_ + "." + velocity_polygon_name + ".angular_limit").as_double();
148+
}
149+
150+
double time_before_collision = 0.0;
151+
double simulation_time_step = 0.0;
152+
double min_vel_before_stop = 0.0;
153+
if (action_type_ == APPROACH) {
154+
nav2_util::declare_parameter_if_not_declared(
155+
node, polygon_name_ + "." + velocity_polygon_name + ".time_before_collision", rclcpp::ParameterValue(
156+
2.0));
157+
time_before_collision = node->get_parameter(
158+
polygon_name_ + "." + velocity_polygon_name + ".time_before_collision").as_double();
159+
nav2_util::declare_parameter_if_not_declared(
160+
node, polygon_name_ + "." + velocity_polygon_name + ".simulation_time_step", rclcpp::ParameterValue(
161+
0.1));
162+
simulation_time_step = node->get_parameter(
163+
polygon_name_ + "." + velocity_polygon_name + ".simulation_time_step").as_double();
164+
nav2_util::declare_parameter_if_not_declared(
165+
node, polygon_name_ + "." + velocity_polygon_name + ".min_vel_before_stop", rclcpp::ParameterValue(
166+
-1.0));
167+
min_vel_before_stop = node->get_parameter(
168+
polygon_name_ + "." + velocity_polygon_name + ".min_vel_before_stop").as_double();
169+
}
170+
126171
SubPolygonParameter sub_polygon = {
127172
poly, velocity_polygon_name, linear_min, linear_max, theta_min,
128-
theta_max, direction_end_angle, direction_start_angle};
173+
theta_max, direction_end_angle, direction_start_angle,
174+
slowdown_ratio, linear_limit, angular_limit, time_before_collision, simulation_time_step,
175+
min_vel_before_stop};
129176
sub_polygons_.push_back(sub_polygon);
130177
}
131178
} catch (const std::exception & ex) {
@@ -153,6 +200,14 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in)
153200
// p_s.z will remain 0.0
154201
polygon_.polygon.points.push_back(p_s);
155202
}
203+
204+
slowdown_ratio_ = sub_polygon.slowdown_ratio_;
205+
linear_limit_ = sub_polygon.linear_limit_;
206+
angular_limit_ = sub_polygon.angular_limit_;
207+
time_before_collision_ = sub_polygon.time_before_collision_;
208+
simulation_time_step_ = sub_polygon.simulation_time_step_;
209+
min_vel_before_stop_ = sub_polygon.min_vel_before_stop_;
210+
156211
return;
157212
}
158213
}

‎nav2_collision_monitor/test/velocity_polygons_test.cpp‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,13 @@ static const char RIGHT_POLYGON_STR[]{
6767
static const bool IS_HOLONOMIC{true};
6868
static const bool IS_NOT_HOLONOMIC{false};
6969
static const int MIN_POINTS{2};
70+
static const double SLOWDOWN_RATIO{0.25};
71+
static const double LINEAR_LIMIT{0.3};
72+
static const double ANGULAR_LIMIT{0.2};
73+
static const double TIME_BEFORE_COLLISION{2.0};
74+
static const double SIMULATION_TIME_STEP{0.05};
75+
static const double MIN_VEL_BEFORE_COLLISION{-0.5};
76+
7077
static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)};
7178

7279
class TestNode : public nav2_util::LifecycleNode
@@ -145,6 +152,10 @@ class Tester : public ::testing::Test
145152
protected:
146153
// Working with parameters
147154
void setCommonParameters(const std::string & polygon_name, const std::string & action_type);
155+
void addSlowdownParameters(const std::string & polygon_name);
156+
void addLimitParameters(const std::string & polygon_name);
157+
void addApproachParameters(const std::string & polygon_name);
158+
148159
void setVelocityPolygonParameters(const bool is_holonomic);
149160
void addPolygonVelocitySubPolygon(
150161
const std::string & sub_polygon_name,
@@ -211,6 +222,39 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st
211222
rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC));
212223
}
213224

225+
void Tester::addSlowdownParameters(const std::string & polygon_name)
226+
{
227+
test_node_->set_parameter(
228+
rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO));
229+
}
230+
231+
void Tester::addLimitParameters(const std::string & polygon_name)
232+
{
233+
test_node_->set_parameter(
234+
rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".linear_limit", LINEAR_LIMIT));
235+
236+
test_node_->set_parameter(
237+
rclcpp::Parameter(std::string(POLYGON_NAME) + "." + polygon_name + ".angular_limit", ANGULAR_LIMIT));
238+
}
239+
240+
void Tester::addApproachParameters(const std::string & polygon_name)
241+
{
242+
test_node_->set_parameter(
243+
rclcpp::Parameter(
244+
std::string(
245+
POLYGON_NAME) + "." + polygon_name + ".time_before_collision", TIME_BEFORE_COLLISION));
246+
247+
test_node_->set_parameter(
248+
rclcpp::Parameter(
249+
std::string(POLYGON_NAME) + "." + polygon_name + ".simulation_time_step",
250+
SIMULATION_TIME_STEP));
251+
252+
test_node_->set_parameter(
253+
rclcpp::Parameter(
254+
std::string(POLYGON_NAME) + "." + polygon_name + ".min_vel_before_stop",
255+
MIN_VEL_BEFORE_COLLISION));
256+
}
257+
214258
void Tester::setVelocityPolygonParameters(const bool is_holonomic)
215259
{
216260
test_node_->declare_parameter(
@@ -270,6 +314,7 @@ void Tester::setVelocityPolygonParameters(const bool is_holonomic)
270314
rclcpp::Parameter(std::string(POLYGON_NAME) + ".velocity_polygons", velocity_polygons));
271315
}
272316

317+
273318
void Tester::addPolygonVelocitySubPolygon(
274319
const std::string & sub_polygon_name,
275320
const double linear_min, const double linear_max,
@@ -560,6 +605,35 @@ TEST_F(Tester, testVelocityPolygonHolonomicVelocitySwitching)
560605
EXPECT_NEAR(poly[3].y, RIGHT_POLYGON[7], EPSILON);
561606
}
562607

608+
// TEST_F(Tester, testVelocityPolygonSlowdownParameters)
609+
// {
610+
// createVelocityPolygon("slowdown", IS_NOT_HOLONOMIC);
611+
// addSlowdownParameters(SUB_POLYGON_FORWARD_NAME);
612+
613+
// EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::SLOWDOWN);
614+
// EXPECT_NEAR(velocity_polygon_->getSlowdownRatio(), 0.25, EPSILON);
615+
// }
616+
617+
// TEST_F(Tester, testVelocityPolygonLimitParameters)
618+
// {
619+
// createVelocityPolygon("limit", IS_NOT_HOLONOMIC);
620+
// addLimitParameters(SUB_POLYGON_FORWARD_NAME);
621+
622+
// EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::LIMIT);
623+
// EXPECT_NEAR(velocity_polygon_->getLinearLimit(), 0.3, EPSILON);
624+
// EXPECT_NEAR(velocity_polygon_->getAngularLimit(), 0.2, EPSILON);
625+
// }
626+
627+
// TEST_F(Tester, testVelocityPolygonApproachParameters)
628+
// {
629+
// createVelocityPolygon("approach", IS_NOT_HOLONOMIC);
630+
// addApproachParameters(SUB_POLYGON_FORWARD_NAME);
631+
632+
// EXPECT_EQ(velocity_polygon_->getActionType(), nav2_collision_monitor::APPROACH);
633+
// EXPECT_NEAR(velocity_polygon_->getTimeBeforeCollision(), 2.0, EPSILON);
634+
// EXPECT_NEAR(velocity_polygon_->getSimulationTimeStep(), 0.05, EPSILON);
635+
// EXPECT_NEAR(velocity_polygon_->getMinVelBeforeStop(), -0.5, EPSILON);
636+
// }
563637

564638
int main(int argc, char ** argv)
565639
{

0 commit comments

Comments
 (0)