TODO for USER: Add class description. More...

#include <points_map_filter_node.hpp>

Inheritance diagram for points_map_filter::Node:

Collaboration diagram for points_map_filter::Node:

Public Member Functions
	Node (const rclcpp::NodeOptions &)
	Node constructor. More...

rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Example callback for dynamic parameter updates. More...

void	points_callback (sensor_msgs::msg::PointCloud2::UniquePtr msg)
	Example subscription callback. More...

void	map_callback (autoware_lanelet2_msgs::msg::MapBin::UniquePtr msg)

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

Private Member Functions
void	recompute_lookup_grid ()

Private Attributes
carma_ros2_utils::SubPtr< sensor_msgs::msg::PointCloud2 >	points_sub_

carma_ros2_utils::SubPtr< autoware_lanelet2_msgs::msg::MapBin >	map_sub_

carma_ros2_utils::PubPtr< sensor_msgs::msg::PointCloud2 >	filtered_points_pub_

Config	config_

lanelet::LaneletMapPtr	map_

approximate_intersection::LookupGrid< PointT >	lookup_grid_

Detailed Description

TODO for USER: Add class description.

Definition at line 39 of file points_map_filter_node.hpp.

Constructor & Destructor Documentation

◆ Node()

points_map_filter::Node::Node ( const rclcpp::NodeOptions & options )

explicit

Node constructor.

Definition at line 39 of file points_map_filter_node.cpp.

      : carma_ros2_utils::CarmaLifecycleNode(options)
  {
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.cell_side_length = declare_parameter<double>("cell_side_length", config_.cell_side_length);
  }

References points_map_filter::Config::cell_side_length, and config_.

Member Function Documentation

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn points_map_filter::Node::handle_on_configure ( const rclcpp_lifecycle::State & )

Definition at line 69 of file points_map_filter_node.cpp.

  {
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<double>("cell_side_length", config_.cell_side_length);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1));
 
    RCLCPP_INFO_STREAM(get_logger(), "Loaded " << config_);
 
    // Setup subscribers
    points_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("points_raw", 10,
                                                                     std::bind(&Node::points_callback, this, std_ph::_1));
 
    // NOTE: Currently, intra-process comms must be disabled for subcribers that are transient_local: https://github.com/ros2/rclcpp/issues/1753
    rclcpp::SubscriptionOptions map_sub_options; 
    map_sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this SubscriptionOptions object
 
    auto map_sub_qos = rclcpp::QoS(rclcpp::KeepLast(1)); // Set the queue size for a subscriber with this QoS
    map_sub_qos.transient_local();  // If it is possible that this node is a late-joiner to its topic, it must be set to transient_local to receive earlier messages that were missed.
                                    // NOTE: The publisher's QoS must be set to transisent_local() as well for earlier messages to be resent to this later-joiner.
 
    map_sub_ = create_subscription<autoware_lanelet2_msgs::msg::MapBin>("lanelet2_map", map_sub_qos,
                                                                        std::bind(&Node::map_callback, this, std_ph::_1), map_sub_options);
 
    // Setup publishers
    filtered_points_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("filtered_points", 10);
 
    // Return success if everthing initialized successfully
    return CallbackReturn::SUCCESS;
  }

References points_map_filter::Config::cell_side_length, config_, filtered_points_pub_, map_callback(), map_sub_, parameter_update_callback(), points_callback(), and points_sub_.

Here is the call graph for this function:

◆ map_callback()

void points_map_filter::Node::map_callback ( autoware_lanelet2_msgs::msg::MapBin::UniquePtr msg )

Definition at line 194 of file points_map_filter_node.cpp.

  {
 
    lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap);
 
    fromBinMsg(*msg, new_map);
 
    map_ = new_map;
 
    approximate_intersection::Config intersection_config;
    intersection_config.cell_side_length = config_.cell_side_length;
 
    // TODO it would be great if lanelet2 already knew the map bounds and this iteration didn't need to happen twice
    double min_x = std::numeric_limits<double>::max();
    double max_x = std::numeric_limits<double>::lowest();
    double min_y = std::numeric_limits<double>::max();
    double max_y = std::numeric_limits<double>::lowest();
    for (const auto &point : map_->pointLayer)
    {
      if (point.x() < min_x)
        min_x = point.x();
 
      if (point.x() > max_x)
        max_x = point.x();
 
      if (point.y() < min_y)
        min_y = point.y();
 
      if (point.y() > max_y)
        max_y = point.y();
    }
 
    double three_cells = config_.cell_side_length * 3;
    intersection_config.min_x = min_x - three_cells;
    intersection_config.max_x = max_x + three_cells;
    intersection_config.min_y = min_y - three_cells;
    intersection_config.max_y = max_y + three_cells;
 
    RCLCPP_INFO_STREAM(get_logger(), "Found base map bounds of (min_x, max_x, min_y, max_y): "
                                         << "( " << min_x << ", " << max_x << ", " << min_y << ", " << max_y << ")");
    RCLCPP_INFO_STREAM(get_logger(), "Expanded map bounds to: "
                                         << "( " << intersection_config.min_x << ", " << intersection_config.max_x << ", " << intersection_config.min_y << ", " << intersection_config.max_y << ")");
 
    lookup_grid_ = approximate_intersection::LookupGrid<PointT>(intersection_config);
 
    recompute_lookup_grid();
  }

References approximate_intersection::Config::cell_side_length, points_map_filter::Config::cell_side_length, config_, points_map_filter::anonymous_namespace{points_map_filter_node.cpp}::fromBinMsg(), lookup_grid_, map_, approximate_intersection::Config::max_x, approximate_intersection::Config::max_y, approximate_intersection::Config::min_x, approximate_intersection::Config::min_y, process_traj_logs::point, and recompute_lookup_grid().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ parameter_update_callback()

rcl_interfaces::msg::SetParametersResult points_map_filter::Node::parameter_update_callback ( const std::vector< rclcpp::Parameter > & parameters )

Example callback for dynamic parameter updates.

Definition at line 49 of file points_map_filter_node.cpp.

  {
    auto error = update_params<double>({{"cell_side_length", config_.cell_side_length}}, parameters);
 
    if (!error)
    {
      auto lookup_config = lookup_grid_.get_config();
      lookup_config.cell_side_length = config_.cell_side_length;
 
      lookup_grid_ = approximate_intersection::LookupGrid<PointT>(lookup_config);
      recompute_lookup_grid();
    }
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error;
 
    return result;
  }

References points_map_filter::Config::cell_side_length, config_, lookup_grid_, and recompute_lookup_grid().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ points_callback()

void points_map_filter::Node::points_callback ( sensor_msgs::msg::PointCloud2::UniquePtr msg )

Example subscription callback.

Definition at line 104 of file points_map_filter_node.cpp.

  {
 
    const auto input_cloud = pcl::make_shared<CloudT>();
    const auto filtered_cloud = pcl::make_shared<CloudT>();
 
    pcl::moveFromROSMsg(*msg, *input_cloud);
 
    pcl::experimental::FilterFunction<PointT> filter =
        [this](const CloudT &cloud, pcl::index_t idx)
    {
      return this->lookup_grid_.intersects(cloud[idx]);
    };
 
    // build the filter
    pcl::experimental::FunctionFilter<PointT> func_filter(filter);
    func_filter.setInputCloud(input_cloud);
 
    // apply filter
    func_filter.filter(*filtered_cloud);
 
    sensor_msgs::msg::PointCloud2 out_msg;
    pcl::toROSMsg(*filtered_cloud, out_msg);
 
    filtered_points_pub_->publish(out_msg);
  }

References filtered_points_pub_, and lookup_grid_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ recompute_lookup_grid()

void points_map_filter::Node::recompute_lookup_grid ( )

private

Definition at line 175 of file points_map_filter_node.cpp.

  {
 
    if (!map_)
    {
      RCLCPP_ERROR(get_logger(), "No map received yet. Cannot recompute lookup grid");
      return;
    }
 
    for (const auto &point : map_->pointLayer)
    {
      PointT p;
      p.x = point.x();
      p.y = point.y();
      p.z = point.z();
      lookup_grid_.insert(p);
    }
  }