[cooperative perception] Add host vehicle filter node (#2183)

* #2182 Add initial host vehicle filter node

This provides the skeleton code for the host vehicle filter node along
with its documentation.

* #2182 Implement main logic in host vehicle filter

Add launch testing for basic component test.

* #2182 Add launch test to CMake

Also fix linting errors.

* #2182 Add host vehicle filter node documentation

* #2182 Address reviewer comments

Add node parameters for host_vehicle_filter_component. Reduce logging
level from error to warning for not having the host vehicle pose yet.

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()
 

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

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

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 <carma_cooperative_perception_interfaces/msg/detection_list.hpp>
+#include <carma_ros2_utils/carma_lifecycle_node.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+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<carma_cooperative_perception_interfaces::msg::DetectionList>::SharedPtr
+    detection_list_sub_{nullptr};
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr host_vehicle_pose_sub_{nullptr};
+
+  rclcpp_lifecycle::LifecyclePublisher<
+    carma_cooperative_perception_interfaces::msg::DetectionList>::SharedPtr detection_list_pub_{
+    nullptr};
+
+  std::optional<geometry_msgs::msg::PoseStamped> 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_

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])

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 <carma_cooperative_perception_interfaces/msg/detection.hpp>
+#include <carma_ros2_utils/carma_lifecycle_node.hpp>
+
+#include <vector>
+
+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<geometry_msgs::msg::PoseStamped>(
+    "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<carma_cooperative_perception_interfaces::msg::DetectionList>(
+      "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<rclcpp::Parameter> & 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

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 <rclcpp/rclcpp.hpp>
+
+#include <carma_cooperative_perception/host_vehicle_filter_component.hpp>
+
+#include <memory>
+
+auto main(int argc, char * argv[]) noexcept -> int
+{
+  rclcpp::init(argc, argv);
+
+  auto node{
+    std::make_shared<carma_cooperative_perception::HostVehicleFilterNode>(rclcpp::NodeOptions{})};
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node->get_node_base_interface());
+  executor.spin();
+
+  rclcpp::shutdown();
+
+  return 0;
+}