feat(default_ad_api): add localization api (#1082)

* new branch

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (WIP) add new messages and publisher for the messages

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* (WIP) Add relay node, but build error occurs.

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* run pre-commit

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* fix topic name

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* pull the latest foundation main

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* change message type

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* Change message name from MatchingScore to ReferenceValue. Modify localizatoin api adapter

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* ci(pre-commit): autofix

* delete unnecessary include

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* add some libraries for ci

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* ci(pre-commit): autofix

* pull the latest foundation/main

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* Rename and change component of localization score messages.

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* pull the latest main

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* pull the latest foundation/main

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

* fix along PR comments and pull the latest foundation main

Signed-off-by: TakumiKozaka-T4 <takumi.kozaka@tier4.jp>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

common/autoware_ad_api_msgs/CMakeLists.txt

@@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  localization/msg/LocalizationScoreArray.msg
+  localization/msg/LocalizationScore.msg
   common/msg/ResponseStatus.msg
   interface/srv/InterfaceVersion.srv
   DEPENDENCIES

common/autoware_ad_api_msgs/localization/msg/LocalizationScore.msg

@@ -0,0 +1,2 @@
+string name
+float32 value

common/autoware_ad_api_msgs/localization/msg/LocalizationScoreArray.msg

@@ -0,0 +1 @@
+autoware_ad_api_msgs/LocalizationScore[] values

system/default_ad_api/CMakeLists.txt

@@ -6,9 +6,11 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/interface.cpp
+  src/localization_score.cpp
 )
 
 rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::InterfaceNode")
+rclcpp_components_register_nodes(${PROJECT_NAME}  "default_ad_api::LocalizationScoreNode")
 
 if(BUILD_TESTING)
   add_launch_test(test/main.test.py)

system/default_ad_api/launch/default_ad_api.launch.py

@@ -31,6 +31,7 @@ def _create_api_node(node_name, class_name, **kwargs):
 def generate_launch_description():
     components = [
         _create_api_node("interface", "InterfaceNode"),
+        _create_api_node("localization_score", "LocalizationScoreNode"),
     ]
     container = ComposableNodeContainer(
         namespace="default_ad_api",

system/default_ad_api/package.xml

@@ -13,8 +13,10 @@
 
   <depend>autoware_ad_api_msgs</depend>
   <depend>component_interface_utils</depend>
+  <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_debug_msgs</depend>
 
   <exec_depend>python3-flask</exec_depend>
 

system/default_ad_api/src/localization_score.cpp

@@ -0,0 +1,88 @@
+// Copyright 2022 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 "localization_score.hpp"
+
+namespace default_ad_api
+{
+
+LocalizationScoreNode::LocalizationScoreNode(const rclcpp::NodeOptions & options)
+: Node("localization_score", options), clock_(this->get_clock())
+{
+  using std::placeholders::_1;
+
+  status_pub_hz_ = this->declare_parameter("status_pub_hz", 10.0);
+
+  score_tp_.name = "transform_probability";
+  score_nvtl_.name = "nearest_voxel_transformation_likelihood";
+  is_tp_received_ = false;
+  is_nvtl_received_ = false;
+
+  // Publisher
+  pub_localization_scores_ =
+    this->create_publisher<LocalizationScoreArray>("api/get/localization_scores", 1);
+
+  // Subscriber
+  sub_transform_probability_ = this->create_subscription<Float32Stamped>(
+    "/localization/pose_estimator/transform_probability", 1,
+    std::bind(&LocalizationScoreNode::callbackTpScore, this, _1));
+  sub_nearest_voxel_transformation_likelihood_ = this->create_subscription<Float32Stamped>(
+    "/localization/pose_estimator/nearest_voxel_transformation_likelihood", 1,
+    std::bind(&LocalizationScoreNode::callbackNvtlScore, this, _1));
+
+  // Timer callback
+  auto timer_callback = std::bind(&LocalizationScoreNode::callbackTimer, this);
+  auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
+    std::chrono::duration<double>(1.0 / status_pub_hz_));
+  // timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
+  //   this->get_clock(), period, std::move(timer_callback),
+  //   this->get_node_base_interface()->get_context());
+  // this->get_node_timers_interface()->add_timer(timer_, nullptr);
+  timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(timer_callback));
+}
+
+void LocalizationScoreNode::callbackTpScore(const Float32Stamped::ConstSharedPtr msg_ptr)
+{
+  is_tp_received_ = true;
+  score_tp_.value = msg_ptr->data;
+}
+void LocalizationScoreNode::callbackNvtlScore(const Float32Stamped::ConstSharedPtr msg_ptr)
+{
+  is_nvtl_received_ = true;
+  score_nvtl_.value = msg_ptr->data;
+}
+
+void LocalizationScoreNode::callbackTimer()
+{
+  LocalizationScoreArray localizatoin_scores_msg;
+
+  if (is_tp_received_) {
+    localizatoin_scores_msg.values.emplace_back(score_tp_);
+    is_tp_received_ = false;
+  }
+
+  if (is_nvtl_received_) {
+    localizatoin_scores_msg.values.emplace_back(score_nvtl_);
+    is_nvtl_received_ = false;
+  }
+
+  if (!localizatoin_scores_msg.values.empty()) {
+    pub_localization_scores_->publish(localizatoin_scores_msg);
+  }
+}
+
+}  // namespace default_ad_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::LocalizationScoreNode)

system/default_ad_api/src/localization_score.hpp

@@ -0,0 +1,59 @@
+// Copyright 2022 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 LOCALIZATION_SCORE_HPP_
+#define LOCALIZATION_SCORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_ad_api_msgs/msg/localization_score_array.hpp"
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <tier4_debug_msgs/msg/float32_stamped.hpp>
+
+#include <memory>
+#include <utility>
+
+namespace default_ad_api
+{
+using autoware_ad_api_msgs::msg::LocalizationScoreArray;
+using geometry_msgs::msg::PoseWithCovarianceStamped;
+using tier4_debug_msgs::msg::Float32Stamped;
+
+class LocalizationScoreNode : public rclcpp::Node
+{
+public:
+  explicit LocalizationScoreNode(const rclcpp::NodeOptions & options);
+
+private:
+  void callbackTpScore(const Float32Stamped::ConstSharedPtr msg_ptr);
+  void callbackNvtlScore(const Float32Stamped::ConstSharedPtr msg_ptr);
+  void callbackTimer();
+
+  rclcpp::Publisher<LocalizationScoreArray>::SharedPtr pub_localization_scores_;
+  rclcpp::Subscription<Float32Stamped>::SharedPtr sub_transform_probability_;
+  rclcpp::Subscription<Float32Stamped>::SharedPtr sub_nearest_voxel_transformation_likelihood_;
+
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  double status_pub_hz_;
+
+  autoware_ad_api_msgs::msg::LocalizationScore score_tp_;
+  autoware_ad_api_msgs::msg::LocalizationScore score_nvtl_;
+  bool is_tp_received_;
+  bool is_nvtl_received_;
+};
+
+}  // namespace default_ad_api
+
+#endif  // LOCALIZATION_SCORE_HPP_