feat(default_ad_api): add door api

common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp

@@ -17,9 +17,12 @@
 
 #include <rclcpp/qos.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/door_status_array.hpp>
 #include <autoware_adapi_v1_msgs/msg/vehicle_kinematics.hpp>
 #include <autoware_adapi_v1_msgs/msg/vehicle_status.hpp>
+#include <autoware_adapi_v1_msgs/srv/get_door_layout.hpp>
 #include <autoware_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_door_command.hpp>
 
 namespace autoware_ad_api::vehicle
 {
@@ -48,6 +51,27 @@ struct Dimensions
   static constexpr char name[] = "/api/vehicle/dimensions";
 };
 
+struct DoorCommand
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetDoorCommand;
+  static constexpr char name[] = "/api/vehicle/doors/command";
+};
+
+struct DoorLayout
+{
+  using Service = autoware_adapi_v1_msgs::srv::GetDoorLayout;
+  static constexpr char name[] = "/api/vehicle/doors/layout";
+};
+
+struct DoorStatus
+{
+  using Message = autoware_adapi_v1_msgs::msg::DoorStatusArray;
+  static constexpr char name[] = "/api/vehicle/doors/status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
 }  // namespace autoware_ad_api::vehicle
 
 #endif  // AUTOWARE_AD_API_SPECS__VEHICLE_HPP_

common/component_interface_specs/include/component_interface_specs/vehicle.hpp

@@ -17,6 +17,9 @@
 
 #include <rclcpp/qos.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/door_status_array.hpp>
+#include <autoware_adapi_v1_msgs/srv/get_door_layout.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_door_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
@@ -71,6 +74,27 @@ struct EnergyStatus
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
 };
 
+struct DoorCommand
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetDoorCommand;
+  static constexpr char name[] = "/vehicle/doors/command";
+};
+
+struct DoorLayout
+{
+  using Service = autoware_adapi_v1_msgs::srv::GetDoorLayout;
+  static constexpr char name[] = "/vehicle/doors/layout";
+};
+
+struct DoorStatus
+{
+  using Message = autoware_adapi_v1_msgs::msg::DoorStatusArray;
+  static constexpr char name[] = "/vehicle/doors/status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
 }  // namespace vehicle_interface
 
 #endif  // COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_

common/tier4_adapi_rviz_plugin/CMakeLists.txt

@@ -12,6 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/route_tool.cpp
   src/route_panel.cpp
+  src/door_panel.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml

@@ -8,4 +8,8 @@
     <description>RoutePanel</description>
   </class>
 
+  <class type="tier4_adapi_rviz_plugins::DoorPanel" base_class_type="rviz_common::Panel">
+    <description>DoorPanel</description>
+  </class>
+
 </library>

common/tier4_adapi_rviz_plugin/src/door_panel.cpp

@@ -0,0 +1,118 @@
+//  Copyright 2023 The Autoware Contributors
+//
+//  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.
+
+#include "door_panel.hpp"
+
+#include <rviz_common/display_context.hpp>
+
+#include <memory>
+
+namespace tier4_adapi_rviz_plugins
+{
+
+DoorPanel::DoorPanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  status_ = new QLabel("Trying to get door layout.");
+  layout_ = new QGridLayout();
+  layout_->addWidget(status_, 0, 0, 1, 4);
+  setLayout(layout_);
+}
+
+void DoorPanel::onInitialize()
+{
+  const auto on_layout = [this](const rclcpp::Client<DoorLayout::Service>::SharedFuture future) {
+    const auto res = future.get();
+    if (!res->status.success) {
+      status_->setText(QString::fromStdString("failed to get layout: " + res->status.message));
+      return;
+    }
+    const auto & layouts = res->doors;
+    for (size_t index = 0; index < layouts.size(); ++index) {
+      const auto & layout = layouts[index];
+      DoorUI door;
+      door.description = new QLabel(QString::fromStdString(layout.description));
+      door.status = new QLabel("unknown");
+      door.open = new QPushButton("open");
+      door.close = new QPushButton("close");
+      doors_.push_back(door);
+
+      layout_->addWidget(door.description, index + 1, 0);
+      layout_->addWidget(door.status, index + 1, 1);
+      layout_->addWidget(door.open, index + 1, 2);
+      layout_->addWidget(door.close, index + 1, 3);
+
+      using Command = autoware_adapi_v1_msgs::msg::DoorCommand;
+      const auto on_open = [this, index] { on_button(index, Command::OPEN); };
+      const auto on_close = [this, index] { on_button(index, Command::CLOSE); };
+      connect(door.open, &QPushButton::clicked, on_open);
+      connect(door.close, &QPushButton::clicked, on_close);
+    }
+    status_->hide();
+  };
+
+  const auto on_status = [this](const DoorStatus::Message::ConstSharedPtr msg) {
+    using Status = autoware_adapi_v1_msgs::msg::DoorStatus;
+    if (doors_.size() != msg->doors.size()) {
+      return;
+    }
+    for (size_t index = 0; index < doors_.size(); ++index) {
+      const auto & label = doors_[index].status;
+      switch (msg->doors[index].status) {
+        case Status::NOT_AVAILABLE:
+          label->setText("not available");
+          break;
+        case Status::OPENED:
+          label->setText("opened");
+          break;
+        case Status::CLOSED:
+          label->setText("closed");
+          break;
+        case Status::OPENING:
+          label->setText("opening");
+          break;
+        case Status::CLOSING:
+          label->setText("closing");
+          break;
+        default:
+          label->setText("unknown");
+          break;
+      }
+    }
+  };
+
+  auto lock = getDisplayContext()->getRosNodeAbstraction().lock();
+  auto node = lock->get_raw_node();
+
+  const auto adaptor = component_interface_utils::NodeAdaptor(node.get());
+  adaptor.init_cli(cli_command_);
+  adaptor.init_cli(cli_layout_);
+  adaptor.init_sub(sub_status_, on_status);
+
+  const auto req = std::make_shared<DoorLayout::Service::Request>();
+  cli_layout_->async_send_request(req, on_layout);
+}
+
+void DoorPanel::on_button(uint32_t index, uint8_t command)
+{
+  const auto req = std::make_shared<DoorCommand::Service::Request>();
+  req->doors.resize(1);
+  req->doors.back().index = index;
+  req->doors.back().command = command;
+  cli_command_->async_send_request(req);
+}
+
+}  // namespace tier4_adapi_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::DoorPanel, rviz_common::Panel)

common/tier4_adapi_rviz_plugin/src/door_panel.hpp

@@ -0,0 +1,64 @@
+//  Copyright 2023 The Autoware Contributors
+//
+//  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 DOOR_PANEL_HPP_
+#define DOOR_PANEL_HPP_
+
+#include <QGridLayout>
+#include <QLabel>
+#include <QPushButton>
+#include <autoware_ad_api_specs/vehicle.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <vector>
+
+namespace tier4_adapi_rviz_plugins
+{
+
+class DoorPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+  using DoorCommand = autoware_ad_api::vehicle::DoorCommand;
+  using DoorLayout = autoware_ad_api::vehicle::DoorLayout;
+  using DoorStatus = autoware_ad_api::vehicle::DoorStatus;
+
+public:
+  explicit DoorPanel(QWidget * parent = nullptr);
+  void onInitialize() override;
+
+private:
+  component_interface_utils::Client<DoorCommand>::SharedPtr cli_command_;
+  component_interface_utils::Client<DoorLayout>::SharedPtr cli_layout_;
+  component_interface_utils::Subscription<DoorStatus>::SharedPtr sub_status_;
+
+  struct DoorUI
+  {
+    QLabel * description;
+    QLabel * status;
+    QPushButton * open;
+    QPushButton * close;
+  };
+  QGridLayout * layout_;
+  QLabel * status_;
+  std::vector<DoorUI> doors_;
+
+private slots:
+  void on_button(uint32_t index, uint8_t command);
+};
+
+}  // namespace tier4_adapi_rviz_plugins
+
+#endif  // DOOR_PANEL_HPP_

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -10,6 +10,7 @@
   <arg name="launch_dummy_perception"/>
   <arg name="launch_dummy_vehicle"/>
   <arg name="launch_dummy_localization"/>
+  <arg name="launch_dummy_doors"/>
   <arg name="launch_diagnostic_converter"/>
   <arg name="vehicle_info_param_file"/>
 
@@ -132,6 +133,11 @@
     </include>
   </group>
 
+  <!-- Dummy doors -->
+  <group if="$(var launch_dummy_doors)">
+    <include file="$(find-pkg-share vehicle_door_simulator)/launch/vehicle_door_simulator.launch.xml"/>
+  </group>
+
   <!-- Diagnostic Converter -->
   <group if="$(var launch_diagnostic_converter)">
     <node name="diagnostic_converter" exec="diagnostic_converter" pkg="diagnostic_converter" output="screen">

simulator/vehicle_door_simulator/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.14)
+project(vehicle_door_simulator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(dummy_doors
+  src/dummy_doors.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE launch)

simulator/vehicle_door_simulator/README.md

@@ -0,0 +1,3 @@
+# vehicle_door_simulator
+
+This package is for testing operations on vehicle devices such as doors.

simulator/vehicle_door_simulator/launch/vehicle_door_simulator.launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="vehicle_door_simulator" exec="dummy_doors" name="dummy_doors">
+    <remap from="~/doors/command" to="/vehicle/doors/command"/>
+    <remap from="~/doors/layout" to="/vehicle/doors/layout"/>
+    <remap from="~/doors/status" to="/vehicle/doors/status"/>
+  </node>
+</launch>

simulator/vehicle_door_simulator/package.xml

@@ -0,0 +1,22 @@
+<?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>vehicle_door_simulator</name>
+  <version>0.1.0</version>
+  <description>The vehicle_door_simulator package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>