Skip to content

Commit eaaab40

Browse files
fix spell, remove debug
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
1 parent df5f1f0 commit eaaab40

File tree

4 files changed

+29
-69
lines changed

4 files changed

+29
-69
lines changed

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

+6-6
Original file line numberDiff line numberDiff line change
@@ -166,8 +166,8 @@
166166
</load_composable_node>
167167
</group>
168168

169-
<!-- First Obstacle Cruise -->
170-
<group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'&quot;)">
169+
<!-- First Obstacle Cruise (original) -->
170+
<group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'&quot;)">
171171
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
172172
<composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner" namespace="">
173173
<!-- topic remap -->
@@ -191,10 +191,10 @@
191191
</load_composable_node>
192192
</group>
193193

194-
<!-- Second Obstacle Cruise -->
195-
<group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'&quot;)">
194+
<!-- Second Obstacle Cruise (pseudo occlusion)-->
195+
<group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'&quot;)">
196196
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
197-
<composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner_pseudo_occulusion" namespace="">
197+
<composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner_pseudo_occlusion" namespace="">
198198
<!-- topic remap -->
199199
<remap from="~/input/trajectory" to="obstacle_cruise/trajectory"/>
200200
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
@@ -209,7 +209,7 @@
209209
<param from="$(var common_param_path)"/>
210210
<param from="$(var vehicle_param_file)"/>
211211
<param from="$(var nearest_search_param_path)"/>
212-
<param from="$(var obstacle_cruise_planner_pseudo_occulusion_param_path)"/>
212+
<param from="$(var obstacle_cruise_planner_pseudo_occlusion_param_path)"/>
213213
<!-- composable node config -->
214214
<extra_arg name="use_intra_process_comms" value="false"/>
215215
</composable_node>

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
enable_debug_info: false
77
enable_calculation_time_info: false
88

9-
enable_slow_down_planning: true
9+
enable_slow_down_planning: false
1010

1111
# longitudinal info
1212
idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -203,9 +203,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
203203
// consideration for the current ego pose
204204
bool enable_to_consider_current_pose{false};
205205
double time_to_convergence{1.5};
206-
bool work_as_pseudo_occulusion{false};
207-
double max_obj_vel_for_pseudo_occulusion{0.0};
208-
std::vector<lanelet::Id> focus_intersections_for_pseudo_occulusion{};
206+
bool work_as_pseudo_occlusion{false};
207+
double max_obj_vel_for_pseudo_occlusion{0.0};
208+
std::vector<lanelet::Id> focus_intersections_for_pseudo_occlusion{};
209209
};
210210
BehaviorDeterminationParam behavior_determination_param_;
211211

planning/obstacle_cruise_planner/src/node.cpp

+19-59
Original file line numberDiff line numberDiff line change
@@ -27,52 +27,17 @@
2727
#include <lanelet2_extension/utility/message_conversion.hpp>
2828
#include <lanelet2_extension/utility/query.hpp>
2929
#include <lanelet2_extension/utility/utilities.hpp>
30-
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
3130

3231
#include <boost/format.hpp>
33-
#include <boost/geometry/algorithms/correct.hpp>
3432
#include <boost/geometry/algorithms/intersects.hpp>
33+
#include <boost/geometry/algorithms/within.hpp>
3534

3635
#include <lanelet2_core/geometry/LineString.h>
3736
#include <lanelet2_core/geometry/Polygon.h>
3837

3938
#include <algorithm>
4039
#include <chrono>
4140

42-
#define debug(var) \
43-
do { \
44-
std::cerr << __LINE__ << ", " << __func__ << ", " << #var << ": "; \
45-
view(var); \
46-
} while (0)
47-
template <typename T>
48-
void view(T e)
49-
{
50-
std::cerr << e << std::endl;
51-
}
52-
template <typename T>
53-
void view(const std::vector<T> & v)
54-
{
55-
for (const auto & e : v) {
56-
std::cerr << e << " ";
57-
}
58-
std::cerr << std::endl;
59-
}
60-
template <typename T>
61-
void view(const std::vector<std::vector<T>> & vv)
62-
{
63-
for (const auto & v : vv) {
64-
view(v);
65-
}
66-
}
67-
#define line() \
68-
{ \
69-
std::cerr << __LINE__ << ", " << __func__ << std::endl; \
70-
}
71-
#define line_with_file() \
72-
{ \
73-
std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \
74-
}
75-
7641
namespace
7742
{
7843
VelocityLimitClearCommand createVelocityLimitClearCommandMessage(
@@ -320,13 +285,13 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
320285
"behavior_determination.consider_current_pose.enable_to_consider_current_pose");
321286
time_to_convergence = node.declare_parameter<double>(
322287
"behavior_determination.consider_current_pose.time_to_convergence");
323-
work_as_pseudo_occulusion = node.declare_parameter<bool>(
324-
"behavior_determination.slow_down.pseudo_occulusion.enable_function");
325-
if (work_as_pseudo_occulusion) {
326-
max_obj_vel_for_pseudo_occulusion = node.declare_parameter<double>(
327-
"behavior_determination.slow_down.pseudo_occulusion.max_obj_vel");
328-
focus_intersections_for_pseudo_occulusion = node.declare_parameter<std::vector<lanelet::Id>>(
329-
"behavior_determination.slow_down.pseudo_occulusion.focus_intersections");
288+
work_as_pseudo_occlusion = node.declare_parameter<bool>(
289+
"behavior_determination.slow_down.pseudo_occlusion.enable_function");
290+
if (work_as_pseudo_occlusion) {
291+
max_obj_vel_for_pseudo_occlusion = node.declare_parameter<double>(
292+
"behavior_determination.slow_down.pseudo_occlusion.max_obj_vel");
293+
focus_intersections_for_pseudo_occlusion = node.declare_parameter<std::vector<lanelet::Id>>(
294+
"behavior_determination.slow_down.pseudo_occlusion.focus_intersections");
330295
}
331296
}
332297

@@ -389,15 +354,15 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
389354
parameters, "behavior_determination.consider_current_pose.time_to_convergence",
390355
time_to_convergence);
391356
tier4_autoware_utils::updateParam<bool>(
392-
parameters, "behavior_determination.slow_down.pseudo_occulusion.enable_function",
393-
work_as_pseudo_occulusion);
394-
if (work_as_pseudo_occulusion) {
357+
parameters, "behavior_determination.slow_down.pseudo_occlusion.enable_function",
358+
work_as_pseudo_occlusion);
359+
if (work_as_pseudo_occlusion) {
395360
tier4_autoware_utils::updateParam<double>(
396-
parameters, "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel",
397-
max_obj_vel_for_pseudo_occulusion);
361+
parameters, "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel",
362+
max_obj_vel_for_pseudo_occlusion);
398363
tier4_autoware_utils::updateParam<std::vector<lanelet::Id>>(
399-
parameters, "behavior_determination.slow_down.pseudo_occulusion.focus_intersections",
400-
focus_intersections_for_pseudo_occulusion);
364+
parameters, "behavior_determination.slow_down.pseudo_occlusion.focus_intersections",
365+
focus_intersections_for_pseudo_occlusion);
401366
}
402367
}
403368

@@ -1199,21 +1164,18 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
11991164
return std::nullopt;
12001165
}
12011166

1202-
const auto is_occulusion_object = [&]() {
1167+
const auto is_occlusion_object = [&]() {
12031168
if (
12041169
std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) >
1205-
behavior_determination_param_.max_obj_vel_for_pseudo_occulusion + 1e-6) {
1206-
line();
1170+
behavior_determination_param_.max_obj_vel_for_pseudo_occlusion + 1e-6) {
12071171
return false;
12081172
}
12091173

12101174
if (motion_utils::calcLateralOffset(traj_points, obstacle.pose.position) > 0.0) {
1211-
line();
12121175
return true;
12131176
}
12141177

1215-
for (const auto & id :
1216-
behavior_determination_param_.focus_intersections_for_pseudo_occulusion) {
1178+
for (const auto & id : behavior_determination_param_.focus_intersections_for_pseudo_occlusion) {
12171179
if (id == 0) {
12181180
continue;
12191181
}
@@ -1223,15 +1185,13 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
12231185
if (
12241186
boost::geometry::within(obstacle_poly, intersection_poly) ||
12251187
boost::geometry::intersects(obstacle_poly, intersection_poly)) {
1226-
line();
12271188
return true;
12281189
}
12291190
}
1230-
line();
12311191
return false;
12321192
};
12331193

1234-
if (behavior_determination_param_.work_as_pseudo_occulusion && !is_occulusion_object()) {
1194+
if (behavior_determination_param_.work_as_pseudo_occlusion && !is_occlusion_object()) {
12351195
return std::nullopt;
12361196
}
12371197

0 commit comments

Comments
 (0)