Merge branch 'main' into fix/incorrect_counter_increment_in_cpu_usage…

…_monitor

NOTICE

@@ -6,3 +6,9 @@ The Autoware Foundation (https://www.autoware.org/).
 
 This product includes code developed by TIER IV.
 Copyright 2017 TIER IV, Inc.
+
+This product includes code developed by AutoCore.
+Copyright 2022 AutoCore Technology (Nanjing) Co., Ltd.
+
+This product includes code developed by Leo Drive.
+Copyright 2022 Leo Drive Teknoloji A.Ş.

common/bag_time_manager_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.14)
+project(bag_time_manager_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 ${rviz_QT_VERSION} EXACT 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/bag_time_manager_panel.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+)
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  icons
+  plugins
+)

common/bag_time_manager_rviz_plugin/README.md

@@ -0,0 +1,26 @@
+# bag_time_manager_rviz_plugin
+
+## Purpose
+
+This plugin allows publishing and controlling the ros bag time.
+
+## Output
+
+tbd.
+
+## HowToUse
+
+1. Start rviz and select panels/Add new panel.
+
+   ![select_panel](./images/select_panels.png)
+
+2. Select BagTimeManagerPanel and press OK.
+
+   ![select_manager_plugin](./images/add_bag_time_manager_panel.png)
+
+3. See bag_time_manager_rviz_plugin/BagTimeManagerPanel is added.
+
+   ![manager_plugin](./images/bag_time_manager_panel.png)
+
+- Pause/Resume: pause/resume the clock.
+- ApplyRate: apply rate of the clock.

common/bag_time_manager_rviz_plugin/package.xml

@@ -0,0 +1,28 @@
+<?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>bag_time_manager_rviz_plugin</name>
+  <version>0.0.1</version>
+  <description>Rviz plugin to publish and control the ros bag</description>
+  <maintainer email="[email protected]">Taiki Tanaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>rosbag2_interfaces</depend>
+  <depend>rviz_common</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>

common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="bag_time_manager_rviz_plugin">
+
+  <class
+    type="rviz_plugins::BagTimeManagerPanel"
+    base_class_type="rviz_common::Panel">
+    <description>Panel that publishes a service to modify its speed.</description>
+  </class>
+
+</library>

common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp

@@ -0,0 +1,117 @@
+//
+//  Copyright 2022 Tier IV, Inc. All rights reserved.
+//
+//  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 "bag_time_manager_panel.hpp"
+
+#include <qt5/QtWidgets/QHBoxLayout>
+#include <qt5/QtWidgets/QLabel>
+#include <qt5/QtWidgets/QWidget>
+#include <rviz_common/display_context.hpp>
+
+namespace rviz_plugins
+{
+BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  // pause / resume
+  {
+    pause_button_ = new QPushButton("Pause");
+    pause_button_->setToolTip("Pause/Resume ROS time.");
+    pause_button_->setStyleSheet("background-color: #00FF00;");
+    pause_button_->setCheckable(true);
+  }
+
+  // apply
+  {
+    apply_rate_button_ = new QPushButton("ApplyRate");
+    apply_rate_button_->setToolTip("control ROS time rate.");
+  }
+
+  // combo
+  {
+    rate_label_ = new QLabel("Rate:");
+    rate_label_->setAlignment(Qt::AlignCenter);
+    rate_combo_ = new QComboBox();
+    rate_combo_->addItems({"0.01", "0.1", "0.5", "1.0", "2.0", "5.0", "10.0"});
+    rate_combo_->setCurrentText(QString("1.0"));
+    time_label_ = new QLabel("X  real time ");
+    rate_label_->setAlignment(Qt::AlignCenter);
+  }
+
+  auto * layout = new QHBoxLayout();
+  layout->addWidget(pause_button_);
+  layout->addWidget(apply_rate_button_);
+  layout->addWidget(rate_label_);
+  layout->addWidget(rate_combo_);
+  layout->addWidget(time_label_);
+  setLayout(layout);
+
+  connect(pause_button_, SIGNAL(clicked()), this, SLOT(onPauseClicked()));
+  connect(apply_rate_button_, SIGNAL(clicked()), this, SLOT(onApplyRateClicked()));
+  connect(rate_combo_, SIGNAL(currentIndexChanged(int)), this, SLOT(onRateChanged()));
+}
+
+void BagTimeManagerPanel::onInitialize()
+{
+  raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  client_pause_ =
+    raw_node_->create_client<Pause>("/rosbag2_player/pause", rmw_qos_profile_services_default);
+  client_resume_ =
+    raw_node_->create_client<Resume>("/rosbag2_player/resume", rmw_qos_profile_services_default);
+  client_set_rate_ =
+    raw_node_->create_client<SetRate>("/rosbag2_player/set_rate", rmw_qos_profile_services_default);
+}
+
+void BagTimeManagerPanel::onPauseClicked()
+{
+  if (current_state_ == STATE::PAUSE) {
+    // do resume
+    current_state_ = STATE::RESUME;
+    pause_button_->setText(QString::fromStdString("Resume"));
+    // green
+    pause_button_->setStyleSheet("background-color: #00FF00;");
+    auto req = std::make_shared<Resume::Request>();
+    client_resume_->async_send_request(
+      req, [this]([[maybe_unused]] rclcpp::Client<Resume>::SharedFuture result) {});
+  } else {
+    // do pause
+    current_state_ = STATE::PAUSE;
+    pause_button_->setText(QString::fromStdString("Pause"));
+    // red
+    pause_button_->setStyleSheet("background-color: #FF0000;");
+    auto req = std::make_shared<Pause::Request>();
+    client_pause_->async_send_request(
+      req, [this]([[maybe_unused]] rclcpp::Client<Pause>::SharedFuture result) {});
+  }
+}
+
+void BagTimeManagerPanel::onApplyRateClicked()
+{
+  auto request = std::make_shared<SetRate::Request>();
+  request->rate = std::stod(rate_combo_->currentText().toStdString());
+  client_set_rate_->async_send_request(
+    request, [this, request](rclcpp::Client<SetRate>::SharedFuture result) {
+      const auto & response = result.get();
+      if (response->success) {
+        RCLCPP_INFO(raw_node_->get_logger(), "set ros bag rate %f x real time", request->rate);
+      } else {
+        RCLCPP_WARN(raw_node_->get_logger(), "service failed");
+      }
+    });
+}
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::BagTimeManagerPanel, rviz_common::Panel)

common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp

@@ -0,0 +1,72 @@
+//
+//  Copyright 2022 Tier IV, Inc. All rights reserved.
+//
+//  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 BAG_TIME_MANAGER_PANEL_HPP_
+#define BAG_TIME_MANAGER_PANEL_HPP_
+
+#include <qt5/QtWidgets/QComboBox>
+#include <qt5/QtWidgets/QLabel>
+#include <qt5/QtWidgets/QPushButton>
+#include <rclcpp/rclcpp.hpp>
+#include <rosbag2_interfaces/srv/pause.hpp>
+#include <rosbag2_interfaces/srv/resume.hpp>
+#include <rosbag2_interfaces/srv/set_rate.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <memory>
+#include <string>
+
+namespace rviz_plugins
+{
+using rosbag2_interfaces::srv::Pause;
+using rosbag2_interfaces::srv::Resume;
+using rosbag2_interfaces::srv::SetRate;
+class BagTimeManagerPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+public:
+  explicit BagTimeManagerPanel(QWidget * parent = nullptr);
+  void onInitialize() override;
+
+protected Q_SLOTS:
+  /// @brief callback for when the publishing rate is changed
+  void onRateChanged() {}
+  /// @brief callback for when the step button is clicked
+  void onPauseClicked();
+  void onApplyRateClicked();
+
+protected:
+  // ROS
+  rclcpp::Node::SharedPtr raw_node_;
+  rclcpp::Client<Pause>::SharedPtr client_pause_;
+  rclcpp::Client<Resume>::SharedPtr client_resume_;
+  rclcpp::Client<SetRate>::SharedPtr client_set_rate_;
+
+  // GUI
+  QPushButton * pause_button_;
+  QPushButton * apply_rate_button_;
+  QLabel * rate_label_;
+  QLabel * time_label_;
+  QComboBox * rate_combo_;
+
+private:
+  enum STATE { PAUSE, RESUME };
+  STATE current_state_{RESUME};
+};
+
+}  // namespace rviz_plugins
+
+#endif  // BAG_TIME_MANAGER_PANEL_HPP_

common/interpolation/README.md

@@ -12,7 +12,7 @@ Then it calculates interpolated values on y-axis for `query_keys` on x-axis.
 
 ## Spline Interpolation
 
-`slerp(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`.
+`spline(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`.
 Then it calculates interpolated values on y-axis for `query_keys` on x-axis.
 
 ### Evaluation of calculation cost

common/interpolation/include/interpolation/spherical_linear_interpolation.hpp

@@ -31,11 +31,11 @@
 
 namespace interpolation
 {
-geometry_msgs::msg::Quaternion spherical_linear_interpolation(
+geometry_msgs::msg::Quaternion slerp(
   const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat,
   const double ratio);
 
-std::vector<geometry_msgs::msg::Quaternion> spherical_linear_interpolation(
+std::vector<geometry_msgs::msg::Quaternion> slerp(
   const std::vector<double> & base_keys,
   const std::vector<geometry_msgs::msg::Quaternion> & base_values,
   const std::vector<double> & query_keys);

common/interpolation/include/interpolation/spline_interpolation.hpp

@@ -46,10 +46,10 @@ struct MultiSplineCoef
 };
 
 // static spline interpolation functions
-std::vector<double> slerp(
+std::vector<double> spline(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys);
-std::vector<double> slerpByAkima(
+std::vector<double> splineByAkima(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys);
 }  // namespace interpolation

common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp

@@ -21,8 +21,12 @@
 
 namespace interpolation
 {
+std::array<std::vector<double>, 3> slerp2dFromXY(
+  const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
+  const std::vector<double> & base_y_values, const std::vector<double> & query_keys);
+
 template <typename T>
-std::vector<double> slerpYawFromPoints(const std::vector<T> & points);
+std::vector<double> splineYawFromPoints(const std::vector<T> & points);
 }  // namespace interpolation
 
 // non-static points spline interpolation
@@ -66,8 +70,8 @@ class SplineInterpolationPoints2d
 
 private:
   void calcSplineCoefficientsInner(const std::vector<geometry_msgs::msg::Point> & points);
-  SplineInterpolation slerp_x_;
-  SplineInterpolation slerp_y_;
+  SplineInterpolation spline_x_;
+  SplineInterpolation spline_y_;
 
   std::vector<double> base_s_vec_;
 };

common/interpolation/include/interpolation/zero_order_hold.hpp

@@ -28,23 +28,7 @@ std::vector<T> zero_order_hold(
 {
   // throw exception for invalid arguments
   interpolation_utils::validateKeys(base_keys, query_keys);
-
-  // when vectors are empty
-  if (base_keys.empty() || base_values.empty()) {
-    throw std::invalid_argument("Points is empty.");
-  }
-
-  // when size of vectors are less than 2
-  if (base_keys.size() < 2 || base_values.size() < 2) {
-    throw std::invalid_argument(
-      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) +
-      ", base_values.size() = " + std::to_string(base_values.size()));
-  }
-
-  // when sizes of indices and values are not same
-  if (base_keys.size() != base_values.size()) {
-    throw std::invalid_argument("The size of base_keys and base_values are not the same.");
-  }
+  interpolation_utils::validateKeysAndValues(base_keys, base_values);
 
   std::vector<T> query_values;
   size_t closest_segment_idx = 0;