diff --git a/common/autoware_ad_api_msgs/CMakeLists.txt b/common/autoware_ad_api_msgs/CMakeLists.txt index 7192382a3054e..031134af5a5de 100644 --- a/common/autoware_ad_api_msgs/CMakeLists.txt +++ b/common/autoware_ad_api_msgs/CMakeLists.txt @@ -5,8 +5,6 @@ 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 diff --git a/common/autoware_ad_api_msgs/localization/msg/LocalizationScore.msg b/common/autoware_ad_api_msgs/localization/msg/LocalizationScore.msg deleted file mode 100644 index bd570b47cbf18..0000000000000 --- a/common/autoware_ad_api_msgs/localization/msg/LocalizationScore.msg +++ /dev/null @@ -1,2 +0,0 @@ -string name -float32 value diff --git a/common/autoware_ad_api_msgs/localization/msg/LocalizationScoreArray.msg b/common/autoware_ad_api_msgs/localization/msg/LocalizationScoreArray.msg deleted file mode 100644 index a280d9a71d358..0000000000000 --- a/common/autoware_ad_api_msgs/localization/msg/LocalizationScoreArray.msg +++ /dev/null @@ -1 +0,0 @@ -autoware_ad_api_msgs/LocalizationScore[] values diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 774e3de9e4d36..ae4125cc822ff 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -6,11 +6,9 @@ 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) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index 918c7a1c74a3f..e9542328ed986 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -31,7 +31,6 @@ 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", diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 7b9c15d52b711..0bb71303d5466 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -13,10 +13,8 @@ autoware_ad_api_msgs component_interface_utils - geometry_msgs rclcpp rclcpp_components - tier4_debug_msgs python3-flask diff --git a/system/default_ad_api/src/localization_score.cpp b/system/default_ad_api/src/localization_score.cpp deleted file mode 100644 index 655d48990b799..0000000000000 --- a/system/default_ad_api/src/localization_score.cpp +++ /dev/null @@ -1,88 +0,0 @@ -// 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("api/get/localization_scores", 1); - - // Subscriber - sub_transform_probability_ = this->create_subscription( - "/localization/pose_estimator/transform_probability", 1, - std::bind(&LocalizationScoreNode::callbackTpScore, this, _1)); - sub_nearest_voxel_transformation_likelihood_ = this->create_subscription( - "/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::duration(1.0 / status_pub_hz_)); - // timer_ = std::make_shared>( - // 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(default_ad_api::LocalizationScoreNode) diff --git a/system/default_ad_api/src/localization_score.hpp b/system/default_ad_api/src/localization_score.hpp deleted file mode 100644 index 0b839c9bf4546..0000000000000 --- a/system/default_ad_api/src/localization_score.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// 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 - -#include "autoware_ad_api_msgs/msg/localization_score_array.hpp" -#include -#include - -#include -#include - -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::SharedPtr pub_localization_scores_; - rclcpp::Subscription::SharedPtr sub_transform_probability_; - rclcpp::Subscription::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_