feat: add external api adaptor for calibration status

autoware_external_api_msgs/package.xml

@@ -1,7 +1,6 @@
 <?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>autoware_external_api_msgs</name>
   <version>0.0.0</version>
   <description>The autoware_external_api_msgs package</description>
@@ -26,5 +25,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-
 </package>

autoware_iv_external_api_adaptor/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/calibration_status.cpp
   src/cpu_usage.cpp
   src/diagnostics.cpp
   src/door.cpp
@@ -34,6 +35,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/velocity.cpp
   src/version.cpp
 )
+rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::CalibrationStatus")
 rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::CpuUsage")
 rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::Diagnostics")
 rclcpp_components_register_nodes(${PROJECT_NAME} "external_api::Door")

autoware_iv_external_api_adaptor/package.xml

@@ -1,7 +1,6 @@
 <?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>autoware_iv_external_api_adaptor</name>
   <version>0.0.0</version>
   <description>The autoware_iv_external_api_adaptor package</description>
@@ -31,5 +30,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-
 </package>

autoware_iv_external_api_adaptor/src/calibration_status.cpp

@@ -0,0 +1,86 @@
+// Copyright 2021 TIER IV, Inc.
+//
+// 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 "calibration_status.hpp"
+
+#include <memory>
+
+namespace external_api
+{
+
+CalibrationStatus::CalibrationStatus(const rclcpp::NodeOptions & options)
+: Node("calibration_status", options)
+{
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+  tier4_api_utils::ServiceProxyNodeInterface proxy(this);
+
+  group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  srv_get_accel_brake_map_calibration_data_ =
+    proxy.create_service<tier4_external_api_msgs::srv::GetAccelBrakeMapCalibrationData>(
+      "/api/external/get/accel_brake_map_calibrator/data",
+      std::bind(&CalibrationStatus::getAccelBrakeMapCalibrationData, this, _1, _2),
+      rmw_qos_profile_services_default, group_);
+  cli_get_accel_brake_map_calibration_data_ =
+    proxy.create_client<tier4_external_api_msgs::srv::GetAccelBrakeMapCalibrationData>(
+      "/accel_brake_map_calibrator/get_data_service", rmw_qos_profile_services_default);
+
+  using namespace std::literals::chrono_literals;
+
+  pub_calibration_status_ = create_publisher<tier4_external_api_msgs::msg::CalibrationStatusArray>(
+    "/api/external/get/calibration_status", rclcpp::QoS(1));
+  timer_ =
+    rclcpp::create_timer(this, get_clock(), 200ms, std::bind(&CalibrationStatus::onTimer, this));
+
+  sub_accel_brake_map_calibration_status_ =
+    create_subscription<tier4_external_api_msgs::msg::CalibrationStatus>(
+      "/accel_brake_map_calibrator/output/calibration_status", rclcpp::QoS(1),
+      [this](const tier4_external_api_msgs::msg::CalibrationStatus::ConstSharedPtr msg) {
+        accel_brake_map_status_ = msg;
+      });
+}
+
+void CalibrationStatus::onTimer()
+{
+  tier4_external_api_msgs::msg::CalibrationStatusArray calibration_status;
+
+  calibration_status.stamp = now();
+  if (accel_brake_map_status_ != nullptr) {
+    calibration_status.status_array.emplace_back(*accel_brake_map_status_);
+    accel_brake_map_status_ = nullptr;
+  }
+
+  if (!calibration_status.status_array.empty()) {
+    pub_calibration_status_->publish(calibration_status);
+  }
+}
+
+void CalibrationStatus::getAccelBrakeMapCalibrationData(
+  const tier4_external_api_msgs::srv::GetAccelBrakeMapCalibrationData::Request::SharedPtr request,
+  const tier4_external_api_msgs::srv::GetAccelBrakeMapCalibrationData::Response::SharedPtr response)
+{
+  const auto [status, resp] =
+    cli_get_accel_brake_map_calibration_data_->call(request, std::chrono::seconds(190));
+  if (!tier4_api_utils::is_success(status)) {
+    return;
+  }
+  response->graph_image = resp->graph_image;
+  response->accel_map = resp->accel_map;
+  response->brake_map = resp->brake_map;
+}
+
+}  // namespace external_api
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(external_api::CalibrationStatus)

autoware_iv_external_api_adaptor/src/calibration_status.hpp

@@ -0,0 +1,61 @@
+// Copyright 2021 TIER IV, Inc.
+//
+// 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 CALIBRATION_STATUS_HPP_
+#define CALIBRATION_STATUS_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_api_utils/tier4_api_utils.hpp"
+
+#include "tier4_external_api_msgs/msg/calibration_status_array.hpp"
+#include "tier4_external_api_msgs/srv/get_accel_brake_map_calibration_data.hpp"
+
+namespace external_api
+{
+
+class CalibrationStatus : public rclcpp::Node
+{
+public:
+  explicit CalibrationStatus(const rclcpp::NodeOptions & options);
+
+private:
+  using GetAccelBrakeMapCalibrationData =
+    tier4_external_api_msgs::srv::GetAccelBrakeMapCalibrationData;
+
+  // ros interface
+  rclcpp::CallbackGroup::SharedPtr group_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  tier4_api_utils::Service<GetAccelBrakeMapCalibrationData>::SharedPtr
+    srv_get_accel_brake_map_calibration_data_;
+  tier4_api_utils::Client<GetAccelBrakeMapCalibrationData>::SharedPtr
+    cli_get_accel_brake_map_calibration_data_;
+  rclcpp::Publisher<tier4_external_api_msgs::msg::CalibrationStatusArray>::SharedPtr
+    pub_calibration_status_;
+  rclcpp::Subscription<tier4_external_api_msgs::msg::CalibrationStatus>::SharedPtr
+    sub_accel_brake_map_calibration_status_;
+
+  // ros callback
+  void onTimer();
+  void getAccelBrakeMapCalibrationData(
+    const tier4_external_api_msgs::srv::GetAccelBrakeMapCalibrationData::Request::SharedPtr request,
+    const tier4_external_api_msgs::srv::GetAccelBrakeMapCalibrationData::Response::SharedPtr
+      response);
+
+  // calibration status
+  tier4_external_api_msgs::msg::CalibrationStatus::ConstSharedPtr accel_brake_map_status_;
+};
+
+}  // namespace external_api
+
+#endif  // CALIBRATION_STATUS_HPP_

autoware_iv_internal_api_adaptor/package.xml

@@ -1,7 +1,6 @@
 <?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>autoware_iv_internal_api_adaptor</name>
   <version>0.0.0</version>
   <description>The autoware_iv_internal_api_adaptor package</description>
@@ -10,31 +9,36 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>std_srvs</depend>
   <depend>tier4_api_utils</depend>
+  <depend>tier4_auto_msgs_converter</depend>
   <depend>tier4_control_msgs</depend>
   <depend>tier4_external_api_msgs</depend>
   <depend>tier4_localization_msgs</depend>
+  <depend>tier4_perception_msgs</depend>
   <depend>tier4_planning_msgs</depend>
+  <depend>tier4_system_msgs</depend>
+  <depend>tier4_vehicle_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 
   <!-- for backward compatibility -->
-  <!-- nolint --> <depend>autoware_auto_perception_msgs</depend>
-  <!-- nolint --> <depend>autoware_auto_system_msgs</depend>
-  <!-- nolint --> <depend>tier4_perception_msgs</depend>
-  <!-- nolint --> <depend>tier4_system_msgs</depend>
-  <!-- nolint --> <depend>tier4_vehicle_msgs</depend>
-  <!-- nolint --> <depend>tier4_auto_msgs_converter</depend>
+  <!-- nolint -->
+  <!-- nolint -->
+  <!-- nolint -->
+  <!-- nolint -->
+  <!-- nolint -->
+  <!-- nolint -->
 
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-
 </package>

awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml

@@ -143,5 +143,4 @@
   </group>
 
   <include file="$(find-pkg-share awapi_awiv_adapter)/launch/awapi_relay_container.launch.py"/>
-
 </launch>