From bc6bbcb6375d8493c44a60f44dc22d3468861937 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Mon, 20 Mar 2023 13:44:40 +0900 Subject: [PATCH] Add a map reciever --- .../lidar_feature_localization/CMakeLists.txt | 26 ++++++++- .../include/ros_dependent/map_receiver.hpp | 43 ++++++++++++++ .../src/ros_dependent/map_receiver.cpp | 44 ++++++++++++++ .../test/ros_dependent/test_map_reciever.cpp | 57 +++++++++++++++++++ 4 files changed, 169 insertions(+), 1 deletion(-) create mode 100644 localization/lidar_feature_localization/include/ros_dependent/map_receiver.hpp create mode 100644 localization/lidar_feature_localization/src/ros_dependent/map_receiver.cpp create mode 100644 localization/lidar_feature_localization/test/ros_dependent/test_map_reciever.cpp diff --git a/localization/lidar_feature_localization/CMakeLists.txt b/localization/lidar_feature_localization/CMakeLists.txt index 2fab2c68717e2..892fc5ee6d7cd 100644 --- a/localization/lidar_feature_localization/CMakeLists.txt +++ b/localization/lidar_feature_localization/CMakeLists.txt @@ -53,20 +53,26 @@ set(EXTRACTION_SOURCE_FILES src/extraction/math.cpp src/extraction/ring.cpp) +set(ROS_DEPENDENT_SOURCE_FILES + src/ros_dependent/map_receiver.cpp) + ament_auto_add_library(library SHARED ${LIBRARY_SOURCE_FILES}) ament_auto_add_library(localization SHARED ${LOCALIZATION_SOURCE_FILES}) ament_auto_add_library(extraction SHARED ${EXTRACTION_SOURCE_FILES}) +ament_auto_add_library(ros_dependent SHARED ${ROS_DEPENDENT_SOURCE_FILES}) target_include_directories(library PUBLIC ${INCLUDE_DIRS}) target_include_directories(localization PUBLIC ${INCLUDE_DIRS}) target_include_directories(extraction PUBLIC ${INCLUDE_DIRS}) +target_include_directories(ros_dependent PUBLIC ${INCLUDE_DIRS}) target_link_libraries(library ${PCL_LIBRARIES} fmt) target_link_libraries(localization fmt) target_link_libraries(extraction fmt) +target_link_libraries(ros_dependent ${PCL_LIBRARIES}) ament_auto_add_executable("lidar_feature_localization" app/localization.cpp) -target_link_libraries("lidar_feature_localization" library localization extraction) +target_link_libraries("lidar_feature_localization" library localization extraction ros_dependent) function(add_localization_testcase filepath) get_filename_component(filename ${filepath} NAME) @@ -98,6 +104,17 @@ function(add_lib_testcase filepath) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() +function(add_ros_dependent_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name_suffix ${filename}) + set(test_name "ros_dependent_${test_name_suffix}") + + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" ros_dependent) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + + if(BUILD_TESTING) file(GLOB FILES_TO_CHECK @@ -174,6 +191,13 @@ if(BUILD_TESTING) foreach(filepath ${LIB_TEST_FILES}) add_lib_testcase(${filepath}) endforeach() + + set(ROS_DEPENDENT_TEST_FILES + test/ros_dependent/test_map_reciever.cpp) + + foreach(filepath ${ROS_DEPENDENT_TEST_FILES}) + add_ros_dependent_testcase(${filepath}) + endforeach() endif() ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/localization/lidar_feature_localization/include/ros_dependent/map_receiver.hpp b/localization/lidar_feature_localization/include/ros_dependent/map_receiver.hpp new file mode 100644 index 0000000000000..2785ad10653f4 --- /dev/null +++ b/localization/lidar_feature_localization/include/ros_dependent/map_receiver.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 ROS_DEPENDENT__MAP_RECEIVER_HPP_ +#define ROS_DEPENDENT__MAP_RECEIVER_HPP_ + +#include +#include + +#include +#include + +#include +#include + +class MapReceiver +{ +public: + MapReceiver(std::shared_ptr node, const std::string & topic_name); + + void Callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); + + bool IsAvailable() const; + + pcl::PointCloud::Ptr Get() const; + +private: + rclcpp::Subscription::SharedPtr map_points_sub_; + pcl::PointCloud::Ptr map_points_ptr_; +}; + +#endif // ROS_DEPENDENT__MAP_RECEIVER_HPP_ diff --git a/localization/lidar_feature_localization/src/ros_dependent/map_receiver.cpp b/localization/lidar_feature_localization/src/ros_dependent/map_receiver.cpp new file mode 100644 index 0000000000000..83611abab4393 --- /dev/null +++ b/localization/lidar_feature_localization/src/ros_dependent/map_receiver.cpp @@ -0,0 +1,44 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "ros_dependent/map_receiver.hpp" + +#include + +#include + + +MapReceiver::MapReceiver(std::shared_ptr node, const std::string & topic_name) +: map_points_sub_( + node->create_subscription( + topic_name, rclcpp::QoS{1}.transient_local(), + std::bind(&MapReceiver::Callback, this, std::placeholders::_1))), + map_points_ptr_(new pcl::PointCloud()) +{ +} + +void MapReceiver::Callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) +{ + pcl::fromROSMsg(*map_points_msg_ptr, *(this->map_points_ptr_)); +} + +bool MapReceiver::IsAvailable() const +{ + return this->map_points_ptr_->size() > 0; +} + +pcl::PointCloud::Ptr MapReceiver::Get() const +{ + return this->map_points_ptr_; +} diff --git a/localization/lidar_feature_localization/test/ros_dependent/test_map_reciever.cpp b/localization/lidar_feature_localization/test/ros_dependent/test_map_reciever.cpp new file mode 100644 index 0000000000000..ede39395f41a4 --- /dev/null +++ b/localization/lidar_feature_localization/test/ros_dependent/test_map_reciever.cpp @@ -0,0 +1,57 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 +#include + +#include + +#include "ros_dependent/map_receiver.hpp" + + +class MapTestSuite : public ::testing::Test +{ +protected: + void SetUp() {rclcpp::init(0, nullptr);} + void TearDown() {(void)rclcpp::shutdown();} +}; + +TEST_F(MapTestSuite, MapReciever) +{ + pcl::PointCloud pcl_pointcloud; + pcl_pointcloud.push_back(pcl::PointXYZ(1., 1., 1.)); + + sensor_msgs::msg::PointCloud2 ros_pointcloud; + pcl::toROSMsg(pcl_pointcloud, ros_pointcloud); + + rclcpp::NodeOptions node_options; + const std::string topic_name = "pointcloud_map"; + auto node = std::make_shared("test_map_node", node_options); + auto pub_pose = node->create_publisher( + topic_name, rclcpp::QoS{1}.transient_local()); + + MapReceiver receiver(node, topic_name); + EXPECT_FALSE(receiver.IsAvailable()); + + for (int i = 0; i < 20; ++i) { + pub_pose->publish(ros_pointcloud); + rclcpp::spin_some(node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_TRUE(receiver.IsAvailable()); + const auto received_cloud = receiver.Get(); + EXPECT_GT(received_cloud->size(), 0U); + EXPECT_EQ(received_cloud->size(), pcl_pointcloud.size()); +}