From b05bd5ccff54c135761f0c24dc1887c2ebf3ce60 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Thu, 11 Apr 2024 14:16:45 +0900
Subject: [PATCH 1/7] add launch
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
.../motion_planning.launch.xml | 49 +++++++++++++++++++
1 file changed, 49 insertions(+)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
index 18de04fd9e317..ea25be8e9d788 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -165,6 +165,55 @@
</load_composable_node>
</group>
+ <!-- First Obstacle Cruise -->
+ <group if="$(eval "'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'")">
+ <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
+ <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner" namespace="">
+ <!-- topic remap -->
+ <remap from="~/input/trajectory" to="obstacle_velocity_limiter/trajectory"/>
+ <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+ <remap from="~/input/acceleration" to="/localization/acceleration"/>
+ <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+ <remap from="~/output/trajectory" to="obstacle_cruise/trajectory"/>
+ <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
+ <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
+ <remap from="~/output/clear_velocity_limit" to="/planning/scenario_planning/clear_velocity_limit"/>
+ <!-- params -->
+ <param from="$(var common_param_path)"/>
+ <param from="$(var vehicle_param_file)"/>
+ <param from="$(var nearest_search_param_path)"/>
+ <param from="$(var obstacle_cruise_planner_param_path)"/>
+ <!-- composable node config -->
+ <extra_arg name="use_intra_process_comms" value="false"/>
+ </composable_node>
+ </load_composable_node>
+ </group>
+
+ <!-- Second Obstacle Cruise -->
+ <group if="$(eval "'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'")">
+ <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
+ <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner_pseudo_occulusion" namespace="">
+ <!-- topic remap -->
+ <remap from="~/input/trajectory" to="obstacle_cruise/trajectory"/>
+ <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+ <remap from="~/input/acceleration" to="/localization/acceleration"/>
+ <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+ <remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
+ <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
+ <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
+ <remap from="~/output/clear_velocity_limit" to="/planning/scenario_planning/clear_velocity_limit"/>
+ <!-- params -->
+ <param from="$(var common_param_path)"/>
+ <param from="$(var vehicle_param_file)"/>
+ <param from="$(var nearest_search_param_path)"/>
+ <param from="$(var obstacle_cruise_planner_pseudo_occulusion_param_path)"/>
+ <!-- composable node config -->
+ <extra_arg name="use_intra_process_comms" value="false"/>
+ </composable_node>
+ </load_composable_node>
+ </group>
+
+
<group if="$(eval "'$(var motion_stop_planner_type)' == 'none'")">
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
<composable_node pkg="topic_tools" plugin="topic_tools::RelayNode" name="obstacle_stop_relay" namespace="">
From df5f1f04fa36d1b8ac7001dd3c3d002013787b4d Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Thu, 11 Apr 2024 18:49:04 +0900
Subject: [PATCH 2/7] dekita
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
.../motion_planning.launch.xml | 3 +
.../include/obstacle_cruise_planner/node.hpp | 8 ++
.../obstacle_cruise_planner/type_alias.hpp | 2 +
.../launch/obstacle_cruise_planner.launch.xml | 1 +
planning/obstacle_cruise_planner/src/node.cpp | 107 +++++++++++++++++-
5 files changed, 120 insertions(+), 1 deletion(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
index ea25be8e9d788..a4401be922426 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -125,6 +125,7 @@
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
<remap from="~/input/acceleration" to="/localization/acceleration"/>
<remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+ <remap from="~/input/vector_map" to="/map/vector_map"/>
<remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
<remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
@@ -174,6 +175,7 @@
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
<remap from="~/input/acceleration" to="/localization/acceleration"/>
<remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+ <remap from="~/input/vector_map" to="/map/vector_map"/>
<remap from="~/output/trajectory" to="obstacle_cruise/trajectory"/>
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
<remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
@@ -198,6 +200,7 @@
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
<remap from="~/input/acceleration" to="/localization/acceleration"/>
<remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+ <remap from="~/input/vector_map" to="/map/vector_map"/>
<remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
<remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
index 4a3654d48afe1..72e52ec71b9d7 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -19,6 +19,7 @@
#include "obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp"
#include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp"
#include "obstacle_cruise_planner/type_alias.hpp"
+#include "route_handler/route_handler.hpp"
#include "signal_processing/lowpass_filter_1d.hpp"
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
#include "tier4_autoware_utils/system/stop_watch.hpp"
@@ -136,11 +137,15 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;
+ rclcpp::Subscription<HADMapBin>::SharedPtr lanelet_map_sub_;
// data for callback functions
PredictedObjects::ConstSharedPtr objects_ptr_{nullptr};
Odometry::ConstSharedPtr ego_odom_ptr_{nullptr};
AccelWithCovarianceStamped::ConstSharedPtr ego_accel_ptr_{nullptr};
+ HADMapBin::ConstSharedPtr vector_map_ptr_{nullptr};
+
+ std::shared_ptr<route_handler::RouteHandler> route_handler_;
// Vehicle Parameters
VehicleInfo vehicle_info_;
@@ -198,6 +203,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
// consideration for the current ego pose
bool enable_to_consider_current_pose{false};
double time_to_convergence{1.5};
+ bool work_as_pseudo_occulusion{false};
+ double max_obj_vel_for_pseudo_occulusion{0.0};
+ std::vector<lanelet::Id> focus_intersections_for_pseudo_occulusion{};
};
BehaviorDeterminationParam behavior_determination_param_;
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp
index d46062dd8f85c..d555650b07690 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp
@@ -19,6 +19,7 @@
#include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp"
#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp"
+#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
#include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
@@ -38,6 +39,7 @@
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
using autoware_adapi_v1_msgs::msg::VelocityFactor;
using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
+using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
diff --git a/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml
index a6c95d65acc6a..5775f1b9a2f1d 100644
--- a/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml
+++ b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml
@@ -26,6 +26,7 @@
<remap from="~/input/trajectory" to="$(var input_trajectory)"/>
<remap from="~/input/odometry" to="$(var input_odometry)"/>
<remap from="~/input/objects" to="$(var input_objects)"/>
+ <remap from="~/input/vector_map" to="$(var input_map)"/>
<!-- output -->
<remap from="~/output/trajectory" to="$(var output_trajectory)"/>
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index 31f011b3749b9..31f217293a693 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -16,6 +16,7 @@
#include "motion_utils/resample/resample.hpp"
#include "motion_utils/trajectory/tmp_conversion.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
#include "object_recognition_utils/predicted_path_utils.hpp"
#include "obstacle_cruise_planner/polygon_utils.hpp"
#include "obstacle_cruise_planner/utils.hpp"
@@ -23,11 +24,55 @@
#include "tier4_autoware_utils/ros/marker_helper.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+
#include <boost/format.hpp>
+#include <boost/geometry/algorithms/correct.hpp>
+#include <boost/geometry/algorithms/intersects.hpp>
+
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Polygon.h>
#include <algorithm>
#include <chrono>
+#define debug(var) \
+ do { \
+ std::cerr << __LINE__ << ", " << __func__ << ", " << #var << ": "; \
+ view(var); \
+ } while (0)
+template <typename T>
+void view(T e)
+{
+ std::cerr << e << std::endl;
+}
+template <typename T>
+void view(const std::vector<T> & v)
+{
+ for (const auto & e : v) {
+ std::cerr << e << " ";
+ }
+ std::cerr << std::endl;
+}
+template <typename T>
+void view(const std::vector<std::vector<T>> & vv)
+{
+ for (const auto & v : vv) {
+ view(v);
+ }
+}
+#define line() \
+ { \
+ std::cerr << __LINE__ << ", " << __func__ << std::endl; \
+ }
+#define line_with_file() \
+ { \
+ std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \
+ }
+
namespace
{
VelocityLimitClearCommand createVelocityLimitClearCommandMessage(
@@ -275,6 +320,14 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
"behavior_determination.consider_current_pose.enable_to_consider_current_pose");
time_to_convergence = node.declare_parameter<double>(
"behavior_determination.consider_current_pose.time_to_convergence");
+ work_as_pseudo_occulusion = node.declare_parameter<bool>(
+ "behavior_determination.slow_down.pseudo_occulusion.enable_function");
+ if (work_as_pseudo_occulusion) {
+ max_obj_vel_for_pseudo_occulusion = node.declare_parameter<double>(
+ "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel");
+ focus_intersections_for_pseudo_occulusion = node.declare_parameter<std::vector<lanelet::Id>>(
+ "behavior_determination.slow_down.pseudo_occulusion.focus_intersections");
+ }
}
void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
@@ -335,6 +388,17 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
tier4_autoware_utils::updateParam<double>(
parameters, "behavior_determination.consider_current_pose.time_to_convergence",
time_to_convergence);
+ tier4_autoware_utils::updateParam<bool>(
+ parameters, "behavior_determination.slow_down.pseudo_occulusion.enable_function",
+ work_as_pseudo_occulusion);
+ if (work_as_pseudo_occulusion) {
+ tier4_autoware_utils::updateParam<double>(
+ parameters, "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel",
+ max_obj_vel_for_pseudo_occulusion);
+ tier4_autoware_utils::updateParam<std::vector<lanelet::Id>>(
+ parameters, "behavior_determination.slow_down.pseudo_occulusion.focus_intersections",
+ focus_intersections_for_pseudo_occulusion);
+ }
}
ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
@@ -357,6 +421,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
acc_sub_ = create_subscription<AccelWithCovarianceStamped>(
"~/input/acceleration", rclcpp::QoS{1},
[this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { ego_accel_ptr_ = msg; });
+ lanelet_map_sub_ = create_subscription<HADMapBin>(
+ "~/input/vector_map", rclcpp::QoS(10).transient_local(),
+ [this](const HADMapBin::ConstSharedPtr msg) { vector_map_ptr_ = msg; });
// publisher
trajectory_pub_ = create_publisher<Trajectory>("~/output/trajectory", 1);
@@ -486,9 +553,11 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg);
// check if subscribed variables are ready
- if (traj_points.empty() || !ego_odom_ptr_ || !ego_accel_ptr_ || !objects_ptr_) {
+ if (
+ traj_points.empty() || !ego_odom_ptr_ || !ego_accel_ptr_ || !objects_ptr_ || !vector_map_ptr_) {
return;
}
+ route_handler_ = std::make_shared<route_handler::RouteHandler>(*vector_map_ptr_);
stop_watch_.tic(__func__);
*debug_data_ptr_ = DebugData();
@@ -1130,6 +1199,42 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
return std::nullopt;
}
+ const auto is_occulusion_object = [&]() {
+ if (
+ std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) >
+ behavior_determination_param_.max_obj_vel_for_pseudo_occulusion + 1e-6) {
+ line();
+ return false;
+ }
+
+ if (motion_utils::calcLateralOffset(traj_points, obstacle.pose.position) > 0.0) {
+ line();
+ return true;
+ }
+
+ for (const auto & id :
+ behavior_determination_param_.focus_intersections_for_pseudo_occulusion) {
+ if (id == 0) {
+ continue;
+ }
+ const auto intersection_poly =
+ lanelet::utils::to2D(route_handler_->getLaneletMapPtr()->polygonLayer.get(id))
+ .basicPolygon();
+ if (
+ boost::geometry::within(obstacle_poly, intersection_poly) ||
+ boost::geometry::intersects(obstacle_poly, intersection_poly)) {
+ line();
+ return true;
+ }
+ }
+ line();
+ return false;
+ };
+
+ if (behavior_determination_param_.work_as_pseudo_occulusion && !is_occulusion_object()) {
+ return std::nullopt;
+ }
+
// calculate front collision point
double front_min_dist = std::numeric_limits<double>::max();
geometry_msgs::msg::Point front_collision_point;
From 728c8baf9f1e82ede4809cd45a80cc3895e158b4 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 16 Apr 2024 04:06:03 +0000
Subject: [PATCH 3/7] style(pre-commit): autofix
---
.../lane_driving/motion_planning/motion_planning.launch.xml | 3 +--
1 file changed, 1 insertion(+), 2 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
index a4401be922426..1cb4c0b3f262d 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -190,7 +190,7 @@
</composable_node>
</load_composable_node>
</group>
-
+
<!-- Second Obstacle Cruise -->
<group if="$(eval "'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'")">
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
@@ -216,7 +216,6 @@
</load_composable_node>
</group>
-
<group if="$(eval "'$(var motion_stop_planner_type)' == 'none'")">
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
<composable_node pkg="topic_tools" plugin="topic_tools::RelayNode" name="obstacle_stop_relay" namespace="">
From eaaab407c484b9a8c9663919d61bf22910bcddb3 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Tue, 16 Apr 2024 14:46:29 +0900
Subject: [PATCH 4/7] fix spell, remove debug
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
.../motion_planning.launch.xml | 12 +--
.../config/obstacle_cruise_planner.param.yaml | 2 +-
.../include/obstacle_cruise_planner/node.hpp | 6 +-
planning/obstacle_cruise_planner/src/node.cpp | 78 +++++--------------
4 files changed, 29 insertions(+), 69 deletions(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
index a4401be922426..a34b5efac7122 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -166,8 +166,8 @@
</load_composable_node>
</group>
- <!-- First Obstacle Cruise -->
- <group if="$(eval "'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'")">
+ <!-- First Obstacle Cruise (original) -->
+ <group if="$(eval "'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'")">
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
<composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner" namespace="">
<!-- topic remap -->
@@ -191,10 +191,10 @@
</load_composable_node>
</group>
- <!-- Second Obstacle Cruise -->
- <group if="$(eval "'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'")">
+ <!-- Second Obstacle Cruise (pseudo occlusion)-->
+ <group if="$(eval "'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'")">
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
- <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner_pseudo_occulusion" namespace="">
+ <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner_pseudo_occlusion" namespace="">
<!-- topic remap -->
<remap from="~/input/trajectory" to="obstacle_cruise/trajectory"/>
<remap from="~/input/odometry" to="/localization/kinematic_state"/>
@@ -209,7 +209,7 @@
<param from="$(var common_param_path)"/>
<param from="$(var vehicle_param_file)"/>
<param from="$(var nearest_search_param_path)"/>
- <param from="$(var obstacle_cruise_planner_pseudo_occulusion_param_path)"/>
+ <param from="$(var obstacle_cruise_planner_pseudo_occlusion_param_path)"/>
<!-- composable node config -->
<extra_arg name="use_intra_process_comms" value="false"/>
</composable_node>
diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
index d8b9c942a986a..55ee8cb638308 100644
--- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
+++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -6,7 +6,7 @@
enable_debug_info: false
enable_calculation_time_info: false
- enable_slow_down_planning: true
+ enable_slow_down_planning: false
# longitudinal info
idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
index 72e52ec71b9d7..6b8f35e862227 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -203,9 +203,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
// consideration for the current ego pose
bool enable_to_consider_current_pose{false};
double time_to_convergence{1.5};
- bool work_as_pseudo_occulusion{false};
- double max_obj_vel_for_pseudo_occulusion{0.0};
- std::vector<lanelet::Id> focus_intersections_for_pseudo_occulusion{};
+ bool work_as_pseudo_occlusion{false};
+ double max_obj_vel_for_pseudo_occlusion{0.0};
+ std::vector<lanelet::Id> focus_intersections_for_pseudo_occlusion{};
};
BehaviorDeterminationParam behavior_determination_param_;
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index 31f217293a693..66cb4a720f59f 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -27,11 +27,10 @@
#include <lanelet2_extension/utility/message_conversion.hpp>
#include <lanelet2_extension/utility/query.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
-#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
#include <boost/format.hpp>
-#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/algorithms/within.hpp>
#include <lanelet2_core/geometry/LineString.h>
#include <lanelet2_core/geometry/Polygon.h>
@@ -39,40 +38,6 @@
#include <algorithm>
#include <chrono>
-#define debug(var) \
- do { \
- std::cerr << __LINE__ << ", " << __func__ << ", " << #var << ": "; \
- view(var); \
- } while (0)
-template <typename T>
-void view(T e)
-{
- std::cerr << e << std::endl;
-}
-template <typename T>
-void view(const std::vector<T> & v)
-{
- for (const auto & e : v) {
- std::cerr << e << " ";
- }
- std::cerr << std::endl;
-}
-template <typename T>
-void view(const std::vector<std::vector<T>> & vv)
-{
- for (const auto & v : vv) {
- view(v);
- }
-}
-#define line() \
- { \
- std::cerr << __LINE__ << ", " << __func__ << std::endl; \
- }
-#define line_with_file() \
- { \
- std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \
- }
-
namespace
{
VelocityLimitClearCommand createVelocityLimitClearCommandMessage(
@@ -320,13 +285,13 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
"behavior_determination.consider_current_pose.enable_to_consider_current_pose");
time_to_convergence = node.declare_parameter<double>(
"behavior_determination.consider_current_pose.time_to_convergence");
- work_as_pseudo_occulusion = node.declare_parameter<bool>(
- "behavior_determination.slow_down.pseudo_occulusion.enable_function");
- if (work_as_pseudo_occulusion) {
- max_obj_vel_for_pseudo_occulusion = node.declare_parameter<double>(
- "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel");
- focus_intersections_for_pseudo_occulusion = node.declare_parameter<std::vector<lanelet::Id>>(
- "behavior_determination.slow_down.pseudo_occulusion.focus_intersections");
+ work_as_pseudo_occlusion = node.declare_parameter<bool>(
+ "behavior_determination.slow_down.pseudo_occlusion.enable_function");
+ if (work_as_pseudo_occlusion) {
+ max_obj_vel_for_pseudo_occlusion = node.declare_parameter<double>(
+ "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel");
+ focus_intersections_for_pseudo_occlusion = node.declare_parameter<std::vector<lanelet::Id>>(
+ "behavior_determination.slow_down.pseudo_occlusion.focus_intersections");
}
}
@@ -389,15 +354,15 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
parameters, "behavior_determination.consider_current_pose.time_to_convergence",
time_to_convergence);
tier4_autoware_utils::updateParam<bool>(
- parameters, "behavior_determination.slow_down.pseudo_occulusion.enable_function",
- work_as_pseudo_occulusion);
- if (work_as_pseudo_occulusion) {
+ parameters, "behavior_determination.slow_down.pseudo_occlusion.enable_function",
+ work_as_pseudo_occlusion);
+ if (work_as_pseudo_occlusion) {
tier4_autoware_utils::updateParam<double>(
- parameters, "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel",
- max_obj_vel_for_pseudo_occulusion);
+ parameters, "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel",
+ max_obj_vel_for_pseudo_occlusion);
tier4_autoware_utils::updateParam<std::vector<lanelet::Id>>(
- parameters, "behavior_determination.slow_down.pseudo_occulusion.focus_intersections",
- focus_intersections_for_pseudo_occulusion);
+ parameters, "behavior_determination.slow_down.pseudo_occlusion.focus_intersections",
+ focus_intersections_for_pseudo_occlusion);
}
}
@@ -1199,21 +1164,18 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
return std::nullopt;
}
- const auto is_occulusion_object = [&]() {
+ const auto is_occlusion_object = [&]() {
if (
std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) >
- behavior_determination_param_.max_obj_vel_for_pseudo_occulusion + 1e-6) {
- line();
+ behavior_determination_param_.max_obj_vel_for_pseudo_occlusion + 1e-6) {
return false;
}
if (motion_utils::calcLateralOffset(traj_points, obstacle.pose.position) > 0.0) {
- line();
return true;
}
- for (const auto & id :
- behavior_determination_param_.focus_intersections_for_pseudo_occulusion) {
+ for (const auto & id : behavior_determination_param_.focus_intersections_for_pseudo_occlusion) {
if (id == 0) {
continue;
}
@@ -1223,15 +1185,13 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
if (
boost::geometry::within(obstacle_poly, intersection_poly) ||
boost::geometry::intersects(obstacle_poly, intersection_poly)) {
- line();
return true;
}
}
- line();
return false;
};
- if (behavior_determination_param_.work_as_pseudo_occulusion && !is_occulusion_object()) {
+ if (behavior_determination_param_.work_as_pseudo_occlusion && !is_occlusion_object()) {
return std::nullopt;
}
From af22e004068215cf4509b68870c722d2bf7d9962 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 16 Apr 2024 05:54:47 +0000
Subject: [PATCH 5/7] style(pre-commit): autofix
---
.../lane_driving/motion_planning/motion_planning.launch.xml | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
index 3201150ad9134..ece5440bd06bd 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -190,7 +190,7 @@
</composable_node>
</load_composable_node>
</group>
-
+
<!-- Second Obstacle Cruise (pseudo occlusion)-->
<group if="$(eval "'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'")">
<load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
From f6c58876deb4fb4b284255bdc15f52bf951bfd57 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Tue, 16 Apr 2024 14:56:16 +0900
Subject: [PATCH 6/7] restore
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
.../config/obstacle_cruise_planner.param.yaml | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
index 55ee8cb638308..d8b9c942a986a 100644
--- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
+++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -6,7 +6,7 @@
enable_debug_info: false
enable_calculation_time_info: false
- enable_slow_down_planning: false
+ enable_slow_down_planning: true
# longitudinal info
idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
From cabb15d019c7f496a239ec20f1f65d64f0d6158f Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Thu, 9 May 2024 19:01:24 +0900
Subject: [PATCH 7/7] fix freequent route handler construction
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
.../include/obstacle_cruise_planner/node.hpp | 2 +-
planning/obstacle_cruise_planner/src/node.cpp | 6 ++++--
2 files changed, 5 insertions(+), 3 deletions(-)
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
index 6b8f35e862227..ed25ee6f4e996 100644
--- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
+++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -145,7 +145,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
AccelWithCovarianceStamped::ConstSharedPtr ego_accel_ptr_{nullptr};
HADMapBin::ConstSharedPtr vector_map_ptr_{nullptr};
- std::shared_ptr<route_handler::RouteHandler> route_handler_;
+ std::unique_ptr<route_handler::RouteHandler> route_handler_;
// Vehicle Parameters
VehicleInfo vehicle_info_;
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index 66cb4a720f59f..37c567a9637b8 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -515,6 +515,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
{
+ stop_watch_.tic(__func__);
const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg);
// check if subscribed variables are ready
@@ -522,9 +523,10 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
traj_points.empty() || !ego_odom_ptr_ || !ego_accel_ptr_ || !objects_ptr_ || !vector_map_ptr_) {
return;
}
- route_handler_ = std::make_shared<route_handler::RouteHandler>(*vector_map_ptr_);
+ if (route_handler_ == nullptr) {
+ route_handler_ = std::make_unique<route_handler::RouteHandler>(*vector_map_ptr_);
+ }
- stop_watch_.tic(__func__);
*debug_data_ptr_ = DebugData();
const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(traj_points);