Merge branch 'main' into feat/z_fitting_by_vectormap

Author: isamu-takagi

common/tier4_adapi_rviz_plugin/README.md

@@ -9,3 +9,21 @@ Enable or disable of allow_goal_modification option can be set with the check bo
 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.
+
+## Material Design Icons
+
+This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0.
+
+Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products.
+
+### License
+
+The Material Design Icons are licensed under the Apache License, Version 2.0. 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.
+
+### Acknowledgments
+
+We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects.

common/tier4_adapi_rviz_plugin/src/route_panel.cpp

@@ -14,11 +14,13 @@
 
 #include "route_panel.hpp"
 
+#include <QGridLayout>
 #include <QHBoxLayout>
 #include <QVBoxLayout>
 #include <rviz_common/display_context.hpp>
 
 #include <memory>
+#include <string>
 
 namespace tier4_adapi_rviz_plugins
 {
@@ -28,6 +30,11 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
   waypoints_mode_ = new QPushButton("mode");
   waypoints_reset_ = new QPushButton("reset");
   waypoints_apply_ = new QPushButton("apply");
+  adapi_clear_ = new QPushButton("clear");
+  adapi_set_ = new QPushButton("set");
+  adapi_change_ = new QPushButton("change");
+  adapi_response_ = new QLabel("the response will be displayed here");
+  adapi_auto_clear_ = new QCheckBox("auto clear");
   allow_goal_modification_ = new QCheckBox("allow goal modification");
 
   waypoints_mode_->setCheckable(true);
@@ -37,6 +44,22 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
   connect(waypoints_reset_, &QPushButton::clicked, this, &RoutePanel::onWaypointsReset);
   connect(waypoints_apply_, &QPushButton::clicked, this, &RoutePanel::onWaypointsApply);
 
+  const auto buttons = new QButtonGroup(this);
+  buttons->addButton(adapi_set_);
+  buttons->addButton(adapi_change_);
+  buttons->setExclusive(true);
+  adapi_set_->setCheckable(true);
+  adapi_change_->setCheckable(true);
+  adapi_response_->setAlignment(Qt::AlignCenter);
+
+  connect(adapi_clear_, &QPushButton::clicked, this, &RoutePanel::clearRoute);
+  connect(adapi_set_, &QPushButton::clicked, [this] { adapi_mode_ = AdapiMode::Set; });
+  connect(adapi_change_, &QPushButton::clicked, [this] { adapi_mode_ = AdapiMode::Change; });
+
+  adapi_auto_clear_->setChecked(true);
+  adapi_set_->setChecked(true);
+  adapi_mode_ = AdapiMode::Set;
+
   const auto layout = new QVBoxLayout();
   setLayout(layout);
 
@@ -52,6 +75,19 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
     waypoints_group_ = group;
   }
 
+  // adapi group
+  {
+    const auto group = new QGroupBox("adapi");
+    const auto local = new QGridLayout();
+    local->addWidget(adapi_clear_, 0, 0);
+    local->addWidget(adapi_set_, 0, 1);
+    local->addWidget(adapi_change_, 0, 2);
+    local->addWidget(adapi_auto_clear_, 1, 0);
+    local->addWidget(adapi_response_, 1, 1, 1, 2);
+    group->setLayout(local);
+    layout->addWidget(group);
+  }
+
   // options group
   {
     const auto group = new QGroupBox("options");
@@ -73,25 +109,14 @@ void RoutePanel::onInitialize()
 
   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);
-  });
+  adaptor.init_cli(cli_set_);
+  adaptor.init_cli(cli_change_);
 }
 
 void RoutePanel::onPose(const PoseStamped::ConstSharedPtr msg)
 {
   if (!waypoints_mode_->isChecked()) {
-    setRoute(*msg);
+    requestRoute(*msg);
   } else {
     waypoints_.push_back(*msg);
     waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size()));
@@ -120,18 +145,74 @@ 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 call = [this] {
     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);
+    asyncSendRequest(req);
     onWaypointsReset();
-  });
+  };
+
+  if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) {
+    const auto req = std::make_shared<ClearRoute::Service::Request>();
+    cli_clear_->async_send_request(req, [call](auto) { call(); });
+  } else {
+    call();
+  }
+}
+
+void RoutePanel::requestRoute(const PoseStamped & pose)
+{
+  const auto call = [this, pose] {
+    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();
+    asyncSendRequest(req);
+  };
+
+  if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) {
+    const auto req = std::make_shared<ClearRoute::Service::Request>();
+    cli_clear_->async_send_request(req, [call](auto) { call(); });
+  } else {
+    call();
+  }
+}
+
+void RoutePanel::clearRoute()
+{
+  const auto callback = [this](auto future) {
+    const auto status = future.get()->status;
+    std::string text = status.success ? "OK" : "Error";
+    text += "[" + std::to_string(status.code) + "]";
+    text += " " + status.message;
+    adapi_response_->setText(QString::fromStdString(text));
+  };
+
+  const auto req = std::make_shared<ClearRoute::Service::Request>();
+  cli_clear_->async_send_request(req, callback);
+}
+
+void RoutePanel::asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req)
+{
+  const auto callback = [this](auto future) {
+    const auto status = future.get()->status;
+    std::string text = status.success ? "OK" : "Error";
+    text += "[" + std::to_string(status.code) + "]";
+    text += " " + status.message;
+    adapi_response_->setText(QString::fromStdString(text));
+  };
+
+  if (adapi_mode_ == AdapiMode::Set) {
+    cli_set_->async_send_request(req, callback);
+  }
+  if (adapi_mode_ == AdapiMode::Change) {
+    cli_change_->async_send_request(req, callback);
+  }
 }
 
 }  // namespace tier4_adapi_rviz_plugins

common/tier4_adapi_rviz_plugin/src/route_panel.hpp

@@ -15,8 +15,10 @@
 #ifndef ROUTE_PANEL_HPP_
 #define ROUTE_PANEL_HPP_
 
+#include <QButtonGroup>
 #include <QCheckBox>
 #include <QGroupBox>
+#include <QLabel>
 #include <QPushButton>
 #include <autoware_ad_api_specs/routing.hpp>
 #include <component_interface_utils/rclcpp.hpp>
@@ -35,6 +37,7 @@ class RoutePanel : public rviz_common::Panel
   Q_OBJECT
   using ClearRoute = autoware_ad_api::routing::ClearRoute;
   using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints;
+  using ChangeRoutePoints = autoware_ad_api::routing::ChangeRoutePoints;
   using PoseStamped = geometry_msgs::msg::PoseStamped;
 
 public:
@@ -45,18 +48,29 @@ class RoutePanel : public rviz_common::Panel
   QPushButton * waypoints_mode_;
   QPushButton * waypoints_reset_;
   QPushButton * waypoints_apply_;
+  QPushButton * adapi_clear_;
+  QPushButton * adapi_set_;
+  QPushButton * adapi_change_;
+  QLabel * adapi_response_;
+  QCheckBox * adapi_auto_clear_;
   QGroupBox * waypoints_group_;
   QCheckBox * allow_goal_modification_;
 
   rclcpp::Subscription<PoseStamped>::SharedPtr sub_pose_;
   std::vector<PoseStamped> waypoints_;
   void onPose(const PoseStamped::ConstSharedPtr msg);
 
+  enum AdapiMode { Set, Change };
+  AdapiMode adapi_mode_;
+
   component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
-  component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
-  void setRoute(const PoseStamped & pose);
+  component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_set_;
+  component_interface_utils::Client<ChangeRoutePoints>::SharedPtr cli_change_;
+  void requestRoute(const PoseStamped & pose);
+  void asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req);
 
 private slots:
+  void clearRoute();
   void onWaypointsMode(bool clicked);
   void onWaypointsReset();
   void onWaypointsApply();

common/tier4_automatic_goal_rviz_plugin/README.md

@@ -78,3 +78,21 @@ In this situation, check the terminal output for more information.
 
 - Often it is enough to try again.
 - Sometimes a clearing of the current route is required before retrying.
+
+## Material Design Icons
+
+This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0.
+
+Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products.
+
+### License
+
+The Material Design Icons are licensed under the Apache License, Version 2.0. 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.
+
+### Acknowledgments
+
+We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects.

common/tier4_camera_view_rviz_plugin/README.md

@@ -7,3 +7,21 @@ Add the `tier4_camera_view_rviz_plugin/ThirdPersonViewTool` tool to the RViz. Pu
 ## BirdEyeView Tool
 
 Add the `tier4_camera_view_rviz_plugin/BirdEyeViewTool` tool to the RViz. Push the button, the camera will turn to the BEV view, the target frame is consistent with the latest frame. Short cut key 'r'.
+
+## Material Design Icons
+
+This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0.
+
+Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products.
+
+### License
+
+The Material Design Icons are licensed under the Apache License, Version 2.0. 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.
+
+### Acknowledgments
+
+We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects.

common/tier4_perception_rviz_plugin/README.md

@@ -112,3 +112,21 @@ You can interactively manipulate the object.
 | ADD    | Shift + Click Right Button             |
 | MOVE   | Hold down Right Button + Drug and Drop |
 | DELETE | Alt + Click Right Button               |
+
+## Material Design Icons
+
+This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0.
+
+Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products.
+
+### License
+
+The Material Design Icons are licensed under the Apache License, Version 2.0. 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.
+
+### Acknowledgments
+
+We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects.

common/tier4_planning_rviz_plugin/README.md

@@ -125,3 +125,21 @@ This plugin displays the path, trajectory, and maximum speed.
    ![select_planning_plugin](./images/select_planning_plugin.png)
 3. Enter the name of the topic where you want to view the path or trajectory.
    ![select_topic_name](./images/select_topic_name.png)
+
+## Material Design Icons
+
+This project uses [Material Design Icons](https://developers.google.com/fonts/docs/material_symbols) by Google. These icons are used under the terms of the Apache License, Version 2.0.
+
+Material Design Icons are a collection of symbols provided by Google that are used to enhance the user interface of applications, websites, and other digital products.
+
+### License
+
+The Material Design Icons are licensed under the Apache License, Version 2.0. 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.
+
+### Acknowledgments
+
+We would like to express our gratitude to Google for making these icons available to the community, helping developers and designers enhance the visual appeal and user experience of their projects.

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -41,6 +41,7 @@
 
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -140,10 +141,9 @@ class AEB : public rclcpp::Node
   bool hasCollision(
     const double current_v, const Path & ego_path, const std::vector<ObjectData> & objects);
 
-  void generateEgoPath(
-    const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons);
-  void generateEgoPath(
-    const Trajectory & predicted_traj, Path & path, std::vector<Polygon2d> & polygons);
+  Path generateEgoPath(const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons);
+  std::optional<Path> generateEgoPath(
+    const Trajectory & predicted_traj, std::vector<Polygon2d> & polygons);
   void createObjectData(
     const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
     std::vector<ObjectData> & objects);

control/autonomous_emergency_braking/package.xml

@@ -6,6 +6,7 @@
   <description>Autonomous Emergency Braking package as a ROS 2 node</description>
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
 
   <license>Apache License 2.0</license>