feat: add scenario selector package (autowarefoundation#41)

* release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "remove ROS1 packages temporarily" This reverts commit 7f877f288151212d5ee578f070fc7e11bd15ad9f. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Rename launch files to launch.xml (autowarefoundation#28) * ROS2 Porting: scenario_selector (autowarefoundation#101) * Add cmake and package.xml - Remove colcon ignore * First pass: - Remove pubs and subs - Remove lane esoteric lanelet method calls * Add parameter fetching and setting * Add publishers, logging and transforms - Fix createCallback * Remove duration field from lookuptransform * Add topic_tools dependency - launching of rostopic in ROS1 hasn't been ported? * Remove comments from cmake * Remove boost dependency * Remove test parameters * Address PR comments: - Add const reference back - Use initializer list for parameter fetching - Fix fromBinMap reference - Launch publishing from executable tag in launch file * Address PR comment: - Fix launch file launching of ros2 topic publisher * Rename h files to hpp (autowarefoundation#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * fixing trasient_local in ROS2 packages (autowarefoundation#160) * [scenario_selector_node] fix timer frequency (autowarefoundation#182) Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Add linters to scenario_selector (autowarefoundation#157) * Added linters to scenario_selector * Removed duplicate dependencies * Add linters to CMake * Only add cppcheck as linter * Ros2 v0.8.0 scenario selector (autowarefoundation#294) * add use_sim-time option (autowarefoundation#454) * Format launch files (autowarefoundation#1219) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * update first tf (autowarefoundation#1223) * update first tf Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * update Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * Make planning modules components (autowarefoundation#1263) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * change trajectory callback implementation (autowarefoundation#1338) (autowarefoundation#1355) * change trajectory callback implementation * change name of callback function * remove unnecessary function * improve readability Co-authored-by: satoshi-ota <satoshi.ota@gmail.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota <satoshi.ota@gmail.com> * Select LaneDriving scenario if vehicle is in lane and goal is in lane (autowarefoundation#1459) (autowarefoundation#1485) Signed-off-by: Azumi Suzuki <azumi.suzuki@tier4.jp> Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: s-azumi <38061530+s-azumi@users.noreply.github.com> * Add markdownlint and prettier (autowarefoundation#1661) * Add markdownlint and prettier Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore .param.yaml Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * port scenario_selector (autowarefoundation#482) * port scenario_selector * remove COLCON_IGNORE * use odom * add nav_msgs dependency Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> * [apply_predicted_obj_type] adapt to autoware auto msgs (autowarefoundation#564) * fix obj shape * fix obj shape * fix goal pose * rename topic name twist -> odometry (autowarefoundation#568) Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> * update iv_msgs -> auto_msgs in planning readme (autowarefoundation#576) * update iv_msgs -> auto_msgs in planning readme * minor change * some fix * some fix Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> * fix scenario selector (autowarefoundation#632) * ci(pre-commit): autofix * fix: package title Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com> Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Esteve Fernandez <esteve@apache.org> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota <satoshi.ota@gmail.com> Co-authored-by: s-azumi <38061530+s-azumi@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
mitsudome-r · Dec 6, 2021 · ed10202 · ed10202
1 parent 744d734
commit ed10202
Show file tree

Hide file tree

Showing 8 changed files with 743 additions and 0 deletions.
diff --git a/planning/scenario_selector/CMakeLists.txt b/planning/scenario_selector/CMakeLists.txt
@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.5)
+project(scenario_selector)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+## Dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Target
+
+## Target executable
+set(SCENARIO_SELECTOR_SRC
+  src/scenario_selector_node/scenario_selector_node.cpp
+)
+
+## scenario_selector_node
+ament_auto_add_library(scenario_selector_node SHARED
+  ${SCENARIO_SELECTOR_SRC}
+)
+
+rclcpp_components_register_node(scenario_selector_node
+  PLUGIN "ScenarioSelectorNode"
+  EXECUTABLE scenario_selector
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)
diff --git a/planning/scenario_selector/README.md b/planning/scenario_selector/README.md
@@ -0,0 +1,121 @@
+# scenario_selector
+
+## scenario_selector_node
+
+`scenario_selector_node` is a node that switches trajectories from each scenario.
+
+### Input topics
+
+| Name                             | Type                                     | Description                                           |
+| -------------------------------- | ---------------------------------------- | ----------------------------------------------------- |
+| `~input/lane_driving/trajectory` | autoware_auto_planning_msgs::Trajectory  | trajectory of LaneDriving scenario                    |
+| `~input/parking/trajectory`      | autoware_auto_planning_msgs::Trajectory  | trajectory of Parking scenario                        |
+| `~input/lanelet_map`             | autoware_auto_mapping_msgs::HADMapBin    |                                                       |
+| `~input/route`                   | autoware_auto_planning_msgs::HADMapRoute | route and goal pose                                   |
+| `~input/odometry`                | nav_msgs::Odometry                       | for checking whether vehicle is stopped               |
+| `is_parking_completed`           | bool (implemented as rosparam)           | whether all split trajectory of Parking are published |
+
+### Output topics
+
+| Name                 | Type                                    | Description                                    |
+| -------------------- | --------------------------------------- | ---------------------------------------------- |
+| `~output/scenario`   | autoware_planning_msgs::Scenario        | current scenario and scenarios to be activated |
+| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed                      |
+
+### Output TFs
+
+None
+
+### How to launch
+
+1. Write your remapping info in `scenario_selector.launch` or add args when executing `roslaunch`
+2. `roslaunch scenario_selector scenario_selector.launch`
+   - If you would like to use only a single scenario, `roslaunch scenario_selector dummy_scenario_selector_{scenario_name}.launch`
+
+### Parameters
+
+| Parameter                  | Type   | Description                                                                     |
+| -------------------------- | ------ | ------------------------------------------------------------------------------- |
+| `update_rate`              | double | timer's update rate                                                             |
+| `th_max_message_delay_sec` | double | threshold time of input messages' maximum delay                                 |
+| `th_arrived_distance_m`    | double | threshold distance to check if vehicle has arrived at the trajectory's endpoint |
+| `th_stopped_time_sec`      | double | threshold time to check if vehicle is stopped                                   |
+| `th_stopped_velocity_mps`  | double | threshold velocity to check if vehicle is stopped                               |
+
+### Flowchart
+
+```plantuml
+@startuml
+title onTimer
+start
+
+:get current pose;
+
+if (all input data are ready?) then (yes)
+else (no)
+  stop
+endif
+
+if (scenario is initialized?) then (yes)
+else (no)
+  :initialize scenario;
+endif
+
+:select scenario;
+
+:publish scenario;
+
+:extract scenario trajectory;
+
+if (scenario trajectory is empty?) then (yes)
+else (no)
+  :publish trajectory;
+endif
+
+stop
+@enduml
+```
+
+```plantuml
+@startuml
+title Scenario Transition
+start
+
+if (current_scenario is completed?\n()) then (yes)
+else (no)
+  stop
+endif
+
+' Empty
+if (scenario is initialized?) then (yes)
+else (no)
+  if (is in lane?) then (yes)
+    :set LaneDriving;
+  else (no)
+    :set Parking;
+  endif
+
+  stop
+endif
+
+' LaneDriving
+if (current scenario is LaneDriving?) then (yes)
+  if (is in parking lot & goal is not in lane?) then (yes)
+    :set Parking;
+    stop
+  endif
+endif
+
+' Parking
+if (current scenario is Parking?) then (yes)
+  if (parking is completed and is in lane?) then (yes)
+    :set LaneDriving;
+    stop
+  endif
+endif
+
+:continue previous scenario;
+
+stop
+@enduml
+```
diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp
@@ -0,0 +1,95 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_
+#define SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_planning_msgs/msg/scenario.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_traffic_rules/TrafficRules.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <deque>
+#include <memory>
+#include <string>
+
+class ScenarioSelectorNode : public rclcpp::Node
+{
+public:
+  explicit ScenarioSelectorNode(const rclcpp::NodeOptions & node_options);
+
+  void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
+  void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg);
+  void onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+
+  void onTimer();
+  void onLaneDrivingTrajectory(
+    const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
+  void onParkingTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
+  void publishTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
+
+  void updateCurrentScenario();
+  std::string selectScenarioByPosition();
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory(
+    const std::string & scenario);
+
+private:
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_lanelet_map_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr sub_route_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    sub_lane_driving_trajectory_;
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
+    sub_parking_trajectory_;
+  rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_;
+  rclcpp::Publisher<autoware_planning_msgs::msg::Scenario>::SharedPtr pub_scenario_;
+
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_;
+  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_;
+  autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr route_;
+  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
+  geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_;
+
+  std::string current_scenario_;
+  std::deque<geometry_msgs::msg::TwistStamped::ConstSharedPtr> twist_buffer_;
+
+  std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
+  std::shared_ptr<lanelet::routing::RoutingGraph> routing_graph_ptr_;
+  std::shared_ptr<lanelet::traffic_rules::TrafficRules> traffic_rules_ptr_;
+
+  // Parameters
+  double update_rate_;
+  double th_max_message_delay_sec_;
+  double th_arrived_distance_m_;
+  double th_stopped_time_sec_;
+  double th_stopped_velocity_mps_;
+};
+
+#endif  // SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_
diff --git a/planning/scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml b/planning/scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml
@@ -0,0 +1,20 @@
+<launch>
+  <arg name="input_lane_driving_trajectory" />
+  <arg name="output_scenario" />
+  <arg name="output_trajectory" />
+
+  <arg name="input_parking_trajectory" default=""/>
+  <arg name="input_lanelet_map" default="" />
+  <arg name="input_route" default="" />
+  <arg name="input_odometry" default="" />
+  <arg name="is_parking_completed" default="" />
+
+  <node pkg="topic_tools" exec="relay" name="scenario_trajectory_relay" output="log">
+    <param name="input_topic" value="$(var input_lane_driving_trajectory)" />
+    <param name="output_topic" value="$(var output_trajectory)" />
+    <param name="type" value="autoware_planning_msgs/msg/Trajectory" />
+  </node>
+
+  <arg name="cmd" default="ros2 topic pub $(var output_scenario) autoware_planning_msgs/msg/Scenario '{current_scenario: LaneDriving, activating_scenarios: [LaneDriving]}'" />
+  <executable cmd="$(var cmd)" name="scenario_pub" output="screen" shell="true"/>
+</launch>
diff --git a/planning/scenario_selector/launch/dummy_scenario_selector_parking.launch.xml b/planning/scenario_selector/launch/dummy_scenario_selector_parking.launch.xml
@@ -0,0 +1,20 @@
+<launch>
+  <arg name="input_parking_trajectory" />
+  <arg name="output_scenario" />
+  <arg name="output_trajectory" />
+
+  <arg name="input_lane_driving_trajectory" default="" />
+  <arg name="input_lanelet_map" default="" />
+  <arg name="input_route" default="" />
+  <arg name="input_odometry" default="" />
+  <arg name="is_parking_completed" default="" />
+
+  <node pkg="topic_tools" exec="relay" name="scenario_trajectory_relay" output="log">
+    <param name="input_topic" value="$(var input_parking_trajectory)" />
+    <param name="output_topic" value="$(var output_trajectory)" />
+    <param name="type" value="autoware_planning_msgs/msg/Trajectory" />
+  </node>
+
+  <arg name="cmd" default="ros2 topic pub $(var output_scenario) autoware_planning_msgs/msg/Scenario '{current_scenario: Parking, activating_scenarios: [Parking]}'" />
+  <executable cmd="$(var cmd)" name="scenario_pub" output="screen" shell="true"/>
+</launch>
diff --git a/planning/scenario_selector/launch/scenario_selector.launch.xml b/planning/scenario_selector/launch/scenario_selector.launch.xml
@@ -0,0 +1,29 @@
+<launch>
+  <arg name="input_lane_driving_trajectory" />
+  <arg name="input_parking_trajectory" />
+  <arg name="input_lanelet_map" />
+  <arg name="input_route" />
+  <arg name="input_odometry" />
+  <arg name="is_parking_completed" />
+
+  <arg name="output_scenario" />
+  <arg name="output_trajectory" />
+
+  <node pkg="scenario_selector" exec="scenario_selector" name="scenario_selector" output="screen">
+    <remap from="input/lane_driving/trajectory" to="$(var input_lane_driving_trajectory)"/>
+    <remap from="input/parking/trajectory" to="$(var input_parking_trajectory)"/>
+    <remap from="input/lanelet_map" to="$(var input_lanelet_map)"/>
+    <remap from="input/route" to="$(var input_route)"/>
+    <remap from="input/odometry" to="$(var input_odometry)"/>
+    <remap from="is_parking_completed" to="$(var is_parking_completed)"/>
+
+    <remap from="output/scenario" to="$(var output_scenario)"/>
+    <remap from="output/trajectory" to="$(var output_trajectory)"/>
+
+    <param name="update_rate" value="10.0" />
+    <param name="th_max_message_delay_sec" value="1.0" />
+    <param name="th_arrived_distance_m" value="1.0" />
+    <param name="th_stopped_time_sec" value="1.0" />
+    <param name="th_stopped_velocity_mps" value="0.01" />
+  </node>
+</launch>
diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>scenario_selector</name>
+  <version>0.1.0</version>
+  <description>The scenario_selector ROS2 package</description>
+  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <exec_depend>ros2cli</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>