Custom Module Development

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>

namespace autoware::my_localizer
{

class MyLocalizerNode : public rclcpp::Node
{
public:
  explicit MyLocalizerNode(const rclcpp::NodeOptions & options)
  : Node("my_localizer", options),
    diagnostic_updater_(this)
  {
    declare_parameter<double>("score_threshold", 2.0);

    sub_points_ = create_subscription<sensor_msgs::msg::PointCloud2>(
      "~/input/points", rclcpp::SensorDataQoS(),
      std::bind(&MyLocalizerNode::on_points, this,
        std::placeholders::_1));

    pub_pose_ = create_publisher<
      geometry_msgs::msg::PoseWithCovarianceStamped>(
      "~/output/pose_with_covariance", 10);

    diagnostic_updater_.setHardwareID("my_localizer");
    diagnostic_updater_.add("localization_status", this,
      &MyLocalizerNode::check_diagnostics);
  }

private:
  void on_points(
    const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
  {
    if (msg->data.empty()) {
      RCLCPP_WARN(get_logger(), "Empty point cloud");
      return;
    }

    // Execute localization algorithm ...

    geometry_msgs::msg::PoseWithCovarianceStamped out;
    out.header.stamp = msg->header.stamp;
    out.header.frame_id = "map";
    out.pose.covariance[0] = 0.04;   // x variance
    out.pose.covariance[7] = 0.04;   // y variance
    out.pose.covariance[35] = 0.001; // yaw variance

    pub_pose_->publish(out);
  }

  void check_diagnostics(
    diagnostic_updater::DiagnosticStatusWrapper & stat)
  {
    if (is_healthy_) {
      stat.summary(
        diagnostic_msgs::msg::DiagnosticStatus::OK,
        "Localization OK");
    } else {
      stat.summary(
        diagnostic_msgs::msg::DiagnosticStatus::ERROR,
        "Localization failed");
    }
  }

  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
    sub_points_;
  rclcpp::Publisher<
    geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
    pub_pose_;
  diagnostic_updater::Updater diagnostic_updater_;
  bool is_healthy_{false};
};

}  // namespace autoware::my_localizer

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(
  autoware::my_localizer::MyLocalizerNode)