feat(slam): add lidar SLAM and pointcloud processing packages

pointcloud_to_laserscan/src/dummy_pointcloud_publisher.cpp

@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Open Source Robotics Foundation, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
+
+#include <memory>
+#include <random>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<rclcpp::Node>("dummy_pointcloud_publisher");
+  auto pub =
+    node->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS());
+
+  sensor_msgs::msg::PointCloud2 dummy_cloud;
+  sensor_msgs::PointCloud2Modifier modifier(dummy_cloud);
+  modifier.setPointCloud2Fields(
+    3,
+    "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32);
+  modifier.resize(node->declare_parameter("cloud_size", 100));
+  std::mt19937 gen(node->declare_parameter("cloud_seed", 0));
+  double extent = node->declare_parameter("cloud_extent", 10.0);
+  std::uniform_real_distribution<float> distribution(-extent / 2, extent / 2);
+  sensor_msgs::PointCloud2Iterator<float> it_x(dummy_cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> it_y(dummy_cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> it_z(dummy_cloud, "z");
+  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) {
+    *it_x = distribution(gen);
+    *it_y = distribution(gen);
+    *it_z = distribution(gen);
+  }
+  dummy_cloud.header.frame_id = node->declare_parameter("cloud_frame_id", "");
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  rclcpp::Rate rate(1.0);
+  while (rclcpp::ok()) {
+    dummy_cloud.header.stamp = node->get_clock()->now();
+    pub->publish(dummy_cloud);
+    executor.spin_some();
+    rate.sleep();
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

pointcloud_to_laserscan/src/laserscan_to_pointcloud_node.cpp

@@ -0,0 +1,150 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Eurotec, Netherlands
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
+
+/*
+ * Author: Rein Appeldoorn
+ */
+
+#include "pointcloud_to_laserscan/laserscan_to_pointcloud_node.hpp"
+
+#include <chrono>
+#include <functional>
+#include <limits>
+#include <memory>
+#include <string>
+#include <thread>
+#include <utility>
+
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
+#include "tf2_ros/create_timer_ros.h"
+
+namespace pointcloud_to_laserscan
+{
+
+LaserScanToPointCloudNode::LaserScanToPointCloudNode(const rclcpp::NodeOptions & options)
+: rclcpp::Node("laserscan_to_pointcloud", options)
+{
+  target_frame_ = this->declare_parameter("target_frame", "");
+  tolerance_ = this->declare_parameter("transform_tolerance", 0.01);
+  // TODO(hidmic): adjust default input queue size based on actual concurrency levels
+  // achievable by the associated executor
+  input_queue_size_ = this->declare_parameter(
+    "queue_size", static_cast<int>(std::thread::hardware_concurrency()));
+
+  pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS());
+
+  using std::placeholders::_1;
+  // if pointcloud target frame specified, we need to filter by transform availability
+  if (!target_frame_.empty()) {
+    tf2_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      this->get_node_base_interface(), this->get_node_timers_interface());
+    tf2_->setCreateTimerInterface(timer_interface);
+    tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_);
+    message_filter_ = std::make_unique<MessageFilter>(
+      sub_, *tf2_, target_frame_, input_queue_size_,
+      this->get_node_logging_interface(),
+      this->get_node_clock_interface());
+    message_filter_->registerCallback(
+      std::bind(
+        &LaserScanToPointCloudNode::scanCallback, this, _1));
+  } else {  // otherwise setup direct subscription
+    sub_.registerCallback(std::bind(&LaserScanToPointCloudNode::scanCallback, this, _1));
+  }
+
+  subscription_listener_thread_ = std::thread(
+    std::bind(&LaserScanToPointCloudNode::subscriptionListenerThreadLoop, this));
+}
+
+LaserScanToPointCloudNode::~LaserScanToPointCloudNode()
+{
+  alive_.store(true);
+  subscription_listener_thread_.join();
+}
+
+void LaserScanToPointCloudNode::subscriptionListenerThreadLoop()
+{
+  rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
+
+  const std::chrono::milliseconds timeout(100);
+  while (rclcpp::ok(context) && alive_.load()) {
+    int subscription_count = pub_->get_subscription_count() +
+      pub_->get_intra_process_subscription_count();
+    if (subscription_count > 0) {
+      if (!sub_.getSubscriber()) {
+        RCLCPP_INFO(
+          this->get_logger(),
+          "Got a subscriber to pointcloud, starting laserscan subscriber");
+        rclcpp::SensorDataQoS qos;
+        qos.keep_last(input_queue_size_);
+        sub_.subscribe(this, "scan_in", qos.get_rmw_qos_profile());
+      }
+    } else if (sub_.getSubscriber()) {
+      RCLCPP_INFO(
+        this->get_logger(),
+        "No subscribers to pointcloud, shutting down laserscan subscriber");
+      sub_.unsubscribe();
+    }
+    rclcpp::Event::SharedPtr event = this->get_graph_event();
+    this->wait_for_graph_change(event, timeout);
+  }
+  sub_.unsubscribe();
+}
+
+void LaserScanToPointCloudNode::scanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg)
+{
+  auto cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  projector_.projectLaser(*scan_msg, *cloud_msg);
+
+  // Transform cloud if necessary
+  if (!target_frame_.empty() && cloud_msg->header.frame_id != target_frame_) {
+    try {
+      *cloud_msg = tf2_->transform(*cloud_msg, target_frame_, tf2::durationFromSec(tolerance_));
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR_STREAM(this->get_logger(), "Transform failure: " << ex.what());
+      return;
+    }
+  }
+  pub_->publish(std::move(cloud_msg));
+}
+
+}  // namespace pointcloud_to_laserscan
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_to_laserscan::LaserScanToPointCloudNode)

pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp

@@ -0,0 +1,242 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ */
+
+/*
+ * Author: Paul Bovbel
+ */
+
+#include "pointcloud_to_laserscan/pointcloud_to_laserscan_node.hpp"
+
+#include <chrono>
+#include <functional>
+#include <limits>
+#include <memory>
+#include <string>
+#include <thread>
+#include <utility>
+
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
+#include "tf2_ros/create_timer_ros.h"
+#include "rmw/qos_profiles.h"
+
+namespace pointcloud_to_laserscan
+{
+
+PointCloudToLaserScanNode::PointCloudToLaserScanNode(const rclcpp::NodeOptions & options)
+: rclcpp::Node("pointcloud_to_laserscan", options)
+{
+  target_frame_ = this->declare_parameter("target_frame", "");
+  tolerance_ = this->declare_parameter("transform_tolerance", 0.01);
+  // TODO(hidmic): adjust default input queue size based on actual concurrency levels
+  // achievable by the associated executor
+  input_queue_size_ = this->declare_parameter(
+    "queue_size", static_cast<int>(std::thread::hardware_concurrency()));
+  min_height_ = this->declare_parameter("min_height", std::numeric_limits<double>::min());
+  max_height_ = this->declare_parameter("max_height", std::numeric_limits<double>::max());
+  angle_min_ = this->declare_parameter("angle_min", -M_PI);
+  angle_max_ = this->declare_parameter("angle_max", M_PI);
+  angle_increment_ = this->declare_parameter("angle_increment", M_PI / 180.0);
+  scan_time_ = this->declare_parameter("scan_time", 1.0 / 30.0);
+  range_min_ = this->declare_parameter("range_min", 0.0);
+  range_max_ = this->declare_parameter("range_max", std::numeric_limits<double>::max());
+  inf_epsilon_ = this->declare_parameter("inf_epsilon", 1.0);
+  use_inf_ = this->declare_parameter("use_inf", true);
+
+  // Create a laser scan publisher, using BEST_EFFORT QoS to accommodate more subscribers.
+  rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
+  qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+  pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", qos_profile);
+
+  using std::placeholders::_1;
+  // if pointcloud target frame specified, we need to filter by transform availability
+  if (!target_frame_.empty()) {
+    tf2_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      this->get_node_base_interface(), this->get_node_timers_interface());
+    tf2_->setCreateTimerInterface(timer_interface);
+    tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_);
+    message_filter_ = std::make_unique<MessageFilter>(
+      sub_, *tf2_, target_frame_, input_queue_size_,
+      this->get_node_logging_interface(),
+      this->get_node_clock_interface());
+    message_filter_->registerCallback(
+      std::bind(&PointCloudToLaserScanNode::cloudCallback, this, _1));
+  } else {  // otherwise setup direct subscription
+    sub_.registerCallback(std::bind(&PointCloudToLaserScanNode::cloudCallback, this, _1));
+  }
+
+  subscription_listener_thread_ = std::thread(
+    std::bind(&PointCloudToLaserScanNode::subscriptionListenerThreadLoop, this));
+}
+
+PointCloudToLaserScanNode::~PointCloudToLaserScanNode()
+{
+  alive_.store(false);
+  subscription_listener_thread_.join();
+}
+
+void PointCloudToLaserScanNode::subscriptionListenerThreadLoop()
+{
+  rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
+
+  const std::chrono::milliseconds timeout(100);
+  while (rclcpp::ok(context) && alive_.load()) {
+    int subscription_count = pub_->get_subscription_count() +
+      pub_->get_intra_process_subscription_count();
+    if (subscription_count > 0) {
+      if (!sub_.getSubscriber()) {
+        RCLCPP_INFO(
+          this->get_logger(),
+          "Got a subscriber to laserscan, starting pointcloud subscriber");
+        rclcpp::SensorDataQoS qos;
+        qos.keep_last(input_queue_size_);
+        sub_.subscribe(this, "cloud_in", qos.get_rmw_qos_profile());
+      }
+    } else if (sub_.getSubscriber()) {
+      RCLCPP_INFO(
+        this->get_logger(),
+        "No subscribers to laserscan, shutting down pointcloud subscriber");
+      sub_.unsubscribe();
+    }
+    rclcpp::Event::SharedPtr event = this->get_graph_event();
+    this->wait_for_graph_change(event, timeout);
+  }
+  sub_.unsubscribe();
+}
+
+void PointCloudToLaserScanNode::cloudCallback(
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg)
+{
+  // build laserscan output
+  auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
+  scan_msg->header = cloud_msg->header;
+  if (!target_frame_.empty()) {
+    scan_msg->header.frame_id = target_frame_;
+  }
+
+  scan_msg->angle_min = angle_min_;
+  scan_msg->angle_max = angle_max_;
+  scan_msg->angle_increment = angle_increment_;
+  scan_msg->time_increment = 0.0;
+  scan_msg->scan_time = scan_time_;
+  scan_msg->range_min = range_min_;
+  scan_msg->range_max = range_max_;
+
+  // determine amount of rays to create
+  uint32_t ranges_size = std::ceil(
+    (scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+
+  // determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
+  if (use_inf_) {
+    scan_msg->ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
+  } else {
+    scan_msg->ranges.assign(ranges_size, scan_msg->range_max + inf_epsilon_);
+  }
+
+  // Transform cloud if necessary
+  if (scan_msg->header.frame_id != cloud_msg->header.frame_id) {
+    try {
+      auto cloud = std::make_shared<sensor_msgs::msg::PointCloud2>();
+      tf2_->transform(*cloud_msg, *cloud, target_frame_, tf2::durationFromSec(tolerance_));
+      cloud_msg = cloud;
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR_STREAM(this->get_logger(), "Transform failure: " << ex.what());
+      return;
+    }
+  }
+
+  // Iterate through pointcloud
+  for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_msg, "x"),
+    iter_y(*cloud_msg, "y"), iter_z(*cloud_msg, "z");
+    iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
+  {
+    if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for nan in point(%f, %f, %f)\n",
+        *iter_x, *iter_y, *iter_z);
+      continue;
+    }
+
+    if (*iter_z > max_height_ || *iter_z < min_height_) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for height %f not in range (%f, %f)\n",
+        *iter_z, min_height_, max_height_);
+      continue;
+    }
+
+    double range = hypot(*iter_x, *iter_y);
+    if (range < range_min_) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for range %f below minimum value %f. Point: (%f, %f, %f)",
+        range, range_min_, *iter_x, *iter_y, *iter_z);
+      continue;
+    }
+    if (range > range_max_) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for range %f above maximum value %f. Point: (%f, %f, %f)",
+        range, range_max_, *iter_x, *iter_y, *iter_z);
+      continue;
+    }
+
+    double angle = atan2(*iter_y, *iter_x);
+    if (angle < scan_msg->angle_min || angle > scan_msg->angle_max) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for angle %f not in range (%f, %f)\n",
+        angle, scan_msg->angle_min, scan_msg->angle_max);
+      continue;
+    }
+
+    // overwrite range at laserscan ray if new range is smaller
+    int index = (angle - scan_msg->angle_min) / scan_msg->angle_increment;
+    if (range < scan_msg->ranges[index]) {
+      scan_msg->ranges[index] = range;
+    }
+  }
+  pub_->publish(std::move(scan_msg));
+}
+
+}  // namespace pointcloud_to_laserscan
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_to_laserscan::PointCloudToLaserScanNode)
+