carma_cooperative_perception::HostVehicleFilterNode Class Reference

#include <host_vehicle_filter_component.hpp>

Inheritance diagram for carma_cooperative_perception::HostVehicleFilterNode:

Collaboration diagram for carma_cooperative_perception::HostVehicleFilterNode:

Public Member Functions
auto	handle_on_configure (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
auto	handle_on_activate (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
auto	handle_on_deactivate (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
auto	handle_on_cleanup (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
auto	handle_on_shutdown (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
auto	update_host_vehicle_pose (const geometry_msgs::msg::PoseStamped &msg) -> void
auto	attempt_filter_and_republish (carma_cooperative_perception_interfaces::msg::DetectionList msg) -> void

Private Attributes
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_
std::optional< geometry_msgs::msg::PoseStamped >	host_vehicle_pose_ {std::nullopt}
OnSetParametersCallbackHandle::SharedPtr	on_set_parameters_callback_ {nullptr}
double	squared_distance_threshold_meters_ {0.0}

Detailed Description

Definition at line 26 of file host_vehicle_filter_component.hpp.

Member Function Documentation

attempt_filter_and_republish()

auto carma_cooperative_perception::HostVehicleFilterNode::attempt_filter_and_republish

(

carma_cooperative_perception_interfaces::msg::DetectionList

msg

)

-> void

Definition at line 162 of file host_vehicle_filter_component.cpp.

{
  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) {
    if (
      detection.semantic_class != detection.SEMANTIC_CLASS_UNKNOWN &&
      detection.semantic_class != detection.SEMANTIC_CLASS_SMALL_VEHICLE) {
      return false;
    }
 
    return euclidean_distance_squared(host_vehicle_pose_.value().pose, detection.pose.pose) <=
           this->squared_distance_threshold_meters_;
  };
 
  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);
}

References carma_cooperative_perception::euclidean_distance_squared().

Here is the call graph for this function:

handle_on_activate()

auto carma_cooperative_perception::HostVehicleFilterNode::handle_on_activate ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn

override

Definition at line 110 of file host_vehicle_filter_component.cpp.

{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: activating");
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully activated");
 
  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

handle_on_cleanup()

auto carma_cooperative_perception::HostVehicleFilterNode::handle_on_cleanup ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn

override

Definition at line 128 of file host_vehicle_filter_component.cpp.

{
  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;
}

handle_on_configure()

auto carma_cooperative_perception::HostVehicleFilterNode::handle_on_configure ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn

override

Definition at line 25 of file host_vehicle_filter_component.cpp.

{
  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");
 
  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_meters") {
          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_meters': " + 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_meters': " + result.reason);
 
            break;
          } else {
            this->squared_distance_threshold_meters_ = std::pow(value, 2);
          }
        } else {
          result.successful = false;
          result.reason = "Unexpected parameter name '" + parameter.get_name() + '\'';
          break;
        }
      }
 
      return result;
    });
 
  declare_parameter("distance_threshold_meters", 0.0);
 
  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

handle_on_deactivate()

auto carma_cooperative_perception::HostVehicleFilterNode::handle_on_deactivate ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn

override

Definition at line 119 of file host_vehicle_filter_component.cpp.

{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: deactivating");
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated");
 
  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

handle_on_shutdown()

auto carma_cooperative_perception::HostVehicleFilterNode::handle_on_shutdown ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn

override

Definition at line 142 of file host_vehicle_filter_component.cpp.

{
  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;
}

update_host_vehicle_pose()

auto carma_cooperative_perception::HostVehicleFilterNode::update_host_vehicle_pose

(

const geometry_msgs::msg::PoseStamped &

msg

)

-> void

Definition at line 156 of file host_vehicle_filter_component.cpp.

{
  host_vehicle_pose_ = msg;
}

Member Data Documentation

detection_list_pub_

rclcpp_lifecycle::LifecyclePublisher<carma_cooperative_perception_interfaces::msg::DetectionList>::SharedPtr carma_cooperative_perception::HostVehicleFilterNode::detection_list_pub_

private

Initial value:

{
    nullptr}

Definition at line 58 of file host_vehicle_filter_component.hpp.

detection_list_sub_

rclcpp::Subscription<carma_cooperative_perception_interfaces::msg::DetectionList>::SharedPtr carma_cooperative_perception::HostVehicleFilterNode::detection_list_sub_ {nullptr}

private

Definition at line 53 of file host_vehicle_filter_component.hpp.

host_vehicle_pose_

std::optional<geometry_msgs::msg::PoseStamped> carma_cooperative_perception::HostVehicleFilterNode::host_vehicle_pose_ {std::nullopt}

private

Definition at line 61 of file host_vehicle_filter_component.hpp.

host_vehicle_pose_sub_

rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr carma_cooperative_perception::HostVehicleFilterNode::host_vehicle_pose_sub_ {nullptr}

private

Definition at line 55 of file host_vehicle_filter_component.hpp.

on_set_parameters_callback_

OnSetParametersCallbackHandle::SharedPtr carma_cooperative_perception::HostVehicleFilterNode::on_set_parameters_callback_ {nullptr}

private

Definition at line 63 of file host_vehicle_filter_component.hpp.

squared_distance_threshold_meters_

double carma_cooperative_perception::HostVehicleFilterNode::squared_distance_threshold_meters_ {0.0}

private

Definition at line 65 of file host_vehicle_filter_component.hpp.

---

The documentation for this class was generated from the following files:

• carma_cooperative_perception/include/carma_cooperative_perception/host_vehicle_filter_component.hpp
• carma_cooperative_perception/src/host_vehicle_filter_component.cpp