feat(slam): add lidar SLAM and pointcloud processing packages
This commit is contained in:
X-lanni
+98
@@ -0,0 +1,98 @@
|
||||
/*
|
||||
* 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
|
||||
*/
|
||||
|
||||
#ifndef POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
|
||||
#define POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "message_filters/subscriber.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/message_filter.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
#include "laser_geometry/laser_geometry.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
|
||||
#include "pointcloud_to_laserscan/visibility_control.h"
|
||||
|
||||
namespace pointcloud_to_laserscan
|
||||
{
|
||||
typedef tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan> MessageFilter;
|
||||
|
||||
//! \brief The PointCloudToLaserScanNodelet class to process incoming laserscans into pointclouds.
|
||||
//!
|
||||
class LaserScanToPointCloudNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
POINTCLOUD_TO_LASERSCAN_PUBLIC
|
||||
explicit LaserScanToPointCloudNode(const rclcpp::NodeOptions & options);
|
||||
|
||||
~LaserScanToPointCloudNode() override;
|
||||
|
||||
private:
|
||||
void scanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg);
|
||||
|
||||
void subscriptionListenerThreadLoop();
|
||||
|
||||
std::unique_ptr<tf2_ros::Buffer> tf2_;
|
||||
std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::LaserScan> sub_;
|
||||
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_;
|
||||
std::unique_ptr<MessageFilter> message_filter_;
|
||||
std::thread subscription_listener_thread_;
|
||||
std::atomic_bool alive_{true};
|
||||
|
||||
laser_geometry::LaserProjection projector_;
|
||||
|
||||
// ROS Parameters
|
||||
int input_queue_size_;
|
||||
std::string target_frame_;
|
||||
double tolerance_;
|
||||
};
|
||||
|
||||
} // namespace pointcloud_to_laserscan
|
||||
|
||||
#endif // POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
|
||||
+102
@@ -0,0 +1,102 @@
|
||||
/*
|
||||
* 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
|
||||
*/
|
||||
|
||||
#ifndef POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
|
||||
#define POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "message_filters/subscriber.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/message_filter.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
|
||||
#include "pointcloud_to_laserscan/visibility_control.h"
|
||||
|
||||
namespace pointcloud_to_laserscan
|
||||
{
|
||||
typedef tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2> MessageFilter;
|
||||
|
||||
/**
|
||||
* Class to process incoming pointclouds into laserscans.
|
||||
* Some initial code was pulled from the defunct turtlebot pointcloud_to_laserscan implementation.
|
||||
*/
|
||||
class PointCloudToLaserScanNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
POINTCLOUD_TO_LASERSCAN_PUBLIC
|
||||
explicit PointCloudToLaserScanNode(const rclcpp::NodeOptions & options);
|
||||
|
||||
~PointCloudToLaserScanNode() override;
|
||||
|
||||
private:
|
||||
void cloudCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg);
|
||||
|
||||
void subscriptionListenerThreadLoop();
|
||||
|
||||
std::unique_ptr<tf2_ros::Buffer> tf2_;
|
||||
std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> sub_;
|
||||
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::LaserScan>> pub_;
|
||||
std::unique_ptr<MessageFilter> message_filter_;
|
||||
|
||||
std::thread subscription_listener_thread_;
|
||||
std::atomic_bool alive_{true};
|
||||
|
||||
// ROS Parameters
|
||||
int input_queue_size_;
|
||||
std::string target_frame_;
|
||||
double tolerance_;
|
||||
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_,
|
||||
range_max_;
|
||||
bool use_inf_;
|
||||
double inf_epsilon_;
|
||||
};
|
||||
|
||||
} // namespace pointcloud_to_laserscan
|
||||
|
||||
#endif // POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
|
||||
#define POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define POINTCLOUD_TO_LASERSCAN_EXPORT __attribute__ ((dllexport))
|
||||
#define POINTCLOUD_TO_LASERSCAN_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define POINTCLOUD_TO_LASERSCAN_EXPORT __declspec(dllexport)
|
||||
#define POINTCLOUD_TO_LASERSCAN_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef POINTCLOUD_TO_LASERSCAN_BUILDING_DLL
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC POINTCLOUD_TO_LASERSCAN_EXPORT
|
||||
#else
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC POINTCLOUD_TO_LASERSCAN_IMPORT
|
||||
#endif
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC_TYPE POINTCLOUD_TO_LASERSCAN_PUBLIC
|
||||
#define POINTCLOUD_TO_LASERSCAN_LOCAL
|
||||
#else
|
||||
#define POINTCLOUD_TO_LASERSCAN_EXPORT __attribute__ ((visibility("default")))
|
||||
#define POINTCLOUD_TO_LASERSCAN_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define POINTCLOUD_TO_LASERSCAN_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC
|
||||
#define POINTCLOUD_TO_LASERSCAN_LOCAL
|
||||
#endif
|
||||
#define POINTCLOUD_TO_LASERSCAN_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
|
||||
Reference in New Issue
Block a user