feat(auto_parking): automated valet parking implementation

common/tier4_state_rviz_plugin/package.xml

@@ -20,6 +20,7 @@
   <depend>qtbase5-dev</depend>
   <depend>rclcpp</depend>
   <depend>rviz_common</depend>
+  <depend>std_srvs</depend>
   <depend>tier4_control_msgs</depend>
   <depend>tier4_external_api_msgs</depend>
   <depend>tier4_planning_msgs</depend>

common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp

@@ -61,6 +61,7 @@
   auto * velocity_limit_layout = new QHBoxLayout();
   v_layout->addWidget(makeOperationModeGroup());
   v_layout->addWidget(makeControlModeGroup());
+  v_layout->addWidget(makeAutoParkModeGroup());
   {
     auto * h_layout = new QHBoxLayout();
     h_layout->addWidget(makeRoutingGroup());
@@ -137,6 +138,25 @@
   return group;
 }
 
+QGroupBox * AutowareStatePanel::makeAutoParkModeGroup()
+{
+  auto * group = new QGroupBox("AutoParking");
+  auto * grid = new QGridLayout;
+
+  park_mode_label_ptr_ = new QLabel("INIT");
+  park_mode_label_ptr_->setAlignment(Qt::AlignCenter);
+  park_mode_label_ptr_->setStyleSheet("border:1px solid black;");
+  grid->addWidget(park_mode_label_ptr_, 0, 0);
+
+  p_enable_button_ptr_ = new QPushButton("Enable");
+  p_enable_button_ptr_->setCheckable(true);
+  connect(p_enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickEngageAutoPark()));
+  grid->addWidget(p_enable_button_ptr_, 0, 1);
+
+  group->setLayout(grid);
+  return group;
+}
+
 QGroupBox * AutowareStatePanel::makeRoutingGroup()
 {
   auto * group = new QGroupBox("Routing");
@@ -222,6 +242,9 @@
   sub_operation_mode_ = raw_node_->create_subscription<OperationModeState>(
     "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareStatePanel::onOperationMode, this, _1));
+  sub_auto_park_status_ = raw_node_->create_subscription<std_msgs::msg::Bool>(
+    "/planning/auto_parking/status", rclcpp::QoS{1}.transient_local(),
+    std::bind(&AutowareStatePanel::onAutoParkStatus, this, _1));
 
   client_change_to_autonomous_ =
     raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/change_to_autonomous");
@@ -241,6 +264,9 @@
   client_enable_direct_control_ =
     raw_node_->create_client<ChangeOperationMode>("/api/operation_mode/disable_autoware_control");
 
+  client_auto_park_ =
+    raw_node_->create_client<std_srvs::srv::SetBool>("/planning/auto_parking/set_status");
+
   // Routing
   sub_route_ = raw_node_->create_subscription<RouteState>(
     "/api/routing/state", rclcpp::QoS{1}.transient_local(),
@@ -352,6 +378,27 @@
   changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled);
 }
 
+void AutowareStatePanel::onAutoParkStatus(const std_msgs::msg::Bool::ConstSharedPtr msg)
+{
+  auto_park_running_ = msg->data;
+  auto changeButtonState = [this](
+                             QPushButton * button, const bool is_desired_mode_available,
+                             const uint8_t current_mode = OperationModeState::UNKNOWN,
+                             const uint8_t desired_mode = OperationModeState::STOP) {
+    if (is_desired_mode_available && current_mode != desired_mode) {
+      activateButton(button);
+    } else {
+      deactivateButton(button);
+    }
+  };
+  if (auto_park_running_) {
+    updateLabel(park_mode_label_ptr_, "Enable", "background-color: #00FF00;");  // green
+  } else {
+    updateLabel(park_mode_label_ptr_, "Disable", "background-color: #FFFF00;");  // yellow
+  }
+  changeButtonState(p_enable_button_ptr_, !auto_park_running_);
+}
+
 void AutowareStatePanel::onRoute(const RouteState::ConstSharedPtr msg)
 {
   QString text = "";
@@ -501,7 +548,7 @@
         text = "NONE";
         style_sheet = "background-color: #00FF00;";  // green
         break;
 
       case MRMState::COMFORTABLE_STOP:
         text = "COMFORTABLE_STOP";
         style_sheet = "background-color: #FFFF00;";  // yellow
@@ -571,6 +618,9 @@
 void AutowareStatePanel::onClickStop()
 {
   callServiceWithoutResponse<ChangeOperationMode>(client_change_to_stop_);
+  if (auto_park_running_) {
+    onClickDisengageAutoPark();
+  }
 }
 void AutowareStatePanel::onClickLocal()
 {
@@ -584,6 +634,30 @@
 {
   callServiceWithoutResponse<ChangeOperationMode>(client_enable_autoware_control_);
 }
+void AutowareStatePanel::onClickEngageAutoPark()
+{
+  auto req = std::make_shared<std_srvs::srv::SetBool::Request>();
+  req->data = true;
+  RCLCPP_DEBUG(raw_node_->get_logger(), "client request");
+  if (!client_auto_park_->service_is_ready()) {
+    RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
+    return;
+  }
+  client_auto_park_->async_send_request(
+    req, [this]([[maybe_unused]] rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture result) {});
+}
+void AutowareStatePanel::onClickDisengageAutoPark()
+{
+  auto req = std::make_shared<std_srvs::srv::SetBool::Request>();
+  req->data = false;
+  RCLCPP_DEBUG(raw_node_->get_logger(), "client request");
+  if (!client_auto_park_->service_is_ready()) {
+    RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
+    return;
+  }
+  client_auto_park_->async_send_request(
+    req, [this]([[maybe_unused]] rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture result) {});
+}
 void AutowareStatePanel::onClickDirectControl()
 {
   callServiceWithoutResponse<ChangeOperationMode>(client_enable_direct_control_);

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -36,6 +36,8 @@
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_srvs/srv/set_bool.hpp>
 #include <tier4_external_api_msgs/msg/emergency.hpp>
 #include <tier4_external_api_msgs/srv/set_emergency.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
@@ -69,6 +71,8 @@ public Q_SLOTS:  // NOLINT for Qt
   void onClickLocal();
   void onClickRemote();
   void onClickAutowareControl();
+  void onClickEngageAutoPark();
+  void onClickDisengageAutoPark();
   void onClickDirectControl();
   void onClickClearRoute();
   void onClickInitByGnss();
@@ -80,6 +84,7 @@ public Q_SLOTS:  // NOLINT for Qt
   // Layout
   QGroupBox * makeOperationModeGroup();
   QGroupBox * makeControlModeGroup();
+  QGroupBox * makeAutoParkModeGroup();
   QGroupBox * makeRoutingGroup();
   QGroupBox * makeLocalizationGroup();
   QGroupBox * makeMotionGroup();
@@ -106,6 +111,7 @@ public Q_SLOTS:  // NOLINT for Qt
   QPushButton * remote_button_ptr_{nullptr};
 
   rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_auto_park_status_;
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_stop_;
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_local_;
@@ -118,6 +124,13 @@ public Q_SLOTS:  // NOLINT for Qt
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_enable_autoware_control_;
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_enable_direct_control_;
 
+  //// Auto Parking Mode
+  QLabel * park_mode_label_ptr_{nullptr};
+  QPushButton * p_enable_button_ptr_{nullptr};
+  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_auto_park_;
+  bool auto_park_running_;
+  void onAutoParkStatus(const std_msgs::msg::Bool::ConstSharedPtr msg);
+
   //// Functions
   void onOperationMode(const OperationModeState::ConstSharedPtr msg);
   void changeOperationMode(const rclcpp::Client<ChangeOperationMode>::SharedPtr client);

launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml

@@ -31,7 +31,27 @@
         <param from="$(var freespace_planner_param_path)"/>
         <extra_arg name="use_intra_process_comms" value="false"/>
       </composable_node>
+      <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+    </node_container>
+  </group>
 
+  <!-- auto parking module -->
+  <group if="$(var launch_avp_module)">
+    <push-ros-namespace namespace="avp"/>
+    <node_container pkg="rclcpp_components" exec="component_container" name="auto_parking_container" namespace="">
+      <composable_node pkg="auto_parking" plugin="auto_parking::AutoParkingNode" name="auto_parking" namespace="">
+        <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+        <remap from="~/input/occupancy_grid" to="/planning/scenario_planning/parking/costmap_generator/occupancy_grid"/>
+        <remap from="~/input/lanelet_map_bin" to="/map/vector_map"/>
+        <remap from="~/input/engage" to="/api/autoware/get/engage"/>
+        <remap from="~/service/engage" to="/api/autoware/set/engage"/>
+        <remap from="~/output/fixed_goal" to="/planning/mission_planning/goal"/>
+        <remap from="~/output/active_status" to="/planning/auto_parking/status"/>
+        <remap from="~/service/set_active" to="/planning/auto_parking/set_status"/>
+        <!-- params -->
+        <param from="$(var vehicle_param_file)"/>
+        <param from="$(var auto_parking_param_path)"/>
+      </composable_node>
       <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
     </node_container>
   </group>

planning/auto_parking/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.14)
+project(auto_parking)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(auto_parking_node SHARED
+  src/auto_parking/auto_parking_node.cpp
+)
+
+rclcpp_components_register_node(auto_parking_node
+  PLUGIN "auto_parking::AutoParkingNode"
+  EXECUTABLE auto_parking)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

planning/auto_parking/README.md

@@ -0,0 +1,108 @@
+# auto_parking
+
+## auto_parking_node
+
+`auto_parking_node` is a node that enables automated valet parking by publishing appropriate goal poses towards an empty parking space in the nearest parking lot.
+
+The auto parking is done in the following two phases.
+
+Phase 1:
+Depending on initial pose when the auto parking feature is engaged, a goal pose is set at the exit of the nearest parking lot. This pose is found by first getting nearest parking lot, the parking spaces inside and lanelets inside the parking lot from the lanelet map. By ordering the lanelets based on routing distance from current position, the exit lanelet is found. Ideally the auto_parking feature should be engaged from outside parking lot so the ordering is proper. The ego vehicle then drives to parking lot exit lanelets, passing through the parking lot using either LANEDRIVING or PARKING scenario based on start pose.
+
+Phase 2:
+Once inside the parking lot, while driving to exit lanelet goal, the node searches for a free parking space using the astar algorithm. Once a free space is found a new goal pose is published and trajectory is generated by the freespace_planner as scenario is PARKING.
+
+### Input topics
+
+| Name                      | Type                                    | Description                           |
+| ------------------------- | --------------------------------------- | ------------------------------------- |
+| `~/input/engage`          | autoware_auto_vehicle_msgs::msg::Engage | status of autoware AUTONOMOUS         |
+| `~/input/lanelet_map_bin` | autoware_auto_mapping_msgs::HADMapBin   | vector map, for drivable areas        |
+| `~/input/occupancy_grid`  | nav_msgs::msg::OccupancyGrid            | occupancy grid from costmap generator |
+| `~input/odometry`         | nav_msgs::Odometry                      | for getting current pose              |
+
+### Output topics
+
+| Name                     | Type                            | Description                            |
+| ------------------------ | ------------------------------- | -------------------------------------- |
+| `~/output/fixed_goal`    | geometry_msgs::msg::PoseStamped | current goal published by auto_parking |
+| `~/output/active_status` | std_msgs::msg::Bool             | active status of auto_park node        |
+
+### Service
+
+| Name                   | Type                   | Description               |
+| ---------------------- | ---------------------- | ------------------------- |
+| `~/service/set_active` | std_srvs::srv::SetBool | set active status of node |
+
+### Client
+
+| Name               | Type                                 | Description                      |
+| ------------------ | ------------------------------------ | -------------------------------- |
+| `~/service/engage` | tier4_external_api_msgs::srv::Engage | client to engage AUTONOMOUS mode |
+
+### How to launch
+
+1. Write your remapping info in `auto-parking.launch.xml` or add args when executing `roslaunch`
+2. Launch node using `ros2 launch auto-parking auto-parking.launch.xml`
+3. Call service to start auto parking after placing ego on map `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: True}"`
+4. To stop/reset call with False arg `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: False}"`
+
+### Parameters
+
+{{ json_to_markdown("schema/auto_parking.schema.json") }}
+
+### Flowchart
+
+```plantuml
+@startuml
+title onTimer
+start
+
+if (active?) then (yes)
+else (no)
+  stop
+endif
+
+if (all input data are ready?) then (yes)
+else (no)
+  :set active false;
+  stop
+endif
+
+:get current pose;
+
+if (parking lot goal not set and set AutoParking init?) then (yes):
+  :publish parking lot goal;
+else (no)
+  stop
+endif
+
+if (parking lot goal set and Arrived?) then (yes):
+  :set active false;
+  stop
+endif
+
+if (parking space goal not set and inside ParkingLot?) then (yes):
+  :findParkingSapce;
+  :publish parking space goal;
+endif
+
+if (parking space goal set and inside ParkingLot?) then (yes):
+  :check if parking space goal still valid;
+  if (not valid) then (yes):
+    :publish parking lot goal;
+    :continue search;
+  endif
+endif
+
+if (parking space goal set and Arrived?) then (yes):
+  stop
+endif
+
+if (autonomous not engaged?) then (yes):
+  :engage autonomous;
+endif
+
+stop
+@enduml
+```

planning/auto_parking/config/auto_parking.param.yaml

@@ -0,0 +1,31 @@
+/**:
+  ros__parameters:
+    # -- Auto Park Node Configurations --
+    th_arrived_distance_m: 1.0
+    th_parking_space_distance_m: 10.0
+    update_rate: 5.0
+    vehicle_shape_margin_m: 0.2
+
+    # -- Configurations common to the all planners --
+    # base configs
+    time_limit: 30000.0
+    minimum_turning_radius: 5.0
+    maximum_turning_radius: 9.0
+    turning_radius_size: 3
+    # search configs
+    theta_size: 144
+    angle_goal_range: 6.0
+    curve_weight: 1.2
+    reverse_weight: 2.0
+    lateral_goal_range: 0.5
+    longitudinal_goal_range: 2.0
+    # costmap configs
+    obstacle_threshold: 100
+
+    # -- A* search Configurations --
+    astar:
+      only_behind_solutions: false
+      use_back: true
+      use_curve_weight: false
+      use_complete_astar: true
+      distance_heuristic_weight: 1.0