feat(surround_obstacle_checker): separate surround_obstacle_checker f…

…rom hierarchical planning flow (tier4#830) * fix(surroud_obstacle_checker): use alias Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(surround_obstacle_checker): use velocity limit Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * chore(surround_obstacle_checker): rename publisher, subscriber and callback functions Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(surround_obstacle_checker): use parameter struct Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(surround_obstacle_checker): use alias Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(surround_obstacle_checker): cleanup member functions Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(surround_obstacle_checker): cleanup polygon handling Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(surround_obstacle_checker): use marker helper Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(planning_launch): separate surround_obstacle_checker from hierarchical motion planning flow Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
boyali · Oct 3, 2022 · 1075236 · 1075236
1 parent 1deb618
commit 1075236
Show file tree

Hide file tree

Showing 10 changed files with 433 additions and 524 deletions.
diff --git a/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
@@ -92,20 +92,23 @@ def launch_setup(context, *args, **kwargs):
         surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     surround_obstacle_checker_component = ComposableNode(
         package="surround_obstacle_checker",
-        plugin="SurroundObstacleCheckerNode",
+        plugin="surround_obstacle_checker::SurroundObstacleCheckerNode",
         name="surround_obstacle_checker",
         namespace="",
         remappings=[
             ("~/output/no_start_reason", "/planning/scenario_planning/status/no_start_reason"),
             ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
-            ("~/output/trajectory", "surround_obstacle_checker/trajectory"),
+            ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"),
+            (
+                "~/output/velocity_limit_clear_command",
+                "/planning/scenario_planning/clear_velocity_limit",
+            ),
             (
                 "~/input/pointcloud",
                 "/perception/obstacle_segmentation/pointcloud",
             ),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
         ],
         parameters=[
             surround_obstacle_checker_param,
@@ -114,22 +117,6 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
-    # relay
-    relay_component = ComposableNode(
-        package="topic_tools",
-        plugin="topic_tools::RelayNode",
-        name="skip_surround_obstacle_check_relay",
-        namespace="",
-        parameters=[
-            {
-                "input_topic": "obstacle_avoidance_planner/trajectory",
-                "output_topic": "surround_obstacle_checker/trajectory",
-                "type": "autoware_auto_planning_msgs/msg/Trajectory",
-            }
-        ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-
     # obstacle stop planner
     obstacle_stop_planner_param_path = os.path.join(
         get_package_share_directory("tier4_planning_launch"),
@@ -173,7 +160,7 @@ def launch_setup(context, *args, **kwargs):
             ),
             ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/odometry", "/localization/kinematic_state"),
-            ("~/input/trajectory", "surround_obstacle_checker/trajectory"),
+            ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
         ],
         parameters=[
             common_param,
@@ -202,13 +189,7 @@ def launch_setup(context, *args, **kwargs):
         condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")),
     )
 
-    relay_loader = LoadComposableNodes(
-        composable_node_descriptions=[relay_component],
-        target_container=container,
-        condition=UnlessCondition(LaunchConfiguration("use_surround_obstacle_check")),
-    )
-
-    group = GroupAction([container, surround_obstacle_checker_loader, relay_loader])
+    group = GroupAction([container, surround_obstacle_checker_loader])
 
     return [group]
 

diff --git a/...g_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/...g_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -19,22 +19,11 @@
   <!-- surround obstacle checker (Do not start if there are pedestrian/bicycles around ego-car) -->
   <group if="$(var use_surround_obstacle_check)">
     <include file="$(find-pkg-share surround_obstacle_checker)/launch/surround_obstacle_checker.launch.xml">
-      <arg name="input_trajectory" value="obstacle_avoidance_planner/trajectory"/>
-      <arg name="output_trajectory" value="surround_obstacle_checker/trajectory"/>
       <arg name="param_path" value="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml"/>
       <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
     </include>
   </group>
 
-  <!-- skip surround obstacle checker -->
-  <group unless="$(var use_surround_obstacle_check)">
-    <node pkg="topic_tools" exec="relay" name="skip_surround_obstacle_check_relay" output="log">
-      <param name="input_topic" value="obstacle_avoidance_planner/trajectory"/>
-      <param name="output_topic" value="surround_obstacle_checker/trajectory"/>
-      <param name="type" value="autoware_auto_planning_msgs/msg/Trajectory"/>
-    </node>
-  </group>
-
   <!-- stop velocity planning with obstacle pointcloud info -->
   <group>
     <include file="$(find-pkg-share obstacle_stop_planner)/launch/obstacle_stop_planner.launch.xml">
@@ -45,7 +34,7 @@
       <arg name="obstacle_stop_planner_param_path" value="$(find-pkg-share tier4_planning_launch)/config/obstacle_stop_planner/obstacle_stop_planner.param.yaml"/>
       <arg name="enable_slow_down" value="false"/>
       <!-- remap topic name -->
-      <arg name="input_trajectory" value="surround_obstacle_checker/trajectory"/>
+      <arg name="input_trajectory" value="obstacle_avoidance_planner/trajectory"/>
       <arg name="input_pointcloud" value="/perception/obstacle_segmentation/pointcloud"/>
       <arg name="output_trajectory" value="/planning/scenario_planning/lane_driving/trajectory"/>
     </include>

diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt
@@ -18,7 +18,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "SurroundObstacleCheckerNode"
+  PLUGIN "surround_obstacle_checker::SurroundObstacleCheckerNode"
   EXECUTABLE ${PROJECT_NAME}_node
 )
 

diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml
@@ -9,3 +9,4 @@
 
     # ego stop state
     stop_state_ego_speed: 0.1 #[m/s]
+    stop_state_entry_duration_time: 0.1 #[s]
diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp
@@ -25,8 +25,18 @@
 #include <memory>
 #include <string>
 
+namespace surround_obstacle_checker
+{
+
+using tier4_planning_msgs::msg::StopFactor;
+using tier4_planning_msgs::msg::StopReason;
+using tier4_planning_msgs::msg::StopReasonArray;
+using visualization_msgs::msg::Marker;
+using visualization_msgs::msg::MarkerArray;
+
 enum class PoseType : int8_t { NoStart = 0 };
 enum class PointType : int8_t { NoStart = 0 };
+
 class SurroundObstacleCheckerDebugNode
 {
 public:
@@ -38,16 +48,16 @@ class SurroundObstacleCheckerDebugNode
   void publish();
 
 private:
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
-  rclcpp::Publisher<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr stop_reason_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_viz_pub_;
+  rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reason_pub_;
   double base_link2front_;
 
-  visualization_msgs::msg::MarkerArray makeVisualizationMarker();
-  tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray();
+  MarkerArray makeVisualizationMarker();
+  StopReasonArray makeStopReasonArray();
 
   std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;
   std::shared_ptr<geometry_msgs::msg::Pose> stop_pose_ptr_;
   rclcpp::Clock::SharedPtr clock_;
 };
-
+}  // namespace surround_obstacle_checker
 #endif  // SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_
diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
@@ -16,9 +16,9 @@
 #define SURROUND_OBSTACLE_CHECKER__NODE_HPP_
 
 #include "surround_obstacle_checker/debug_marker.hpp"
-#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
@@ -27,30 +27,31 @@
 #include <diagnostic_msgs/msg/key_value.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_planning_msgs/msg/velocity_limit.hpp>
+#include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
-#include <boost/assert.hpp>
-#include <boost/assign/list_of.hpp>
-#include <boost/format.hpp>
-#include <boost/geometry.hpp>
-#include <boost/geometry/geometries/linestring.hpp>
-#include <boost/geometry/geometries/point_xy.hpp>
-
-#include <pcl_conversions/pcl_conversions.h>
 #include <tf2/utils.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
-using Point2d = boost::geometry::model::d2::point_xy<double>;
-using Polygon2d =
-  boost::geometry::model::polygon<Point2d, false, false>;  // counter-clockwise, open
+namespace surround_obstacle_checker
+{
+
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_perception_msgs::msg::Shape;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using tier4_planning_msgs::msg::VelocityLimit;
+using tier4_planning_msgs::msg::VelocityLimitClearCommand;
+using vehicle_info_util::VehicleInfo;
+
+using Obstacle = std::pair<double /* distance */, geometry_msgs::msg::Point>;
 
 enum class State { PASS, STOP };
 
@@ -59,70 +60,73 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
 public:
   explicit SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options);
 
+  struct NodeParam
+  {
+    bool use_pointcloud;
+    bool use_dynamic_object;
+    bool is_surround_obstacle;
+    // For preventing chattering,
+    // surround_check_recover_distance_ must be  bigger than surround_check_distance_
+    double surround_check_recover_distance;
+    double surround_check_distance;
+    double state_clear_time;
+    double stop_state_ego_speed;
+    double stop_state_entry_duration_time;
+  };
+
 private:
-  void pathCallback(const Trajectory::ConstSharedPtr input_msg);
-  void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg);
-  void dynamicObjectCallback(
-    const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_msg);
-  void currentVelocityCallback(const nav_msgs::msg::Odometry::ConstSharedPtr input_msg);
-  void insertStopVelocity(const size_t closest_idx, TrajectoryPoints * traj);
-  bool convertPose(
-    const geometry_msgs::msg::Pose & pose, const std::string & source, const std::string & target,
-    const rclcpp::Time & time, geometry_msgs::msg::Pose & conv_pose);
-  bool getPose(
-    const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose);
-  void getNearestObstacle(double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
-  void getNearestObstacleByPointCloud(
-    double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
-  void getNearestObstacleByDynamicObject(
-    double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
-  bool isObstacleFound(const double min_dist_to_obj);
+  void onTimer();
+
+  void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
+
+  void onDynamicObjects(const PredictedObjects::ConstSharedPtr msg);
+
+  void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+
+  boost::optional<Obstacle> getNearestObstacle() const;
+
+  boost::optional<Obstacle> getNearestObstacleByPointCloud() const;
+
+  boost::optional<Obstacle> getNearestObstacleByDynamicObject() const;
+
+  boost::optional<geometry_msgs::msg::TransformStamped> getTransform(
+    const std::string & source, const std::string & target, const rclcpp::Time & stamp,
+    double duration_sec) const;
+
   bool isStopRequired(const bool is_obstacle_found, const bool is_stopped);
-  size_t getClosestIdx(const TrajectoryPoints & traj, const geometry_msgs::msg::Pose current_pose);
-  bool checkStop(const TrajectoryPoint & closest_point);
-  Polygon2d createSelfPolygon();
-  Polygon2d createObjPolygon(
-    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size);
-  Polygon2d createObjPolygon(
-    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint);
-  diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag(
-    const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose);
-  std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose);
-
-  /*
-   * ROS
-   */
+
+  bool isVehicleStopped();
+
+  // ros
+  mutable tf2_ros::Buffer tf_buffer_{get_clock()};
+  mutable tf2_ros::TransformListener tf_listener_{tf_buffer_};
+  rclcpp::TimerBase::SharedPtr timer_;
+
   // publisher and subscriber
-  rclcpp::Subscription<Trajectory>::SharedPtr path_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
-  rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr
-    dynamic_object_sub_;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr current_velocity_sub_;
-  rclcpp::Publisher<Trajectory>::SharedPtr path_pub_;
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr stop_reason_diag_pub_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pointcloud_;
+  rclcpp::Subscription<PredictedObjects>::SharedPtr sub_dynamic_objects_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odometry_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr pub_stop_reason_;
+  rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr pub_clear_velocity_limit_;
+  rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
+
+  // debug
   std::shared_ptr<SurroundObstacleCheckerDebugNode> debug_ptr_;
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_listener_;
 
   // parameter
-  nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_;
-  sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_;
-  autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr_;
+  NodeParam node_param_;
   vehicle_info_util::VehicleInfo vehicle_info_;
-  Polygon2d self_poly_;
-  bool use_pointcloud_;
-  bool use_dynamic_object_;
-  double surround_check_distance_;
-  // For preventing chattering,
-  // surround_check_recover_distance_ must be  bigger than surround_check_distance_
-  double surround_check_recover_distance_;
-  double state_clear_time_;
-  double stop_state_ego_speed_;
-  bool is_surround_obstacle_;
+
+  // data
+  nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr_;
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_;
+  PredictedObjects::ConstSharedPtr object_ptr_;
 
   // State Machine
   State state_ = State::PASS;
   std::shared_ptr<const rclcpp::Time> last_obstacle_found_time_;
+  std::shared_ptr<const rclcpp::Time> last_running_time_;
 };
+}  // namespace surround_obstacle_checker
 
 #endif  // SURROUND_OBSTACLE_CHECKER__NODE_HPP_
diff --git a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml
@@ -1,24 +1,20 @@
 <launch>
   <arg name="param_path" default="$(find-pkg-share surround_obstacle_checker)/config/surround_obstacle_checker.param.yaml"/>
 
-  <!-- vehicle info -->
-  <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
-
-  <arg name="input_trajectory" default="input/trajectory"/>
   <arg name="input_objects" default="/perception/object_recognition/objects"/>
   <arg name="input_odometry" default="/localization/kinematic_state"/>
   <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
-  <arg name="output_trajectory" default="output/trajectory"/>
+  <arg name="output_velocity_limit" default="/planning/scenario_planning/max_velocity_candidates"/>
+  <arg name="output_velocity_limit_clear_command" default="/planning/scenario_planning/clear_velocity_limit"/>
 
   <node pkg="surround_obstacle_checker" exec="surround_obstacle_checker_node" name="surround_obstacle_checker" output="screen">
     <param from="$(var param_path)"/>
-    <param from="$(var vehicle_info_param_file)"/>
     <remap from="~/output/no_start_reason" to="/planning/scenario_planning/status/no_start_reason"/>
     <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
-    <remap from="~/output/trajectory" to="$(var output_trajectory)"/>
+    <remap from="~/output/max_velocity" to="$(var output_velocity_limit)"/>
+    <remap from="~/output/velocity_limit_clear_command" to="$(var output_velocity_limit_clear_command)"/>
     <remap from="~/input/pointcloud" to="$(var input_pointcloud)"/>
     <remap from="~/input/objects" to="$(var input_objects)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
-    <remap from="~/input/trajectory" to="$(var input_trajectory)"/>
   </node>
 </launch>
diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml
@@ -14,6 +14,7 @@
 
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_tf2</depend>
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>libpcl-all-dev</depend>