Merge pull request #544 from tier4/sync-upstream

chore: sync upstream

common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp

@@ -33,7 +33,7 @@ struct InitializationState
 {
   using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
   static constexpr char name[] = "/api/localization/initialization_state";
-  static constexpr size_t depth = 3;
+  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;
 };

common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp

@@ -60,7 +60,7 @@ struct RouteState
 {
   using Message = autoware_adapi_v1_msgs::msg::RouteState;
   static constexpr char name[] = "/api/routing/state";
-  static constexpr size_t depth = 3;
+  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;
 };

common/component_interface_specs/include/component_interface_specs/control.hpp

@@ -19,7 +19,9 @@
 
 #include <tier4_control_msgs/msg/is_paused.hpp>
 #include <tier4_control_msgs/msg/is_start_requested.hpp>
+#include <tier4_control_msgs/msg/is_stopped.hpp>
 #include <tier4_control_msgs/srv/set_pause.hpp>
+#include <tier4_control_msgs/srv/set_stop.hpp>
 
 namespace control_interface
 {
@@ -48,6 +50,21 @@ struct IsStartRequested
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
 };
 
+struct SetStop
+{
+  using Service = tier4_control_msgs::srv::SetStop;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/set_stop";
+};
+
+struct IsStopped
+{
+  using Message = tier4_control_msgs::msg::IsStopped;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/is_stopped";
+  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 control_interface
 
 #endif  // COMPONENT_INTERFACE_SPECS__CONTROL_HPP_

common/component_interface_specs/include/component_interface_specs/localization.hpp

@@ -34,7 +34,7 @@ struct InitializationState
 {
   using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
   static constexpr char name[] = "/localization/initialization_state";
-  static constexpr size_t depth = 3;
+  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;
 };

common/component_interface_specs/include/component_interface_specs/planning.hpp

@@ -61,7 +61,7 @@ struct RouteState
 {
   using Message = autoware_adapi_v1_msgs::msg::RouteState;
   static constexpr char name[] = "/planning/mission_planning/route_state";
-  static constexpr size_t depth = 3;
+  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;
 };

common/component_interface_utils/README.md

@@ -30,7 +30,7 @@ struct SampleMessage
 {
   using Message = sample_msgs::msg::MessageType;
   static constexpr char name[] = "/sample/message";
-  static constexpr size_t depth = 3;
+  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;
 };

common/tier4_adapi_rviz_plugin/CMakeLists.txt

@@ -8,10 +8,10 @@ find_package(Qt5 REQUIRED Core Widgets)
 set(QT_LIBRARIES Qt5::Widgets)
 set(CMAKE_AUTOMOC ON)
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
-add_definitions(-DQT_NO_KEYWORDS)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/route_tool.cpp
+  src/route_panel.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

common/tier4_adapi_rviz_plugin/README.md

@@ -0,0 +1,11 @@
+# tier4_adapi_rviz_plugin
+
+## RoutePanel
+
+To use the panel, set the topic name from 2D Goal Pose Tool to `/rviz/routing/pose`.
+By default, when a tool publish a pose, the panel immediately sets a route with that as the goal.
+Enable or disable of allow_goal_modification option can be set with the check box.
+
+Push the mode button in the waypoint to enter waypoint mode. In this mode, the pose is added to waypoints.
+Press the apply button to set the route using the saved waypoints (the last one is a goal).
+Reset the saved waypoints with the reset button.

common/tier4_adapi_rviz_plugin/package.xml

@@ -12,6 +12,9 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_ad_api_specs</depend>
+  <depend>component_interface_utils</depend>
+  <depend>geometry_msgs</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>

common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml

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

common/tier4_adapi_rviz_plugin/src/route_panel.cpp

@@ -0,0 +1,140 @@
+//  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 "route_panel.hpp"
+
+#include <QHBoxLayout>
+#include <QVBoxLayout>
+#include <rviz_common/display_context.hpp>
+
+#include <memory>
+
+namespace tier4_adapi_rviz_plugins
+{
+
+RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  waypoints_mode_ = new QPushButton("mode");
+  waypoints_reset_ = new QPushButton("reset");
+  waypoints_apply_ = new QPushButton("apply");
+  allow_goal_modification_ = new QCheckBox("allow goal modification");
+
+  waypoints_mode_->setCheckable(true);
+  waypoints_reset_->setDisabled(true);
+  waypoints_apply_->setDisabled(true);
+  connect(waypoints_mode_, &QPushButton::clicked, this, &RoutePanel::onWaypointsMode);
+  connect(waypoints_reset_, &QPushButton::clicked, this, &RoutePanel::onWaypointsReset);
+  connect(waypoints_apply_, &QPushButton::clicked, this, &RoutePanel::onWaypointsApply);
+
+  const auto layout = new QVBoxLayout();
+  setLayout(layout);
+
+  // waypoints group
+  {
+    const auto group = new QGroupBox("waypoints");
+    const auto local = new QHBoxLayout();
+    local->addWidget(waypoints_mode_);
+    local->addWidget(waypoints_reset_);
+    local->addWidget(waypoints_apply_);
+    group->setLayout(local);
+    layout->addWidget(group);
+    waypoints_group_ = group;
+  }
+
+  // options group
+  {
+    const auto group = new QGroupBox("options");
+    const auto local = new QHBoxLayout();
+    local->addWidget(allow_goal_modification_);
+    group->setLayout(local);
+    layout->addWidget(group);
+  }
+}
+
+void RoutePanel::onInitialize()
+{
+  auto lock = getDisplayContext()->getRosNodeAbstraction().lock();
+  auto node = lock->get_raw_node();
+
+  sub_pose_ = node->create_subscription<PoseStamped>(
+    "/rviz/routing/pose", rclcpp::QoS(1),
+    std::bind(&RoutePanel::onPose, this, std::placeholders::_1));
+
+  const auto adaptor = component_interface_utils::NodeAdaptor(node.get());
+  adaptor.init_cli(cli_clear_);
+  adaptor.init_cli(cli_route_);
+}
+
+void RoutePanel::setRoute(const PoseStamped & pose)
+{
+  const auto req = std::make_shared<ClearRoute::Service::Request>();
+  cli_clear_->async_send_request(req, [this, pose](auto) {
+    const auto req = std::make_shared<SetRoutePoints::Service::Request>();
+    req->header = pose.header;
+    req->goal = pose.pose;
+    req->option.allow_goal_modification = allow_goal_modification_->isChecked();
+    cli_route_->async_send_request(req);
+  });
+}
+
+void RoutePanel::onPose(const PoseStamped::ConstSharedPtr msg)
+{
+  if (!waypoints_mode_->isChecked()) {
+    setRoute(*msg);
+  } else {
+    waypoints_.push_back(*msg);
+    waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size()));
+  }
+}
+
+void RoutePanel::onWaypointsMode(bool clicked)
+{
+  waypoints_reset_->setEnabled(clicked);
+  waypoints_apply_->setEnabled(clicked);
+
+  if (clicked) {
+    onWaypointsReset();
+  } else {
+    waypoints_group_->setTitle("waypoints");
+  }
+}
+
+void RoutePanel::onWaypointsReset()
+{
+  waypoints_.clear();
+  waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size()));
+}
+
+void RoutePanel::onWaypointsApply()
+{
+  if (waypoints_.empty()) return;
+
+  const auto req = std::make_shared<ClearRoute::Service::Request>();
+  cli_clear_->async_send_request(req, [this](auto) {
+    const auto req = std::make_shared<SetRoutePoints::Service::Request>();
+    req->header = waypoints_.back().header;
+    req->goal = waypoints_.back().pose;
+    for (size_t i = 0; i + 1 < waypoints_.size(); ++i) {
+      req->waypoints.push_back(waypoints_[i].pose);
+    }
+    req->option.allow_goal_modification = allow_goal_modification_->isChecked();
+    cli_route_->async_send_request(req);
+    onWaypointsReset();
+  });
+}
+
+}  // namespace tier4_adapi_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::RoutePanel, rviz_common::Panel)

common/tier4_adapi_rviz_plugin/src/route_panel.hpp

@@ -0,0 +1,67 @@
+//  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 ROUTE_PANEL_HPP_
+#define ROUTE_PANEL_HPP_
+
+#include <QCheckBox>
+#include <QGroupBox>
+#include <QPushButton>
+#include <autoware_ad_api_specs/routing.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <vector>
+
+namespace tier4_adapi_rviz_plugins
+{
+
+class RoutePanel : public rviz_common::Panel
+{
+  Q_OBJECT
+  using ClearRoute = autoware_ad_api::routing::ClearRoute;
+  using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints;
+  using PoseStamped = geometry_msgs::msg::PoseStamped;
+
+public:
+  explicit RoutePanel(QWidget * parent = nullptr);
+  void onInitialize() override;
+
+private:
+  QPushButton * waypoints_mode_;
+  QPushButton * waypoints_reset_;
+  QPushButton * waypoints_apply_;
+  QGroupBox * waypoints_group_;
+  QCheckBox * allow_goal_modification_;
+
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_pose_;
+  std::vector<PoseStamped> waypoints_;
+  void onPose(const PoseStamped::ConstSharedPtr msg);
+
+  component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
+  component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
+  void setRoute(const PoseStamped & pose);
+
+private slots:
+  void onWaypointsMode(bool clicked);
+  void onWaypointsReset();
+  void onWaypointsApply();
+};
+
+}  // namespace tier4_adapi_rviz_plugins
+
+#endif  // ROUTE_PANEL_HPP_

control/vehicle_cmd_gate/CMakeLists.txt

@@ -8,6 +8,7 @@ ament_auto_add_library(vehicle_cmd_gate_node SHARED
   src/vehicle_cmd_gate.cpp
   src/vehicle_cmd_filter.cpp
   src/pause_interface.cpp
+  src/moderate_stop_interface.cpp
 )
 
 rclcpp_components_register_node(vehicle_cmd_gate_node

control/vehicle_cmd_gate/README.md

@@ -52,6 +52,7 @@
 | `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency                                              |
 | `stop_hold_acceleration`                    | double | longitudinal acceleration cmd when vehicle should stop                      |
 | `emergency_acceleration`                    | double | longitudinal acceleration cmd when vehicle stop with emergency              |
+| `moderate_stop_service_acceleration`        | double | longitudinal acceleration cmd when vehicle stop with moderate stop service  |
 | `nominal.vel_lim`                           | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode)     |
 | `nominal.lon_acc_lim`                       | double | limit of longitudinal acceleration (activated in AUTONOMOUS operation mode) |
 | `nominal.lon_jerk_lim`                      | double | limit of longitudinal jerk (activated in AUTONOMOUS operation mode)         |

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -8,6 +8,7 @@
     external_emergency_stop_heartbeat_timeout: 0.0
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4
+    moderate_stop_service_acceleration: -1.5
     stopped_state_entry_duration_time: 0.1
     nominal:
       vel_lim: 25.0