diff --git a/carma_cooperative_perception/CMakeLists.txt b/carma_cooperative_perception/CMakeLists.txt index bc79a62ee0..229e2cb603 100644 --- a/carma_cooperative_perception/CMakeLists.txt +++ b/carma_cooperative_perception/CMakeLists.txt @@ -64,6 +64,7 @@ ament_auto_add_library(carma_cooperative_perception SHARED src/track_list_to_external_object_list_component.cpp src/utm_zone.cpp src/multiple_object_tracker_component.cpp + src/host_vehicle_filter_component.cpp ) target_link_libraries(carma_cooperative_perception @@ -97,6 +98,10 @@ ament_auto_add_executable(external_object_list_to_sdsm_node src/external_object_list_to_sdsm_node.cpp ) +ament_auto_add_executable(host_vehicle_filter_node + src/host_vehicle_filter_node.cpp +) + # boost::posix_time definition for using nanoseconds add_definitions(-DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG) @@ -123,6 +128,7 @@ if(carma_cooperative_perception_BUILD_TESTS) add_launch_test(test/track_list_to_external_object_list_launch_test.py) add_launch_test(test/multiple_object_tracker_duplicates_launch_test.py) add_launch_test(test/multiple_object_tracker_static_obstacle_launch_test.py) + add_launch_test(test/host_vehicle_filter_launch_test.py) endif() diff --git a/carma_cooperative_perception/config/params.yaml b/carma_cooperative_perception/config/params.yaml index b0b039ce10..973075aafd 100644 --- a/carma_cooperative_perception/config/params.yaml +++ b/carma_cooperative_perception/config/params.yaml @@ -3,3 +3,7 @@ multiple_object_tracker: execution_frequency_hz: 5.0 track_promotion_threshold: 3 track_removal_threshold: 0 + +host_vehicle_filter: + ros__parameters: + distance_threshold: 2.0 diff --git a/carma_cooperative_perception/docs/host_vehicle_filter_node.md b/carma_cooperative_perception/docs/host_vehicle_filter_node.md new file mode 100644 index 0000000000..eaf73752cc --- /dev/null +++ b/carma_cooperative_perception/docs/host_vehicle_filter_node.md @@ -0,0 +1,37 @@ +# Host vehicle filter Node + +This Node removes detections that are likely to associate with the host vehicle. For each detection in the received +detection list, the Node will check the distance between it and the host vehicle's current pose. Any detections that +fall below the specified distance threshold will be pruned from the incoming detection list. The Node will then +republish the (possibly pruned) list. + +## Subscriptions + +| Topic | Message Type | Description | +| --------------------------- | --------------------------------------------------------------------------------- | ------------------------------- | +| `~/input/detection_list` | [`carma_cooperative_perception_interfaces/DetectionList.msg`][detection_list_msg] | Incoming detections | +| `~/input/host_vehicle_pose` | [`geometry_msgs/PoseStamped.msg`][pose_stamped_msg] | The host vehicle's current pose | + +[pose_stamped_msg]: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html + +## Publishers + +| Topic | Message Type | Frequency | Description | +| ------------------------- | --------------------------------------------------------------------------------- | ------------------- | -------------------------------------------------------------------------------------- | +| `~/output/detection_list` | [`carma_cooperative_perception_interfaces/DetectionList.msg`][detection_list_msg] | Subscription-driven | Incoming detections excluding any detections likely associating with the host vehicle. | + +## Parameters + +| Topic | Data Type | Default Value | Required | Read Only | Description | +| ---------------------- | --------- | ------------- | -------- | --------- | --------------------------------------------------------------------------------- | +| `~/distance_threshold` | `float` | `0.0` | No | No | Distance below which a detection will be considered to represent the host vehicle | + +## Services + +This Node does not provide services. + +## Actions + +This Node does not provide actions. + +[detection_list_msg]: https://github.com/usdot-fhwa-stol/carma-msgs/blob/develop/carma_cooperative_perception_interfaces/msg/DetectionList.msg diff --git a/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp b/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp new file mode 100644 index 0000000000..6f63ef46fb --- /dev/null +++ b/carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp @@ -0,0 +1,73 @@ +// Copyright 2023 Leidos +// +// 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 CARMA_COOPERATIVE_PERCEPTION__HOST_VEHICLE_FILTER_COMPONENT_HPP_ +#define CARMA_COOPERATIVE_PERCEPTION__HOST_VEHICLE_FILTER_COMPONENT_HPP_ + +#include +#include +#include +#include + +namespace carma_cooperative_perception +{ + +class HostVehicleFilterNode : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + using carma_ros2_utils::CarmaLifecycleNode::CarmaLifecycleNode; + + auto handle_on_configure(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_activate(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_deactivate(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn override; + + auto update_host_vehicle_pose(const geometry_msgs::msg::PoseStamped & msg) noexcept -> void; + + auto attempt_filter_and_republish( + carma_cooperative_perception_interfaces::msg::DetectionList msg) noexcept -> void; + +private: + rclcpp::Subscription::SharedPtr + detection_list_sub_{nullptr}; + + rclcpp::Subscription::SharedPtr host_vehicle_pose_sub_{nullptr}; + + rclcpp_lifecycle::LifecyclePublisher< + carma_cooperative_perception_interfaces::msg::DetectionList>::SharedPtr detection_list_pub_{ + nullptr}; + + std::optional host_vehicle_pose_{std::nullopt}; + + OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; + + double squared_distance_threshold_{0.0}; +}; + +auto euclidean_distance_squared( + const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b) noexcept -> double; + +} // namespace carma_cooperative_perception + +#endif // CARMA_COOPERATIVE_PERCEPTION__HOST_VEHICLE_FILTER_COMPONENT_HPP_ diff --git a/carma_cooperative_perception/launch/cooperative_perception_launch.py b/carma_cooperative_perception/launch/cooperative_perception_launch.py index 14b12eb55f..c14c554f7c 100644 --- a/carma_cooperative_perception/launch/cooperative_perception_launch.py +++ b/carma_cooperative_perception/launch/cooperative_perception_launch.py @@ -27,4 +27,11 @@ def generate_launch_description(): parameters=[package_share_path / "config/params.yaml"], ) - return LaunchDescription([multiple_object_tracker_node]) + host_vehicle_filter_node = Node( + package="carma_cooperative_perception", + executable="host_vehicle_filter_node", + name="host_vehicle_filter", + parameters=[package_share_path / "config/params.yaml"], + ) + + return LaunchDescription([multiple_object_tracker_node, host_vehicle_filter_node]) diff --git a/carma_cooperative_perception/src/host_vehicle_filter_component.cpp b/carma_cooperative_perception/src/host_vehicle_filter_component.cpp new file mode 100644 index 0000000000..c8b8c583d4 --- /dev/null +++ b/carma_cooperative_perception/src/host_vehicle_filter_component.cpp @@ -0,0 +1,190 @@ +// Copyright 2023 Leidos +// +// 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 "carma_cooperative_perception/host_vehicle_filter_component.hpp" + +#include +#include + +#include + +namespace carma_cooperative_perception +{ +auto HostVehicleFilterNode::handle_on_configure( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: configuring"); + + detection_list_sub_ = create_subscription< + carma_cooperative_perception_interfaces::msg::DetectionList>( + "input/detection_list", 1, + [this](const carma_cooperative_perception_interfaces::msg::DetectionList::SharedPtr msg_ptr) { + if (const auto current_state{this->get_current_state().label()}; current_state == "active") { + attempt_filter_and_republish(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->detection_list_sub_->get_topic_name(), current_state.c_str()); + } + }); + + host_vehicle_pose_sub_ = create_subscription( + "input/host_vehicle_pose", 1, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg_ptr) { + if (const auto current_state{this->get_current_state().label()}; current_state == "active") { + update_host_vehicle_pose(*msg_ptr); + } else { + RCLCPP_WARN( + this->get_logger(), + "Trying to receive message on the topic '%s', but the containing node is not activated. " + "Current node state: '%s'", + this->detection_list_sub_->get_topic_name(), current_state.c_str()); + } + }); + + detection_list_pub_ = + create_publisher( + "output/detection_list", 1); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured"); + + declare_parameter("distance_threshold", 0.0); + + on_set_parameters_callback_ = + add_on_set_parameters_callback([this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + for (const auto & parameter : parameters) { + if (parameter.get_name() == "distance_threshold") { + if (this->get_current_state().label() == "active") { + result.successful = false; + result.reason = "parameter is read-only while node is in 'Active' state"; + + RCLCPP_ERROR( + get_logger(), "Cannot change parameter 'distance_threshold': " + result.reason); + + break; + } + + if (const auto value{parameter.as_double()}; value < 0) { + result.successful = false; + result.reason = "parameter must be nonnegative"; + + RCLCPP_ERROR( + get_logger(), "Cannot change parameter 'distance_threshold': " + result.reason); + + break; + } else { + this->squared_distance_threshold_ = std::pow(value, 2); + } + } else { + result.successful = false; + result.reason = "Unexpected parameter name '" + parameter.get_name() + '\''; + break; + } + } + + return result; + }); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::handle_on_activate(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: activating"); + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully activated"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::handle_on_deactivate( + const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: deactivating"); + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::handle_on_cleanup(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: cleaning up"); + + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + detection_list_sub_.reset(); + host_vehicle_pose_sub_.reset(); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully cleaned up"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::handle_on_shutdown(const rclcpp_lifecycle::State & /* previous_state */) + -> carma_ros2_utils::CallbackReturn +{ + RCLCPP_INFO(get_logger(), "Lifecycle transition: shutting down"); + + // CarmaLifecycleNode does not handle subscriber pointer reseting for us + detection_list_sub_.reset(); + host_vehicle_pose_sub_.reset(); + + RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully shut down"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; +} + +auto HostVehicleFilterNode::update_host_vehicle_pose( + const geometry_msgs::msg::PoseStamped & msg) noexcept -> void +{ + host_vehicle_pose_ = msg; +} + +auto HostVehicleFilterNode::attempt_filter_and_republish( + carma_cooperative_perception_interfaces::msg::DetectionList msg) noexcept -> void +{ + if (!host_vehicle_pose_.has_value()) { + RCLCPP_WARN(get_logger(), "Could not filter detection list: host vehicle pose unknown"); + return; + } + + const auto is_within_distance = [this](const auto & detection) { + return euclidean_distance_squared(host_vehicle_pose_.value().pose, detection.pose.pose) <= + this->squared_distance_threshold_; + }; + + const auto new_end{ + std::remove_if(std::begin(msg.detections), std::end(msg.detections), is_within_distance)}; + + msg.detections.erase(new_end, std::end(msg.detections)); + + this->detection_list_pub_->publish(msg); +} + +auto euclidean_distance_squared( + const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b) noexcept -> double +{ + return std::pow(a.position.x - b.position.x, 2) + std::pow(a.position.y - b.position.y, 2) + + std::pow(a.position.z - b.position.z, 2) + std::pow(a.orientation.x - b.orientation.x, 2) + + std::pow(a.orientation.y - b.orientation.y, 2) + + std::pow(a.orientation.z - b.orientation.z, 2) + + std::pow(a.orientation.w - b.orientation.w, 2); +} + +} // namespace carma_cooperative_perception diff --git a/carma_cooperative_perception/src/host_vehicle_filter_node.cpp b/carma_cooperative_perception/src/host_vehicle_filter_node.cpp new file mode 100644 index 0000000000..529c485f01 --- /dev/null +++ b/carma_cooperative_perception/src/host_vehicle_filter_node.cpp @@ -0,0 +1,35 @@ +// Copyright 2023 Leidos +// +// 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 + +auto main(int argc, char * argv[]) noexcept -> int +{ + rclcpp::init(argc, argv); + + auto node{ + std::make_shared(rclcpp::NodeOptions{})}; + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/carma_cooperative_perception/test/data/detection_list.yaml b/carma_cooperative_perception/test/data/detection_list.yaml new file mode 100644 index 0000000000..ccdaa06020 --- /dev/null +++ b/carma_cooperative_perception/test/data/detection_list.yaml @@ -0,0 +1,19 @@ +--- +message_type: carma_cooperative_perception_interfaces/msg/DetectionList +message_fields: + detections: + - id: "1" + pose: + pose: + position: + x: 1.0 + - id: "2" + pose: + pose: + position: + x: 10.0 + - id: "3" + pose: + pose: + position: + x: 20.0 diff --git a/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py b/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py new file mode 100644 index 0000000000..b8e306d434 --- /dev/null +++ b/carma_cooperative_perception/test/host_vehicle_filter_launch_test.py @@ -0,0 +1,137 @@ +# Copyright 2023 Leidos +# +# 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. + +import unittest + +from ament_index_python import get_package_share_path + +from carma_launch_testing.transitions import transition_node +from carma_launch_testing.predicates import LenIncreases +from carma_launch_testing.spinning import spin_node_until + +import carma_message_utilities + +from carma_cooperative_perception_interfaces.msg import DetectionList + +from geometry_msgs.msg import PoseStamped + +import launch_ros.actions + +from launch import LaunchDescription +from launch.actions import TimerAction + +from launch_testing import post_shutdown_test +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes + +from lifecycle_msgs.msg import Transition + +import pytest + +import rclpy +from rclpy.context import Context +import rclpy.node + + +class TestHarnessNode(rclpy.node.Node): + def __init__(self, *args, **kwargs) -> None: + super().__init__("test_harness", *args, **kwargs) + + self.detection_list_pub = self.create_publisher( + DetectionList, "input/detection_list", 1 + ) + + self.host_vehicle_pose_pub = self.create_publisher( + PoseStamped, "input/host_vehicle_pose", 1 + ) + + self.detection_list_sub = self.create_subscription( + DetectionList, + "output/detection_list", + lambda msg: self.detection_list_msgs.append(msg), + 1, + ) + + self.detection_list_msgs = [] + + +@pytest.mark.launch_test +def generate_test_description(): + node_under_test = launch_ros.actions.Node( + package="carma_cooperative_perception", + executable="host_vehicle_filter_node", + name="node_under_test", + parameters=[{"distance_threshold": 2.0}], + ) + + launch_description = LaunchDescription( + [node_under_test, TimerAction(period=1.0, actions=[ReadyToTest()])] + ) + + return launch_description + + +class TestMotionComputation(unittest.TestCase): + @classmethod + def setUpClass(cls) -> None: + super().setUpClass() + + cls.context = Context() + + rclpy.init(context=cls.context) + cls.test_harness_node = TestHarnessNode(context=cls.context) + + @classmethod + def tearDownClass(cls) -> None: + super().tearDownClass() + + rclpy.shutdown(context=cls.context) + + def test_track_list_conversion(self): + transition_id = Transition.TRANSITION_CONFIGURE + transition_node("node_under_test", transition_id, self.context) + + transition_id = Transition.TRANSITION_ACTIVATE + transition_node("node_under_test", transition_id, self.context) + + host_vehicle_pose = PoseStamped() + host_vehicle_pose.pose.position.x = 1.0 + + self.test_harness_node.host_vehicle_pose_pub.publish(host_vehicle_pose) + + package_share_path = get_package_share_path("carma_cooperative_perception") + + msg_file = package_share_path / "test/data/detection_list.yaml" + msg = carma_message_utilities.msg_from_yaml_file(msg_file) + self.test_harness_node.detection_list_pub.publish(msg) + + spin_node_until( + self.test_harness_node, + LenIncreases(self.test_harness_node.detection_list_msgs), + self.context, + ) + + self.assertGreaterEqual(len(self.test_harness_node.detection_list_msgs), 0) + detection_list = self.test_harness_node.detection_list_msgs[-1] + + self.assertEqual(len(detection_list.detections), 2) + + self.assertEqual(detection_list.detections[0].id, '2') + self.assertEqual(detection_list.detections[1].id, '3') + + +@post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info)