Skip to content

Commit 04343b6

Browse files
authored
fix(behavior_velocity_planner): fix virtual_traffic_light singed_arc_length and state (autowarefoundation#306)
* fix(behavior_velocity): fix virtual_traffic_light singed_arc_length and state Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Use <> to include from another packages Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent a3b0ba8 commit 04343b6

File tree

5 files changed

+113
-14
lines changed

5 files changed

+113
-14
lines changed

common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp

+66
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,44 @@ size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point
166166
return nearest_idx;
167167
}
168168

169+
/**
170+
* @brief find nearest segment index to pose
171+
* segment is straight path between two continuous points of trajectory
172+
* When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1
173+
* @param points points of trajectory
174+
* @param pose pose to which to find nearest segment index
175+
* @param max_dist max distance to search
176+
* @param max_yaw max yaw to search
177+
* @return nearest index
178+
*/
179+
template <class T>
180+
boost::optional<size_t> findNearestSegmentIndex(
181+
const T & points, const geometry_msgs::msg::Pose & pose,
182+
const double max_dist = std::numeric_limits<double>::max(),
183+
const double max_yaw = std::numeric_limits<double>::max())
184+
{
185+
const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw);
186+
187+
if (!nearest_idx) {
188+
return boost::none;
189+
}
190+
191+
if (*nearest_idx == 0) {
192+
return 0;
193+
}
194+
if (*nearest_idx == points.size() - 1) {
195+
return points.size() - 2;
196+
}
197+
198+
const double signed_length = calcLongitudinalOffsetToSegment(points, *nearest_idx, pose.position);
199+
200+
if (signed_length <= 0) {
201+
return *nearest_idx - 1;
202+
}
203+
204+
return *nearest_idx;
205+
}
206+
169207
/**
170208
* @brief calculate lateral offset from p_target (length from p_target to trajectory)
171209
* If seg_idx point is after that nearest point, length is negative
@@ -265,6 +303,34 @@ double calcSignedArcLength(
265303
return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
266304
}
267305

306+
/**
307+
* @brief calcSignedArcLength from pose to point
308+
*/
309+
template <class T>
310+
boost::optional<double> calcSignedArcLength(
311+
const T & points, const geometry_msgs::msg::Pose & src_pose,
312+
const geometry_msgs::msg::Point & dst_point,
313+
const double max_dist = std::numeric_limits<double>::max(),
314+
const double max_yaw = std::numeric_limits<double>::max())
315+
{
316+
validateNonEmpty(points);
317+
318+
const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw);
319+
if (!src_seg_idx) {
320+
return boost::none;
321+
}
322+
323+
const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point);
324+
325+
const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_seg_idx);
326+
const double signed_length_src_offset =
327+
calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position);
328+
const double signed_length_dst_offset =
329+
calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point);
330+
331+
return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
332+
}
333+
268334
/**
269335
* @brief calcArcLength for the whole length
270336
*/

planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,5 @@
44
max_delay_sec: 3.0
55
near_line_distance: 1.0
66
dead_line_margin: 1.0
7+
max_yaw_deviation_deg: 90.0
78
check_timeout_after_stop_line: true

planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface
6868
double max_delay_sec;
6969
double near_line_distance;
7070
double dead_line_margin;
71+
double max_yaw_deviation_rad;
7172
bool check_timeout_after_stop_line;
7273
};
7374

planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp

+3
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
// limitations under the License.
1414

1515
#include <scene_module/virtual_traffic_light/manager.hpp>
16+
#include <tier4_autoware_utils/math/unit_conversion.hpp>
1617

1718
#include <memory>
1819
#include <set>
@@ -70,6 +71,8 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
7071
p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec", 3.0);
7172
p.near_line_distance = node.declare_parameter(ns + ".near_line_distance", 1.0);
7273
p.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 1.0);
74+
p.max_yaw_deviation_rad =
75+
tier4_autoware_utils::deg2rad(node.declare_parameter(ns + ".max_yaw_deviation_deg", 90.0));
7376
p.check_timeout_after_stop_line =
7477
node.declare_parameter(ns + ".check_timeout_after_stop_line", true);
7578
}

planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp

+42-14
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,20 @@
1919
#include <tier4_v2x_msgs/msg/key_value.hpp>
2020

2121
#include <algorithm>
22+
#include <limits>
2223
#include <string>
2324
#include <vector>
2425

26+
namespace tier4_autoware_utils
27+
{
28+
template <>
29+
inline geometry_msgs::msg::Pose getPose(
30+
const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
31+
{
32+
return p.point.pose;
33+
}
34+
} // namespace tier4_autoware_utils
35+
2536
namespace behavior_velocity_planner
2637
{
2738
namespace
@@ -182,7 +193,7 @@ SegmentIndexWithOffset findBackwardOffsetSegment(
182193
const double offset_length)
183194
{
184195
double sum_length = 0.0;
185-
for (size_t i = base_idx - 1; i > 0; --i) {
196+
for (size_t i = base_idx; i > 0; --i) {
186197
const auto p_front = path.points.at(i - 1);
187198
const auto p_back = path.points.at(i);
188199

@@ -399,10 +410,10 @@ bool VirtualTrafficLightModule::modifyPathVelocity(
399410
return true;
400411
}
401412

402-
// Stop at stop_line if state is timeout
403-
if (isBeforeStopLine() || planner_param_.check_timeout_after_stop_line) {
413+
// Stop at stop_line if state is timeout before stop_line
414+
if (isBeforeStopLine()) {
404415
if (isStateTimeout(*virtual_traffic_light_state)) {
405-
RCLCPP_DEBUG(logger_, "state is timeout");
416+
RCLCPP_DEBUG(logger_, "state is timeout before stop line");
406417
insertStopVelocityAtStopLine(path, stop_reason);
407418
}
408419

@@ -413,6 +424,15 @@ bool VirtualTrafficLightModule::modifyPathVelocity(
413424
// After stop_line
414425
state_ = State::PASSING;
415426

427+
// Check timeout after stop_line if the option is on
428+
if (
429+
planner_param_.check_timeout_after_stop_line && isStateTimeout(*virtual_traffic_light_state)) {
430+
RCLCPP_DEBUG(logger_, "state is timeout after stop line");
431+
insertStopVelocityAtStopLine(path, stop_reason);
432+
updateInfrastructureCommand();
433+
return true;
434+
}
435+
416436
// Stop at stop_line if finalization isn't completed
417437
if (!virtual_traffic_light_state->is_finalized) {
418438
RCLCPP_DEBUG(logger_, "finalization isn't completed");
@@ -453,10 +473,12 @@ bool VirtualTrafficLightModule::isBeforeStartLine()
453473
return false;
454474
}
455475

476+
const double max_dist = std::numeric_limits<double>::max();
456477
const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
457-
module_data_.path.points, module_data_.head_pose.position, collision->point);
478+
module_data_.path.points, module_data_.head_pose, collision->point, max_dist,
479+
planner_param_.max_yaw_deviation_rad);
458480

459-
return signed_arc_length > 0;
481+
return *signed_arc_length > 0;
460482
}
461483

462484
bool VirtualTrafficLightModule::isBeforeStopLine()
@@ -469,10 +491,12 @@ bool VirtualTrafficLightModule::isBeforeStopLine()
469491
return false;
470492
}
471493

494+
const double max_dist = std::numeric_limits<double>::max();
472495
const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
473-
module_data_.path.points, module_data_.head_pose.position, collision->point);
496+
module_data_.path.points, module_data_.head_pose, collision->point, max_dist,
497+
planner_param_.max_yaw_deviation_rad);
474498

475-
return signed_arc_length > -planner_param_.dead_line_margin;
499+
return *signed_arc_length > -planner_param_.dead_line_margin;
476500
}
477501

478502
bool VirtualTrafficLightModule::isAfterAnyEndLine()
@@ -484,16 +508,18 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine()
484508

485509
const auto collision = findCollision(module_data_.path.points, map_data_.end_lines);
486510

487-
// Since the module is registered, a collision should be detected usually.
488-
// Therefore if no collision found, vehicle's path is fully after the line.
511+
// If the goal is set before the end line, collision will not be detected.
512+
// Therefore if there is no collision, the ego vehicle is assumed to be before the end line.
489513
if (!collision) {
490514
return false;
491515
}
492516

517+
const double max_dist = std::numeric_limits<double>::max();
493518
const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
494-
module_data_.path.points, module_data_.head_pose.position, collision->point);
519+
module_data_.path.points, module_data_.head_pose, collision->point, max_dist,
520+
planner_param_.max_yaw_deviation_rad);
495521

496-
return signed_arc_length < -planner_param_.dead_line_margin;
522+
return *signed_arc_length < -planner_param_.dead_line_margin;
497523
}
498524

499525
bool VirtualTrafficLightModule::isNearAnyEndLine()
@@ -504,10 +530,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine()
504530
return false;
505531
}
506532

533+
const double max_dist = std::numeric_limits<double>::max();
507534
const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength(
508-
module_data_.path.points, module_data_.head_pose.position, collision->point);
535+
module_data_.path.points, module_data_.head_pose, collision->point, max_dist,
536+
planner_param_.max_yaw_deviation_rad);
509537

510-
return std::abs(signed_arc_length) < planner_param_.near_line_distance;
538+
return std::abs(*signed_arc_length) < planner_param_.near_line_distance;
511539
}
512540

513541
boost::optional<tier4_v2x_msgs::msg::VirtualTrafficLightState>

0 commit comments

Comments
 (0)