Add support for ROS2

laser_line_extraction/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.8)
+project(laser_line_extraction)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(laser_line_extraction_interfaces REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+set(ament_dependencies
+   rclcpp 
+   std_msgs 
+   sensor_msgs
+   visualization_msgs
+   laser_line_extraction_interfaces
+)
+
+
+
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+include_directories(include
+${EIGEN3_INCLUDE_DIRS})
+
+add_library(line src/line.cpp)
+ament_target_dependencies(line ${ament_dependencies})
+
+add_library(line_extraction src/line_extraction.cpp)
+ament_target_dependencies(line_extraction ${ament_dependencies})
+
+add_library(line_extraction_ros src/line_extraction_ros.cpp)
+ament_target_dependencies(line_extraction_ros ${ament_dependencies})
+target_link_libraries(line_extraction_ros line line_extraction)
+
+add_executable(line_extraction_node src/line_extraction_node.cpp)
+ament_target_dependencies(line_extraction_node ${ament_dependencies})
+
+target_link_libraries(line_extraction_node line line_extraction line_extraction_ros)
+
+install(TARGETS
+line_extraction_node
+DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

laser_line_extraction/include/laser_line_extraction/line.h

@@ -0,0 +1,67 @@
+#ifndef LINE_EXTRACTION_LINE_H
+#define LINE_EXTRACTION_LINE_H
+
+#include <vector>
+#include <boost/array.hpp>
+#include "laser_line_extraction/utilities.h"
+
+namespace line_extraction
+{
+
+  class Line
+  {
+
+  public:
+    // Constructor / destructor
+    Line(const CachedData &, const RangeData &, const Params &, std::vector<unsigned int>);
+    Line(double angle, double radius, const std::array<double, 4> &covariance,
+         const std::array<double, 2> &start, const std::array<double, 2> &end,
+         const std::vector<unsigned int> &indices);
+    ~Line();
+    // Get methods for the line parameters
+    double getAngle() const;
+    const std::array<double, 4> &getCovariance() const;
+    const std::array<double, 2> &getEnd() const;
+    const std::vector<unsigned int> &getIndices() const;
+    double getRadius() const;
+    const std::array<double, 2> &getStart() const;
+    // Methods for line fitting
+    double distToPoint(unsigned int);
+    void endpointFit();
+    void leastSqFit();
+    double length() const;
+    unsigned int numPoints() const;
+    void projectEndpoints();
+
+  private:
+    std::vector<unsigned int> indices_;
+    // Data structures
+    CachedData c_data_;
+    RangeData r_data_;
+    Params params_;
+    PointParams p_params_;
+    // Point variances used for least squares
+    std::vector<double> point_scalar_vars_;
+    std::vector<std::array<double, 4>> point_covs_;
+    double p_rr_;
+    // Line parameters
+    double angle_;
+    double radius_;
+    std::array<double, 2> start_;
+    std::array<double, 2> end_;
+    std::array<double, 4> covariance_;
+    // Methods
+    void angleFromEndpoints();
+    void angleFromLeastSq();
+    double angleIncrement();
+    void calcCovariance();
+    void calcPointCovariances();
+    void calcPointParameters();
+    void calcPointScalarCovariances();
+    void radiusFromEndpoints();
+    void radiusFromLeastSq();
+  }; // class Line
+
+} // namespace line_extraction
+
+#endif

laser_line_extraction/include/laser_line_extraction/line_extraction_ros.h

@@ -0,0 +1,53 @@
+#ifndef LINE_EXTRACTION_ROS_H
+#define LINE_EXTRACTION_ROS_H
+
+#include <vector>
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include "laser_line_extraction_interfaces/msg/line_segment.hpp"
+#include "laser_line_extraction_interfaces/msg/line_segment_list.hpp"
+#include "laser_line_extraction/line_extraction.h"
+#include "laser_line_extraction/line.h"
+
+namespace line_extraction
+{
+
+  class LineExtractionROS : public rclcpp::Node
+  {
+
+  public:
+    // Constructor / destructor
+    // LineExtractionROS(ros::NodeHandle &, ros::NodeHandle &);
+    LineExtractionROS();
+    ~LineExtractionROS();
+    // Running
+    void run();
+
+  private:
+    // ROS
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_subscriber_;
+    rclcpp::Publisher<laser_line_extraction_interfaces::msg::LineSegmentList>::SharedPtr line_publisher_;
+    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_publisher_;
+    // Parameters
+    std::string frame_id_;
+    std::string scan_topic_;
+    bool pub_markers_;
+    double frequency_;
+    // Line extraction
+    rclcpp::TimerBase::SharedPtr timer_;
+    LineExtraction line_extraction_;
+    bool data_cached_; // true after first scan used to cache data
+    // Members
+    void loadParameters();
+    void populateLineSegListMsg(const std::vector<Line> &, laser_line_extraction_interfaces::msg::LineSegmentList &);
+    void populateMarkerMsg(const std::vector<Line> &, visualization_msgs::msg::Marker &);
+    void cacheData(const sensor_msgs::msg::LaserScan::SharedPtr &);
+    void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr);
+  };
+
+} // namespace line_extraction
+
+#endif

laser_line_extraction/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>laser_line_extraction</name>
+  <version>0.1.0</version>
+  <description>A ROS package to extract line segments from LaserScan messages.</description>
+  <maintainer email="m.gallant@queensu.ca">Marc Gallant</maintainer>
+  <license>BSD</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>eigen</build_depend>
+  <depend>laser_line_extraction_interfaces</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>