From 018f5413cc77d0f04c6fe1f73b106653ba3f1357 Mon Sep 17 00:00:00 2001 From: GnSight Date: Mon, 15 Aug 2022 09:10:38 +0800 Subject: [PATCH] add ros2 support --- CMakeLists.txt | 53 --- include/laser_line_extraction/line.h | 67 ---- .../line_extraction_ros.h | 52 --- laser_line_extraction/CMakeLists.txt | 55 +++ .../include/laser_line_extraction/line.h | 67 ++++ .../laser_line_extraction/line_extraction.h | 0 .../line_extraction_ros.h | 53 +++ .../laser_line_extraction/utilities.h | 0 laser_line_extraction/package.xml | 24 ++ laser_line_extraction/src/line.cpp | 304 +++++++++++++++ laser_line_extraction/src/line_extraction.cpp | 362 +++++++++++++++++ .../src/line_extraction_node.cpp | 24 ++ .../src/line_extraction_ros.cpp | 237 ++++++++++++ .../CMakeLists.txt | 34 ++ .../msg/LineSegment.msg | 5 + .../msg}/LineSegmentList.msg | 4 +- laser_line_extraction_interfaces/package.xml | 23 ++ launch/debug.launch | 8 - launch/example.launch | 20 - msg/LineSegment.msg | 5 - package.xml | 25 -- src/line.cpp | 304 --------------- src/line_extraction.cpp | 363 ------------------ src/line_extraction_node.cpp | 32 -- src/line_extraction_ros.cpp | 220 ----------- 25 files changed, 1190 insertions(+), 1151 deletions(-) delete mode 100644 CMakeLists.txt delete mode 100644 include/laser_line_extraction/line.h delete mode 100644 include/laser_line_extraction/line_extraction_ros.h create mode 100644 laser_line_extraction/CMakeLists.txt create mode 100644 laser_line_extraction/include/laser_line_extraction/line.h rename {include => laser_line_extraction/include}/laser_line_extraction/line_extraction.h (100%) create mode 100644 laser_line_extraction/include/laser_line_extraction/line_extraction_ros.h rename {include => laser_line_extraction/include}/laser_line_extraction/utilities.h (100%) create mode 100644 laser_line_extraction/package.xml create mode 100644 laser_line_extraction/src/line.cpp create mode 100644 laser_line_extraction/src/line_extraction.cpp create mode 100644 laser_line_extraction/src/line_extraction_node.cpp create mode 100644 laser_line_extraction/src/line_extraction_ros.cpp create mode 100644 laser_line_extraction_interfaces/CMakeLists.txt create mode 100644 laser_line_extraction_interfaces/msg/LineSegment.msg rename {msg => laser_line_extraction_interfaces/msg}/LineSegmentList.msg (52%) create mode 100644 laser_line_extraction_interfaces/package.xml delete mode 100644 launch/debug.launch delete mode 100644 launch/example.launch delete mode 100644 msg/LineSegment.msg delete mode 100644 package.xml delete mode 100644 src/line.cpp delete mode 100644 src/line_extraction.cpp delete mode 100644 src/line_extraction_node.cpp delete mode 100644 src/line_extraction_ros.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index ce83f87..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(laser_line_extraction) - -find_package(catkin REQUIRED COMPONENTS - cmake_modules - geometry_msgs - message_generation - roscpp - rospy - sensor_msgs - visualization_msgs -) - -find_package(Eigen3 REQUIRED) - -add_message_files( - FILES - LineSegment.msg - LineSegmentList.msg -) - -generate_messages( - DEPENDENCIES - sensor_msgs -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES line line_extraction line_extraction_ros - CATKIN_DEPENDS geometry_msgs message_runtime roscpp sensor_msgs visualization_msgs -) - -add_library(line src/line.cpp) -target_link_libraries(line ${catkin_LIBRARIES}) - -add_library(line_extraction src/line_extraction.cpp) -target_link_libraries(line_extraction ${catkin_LIBRARIES}) - -add_library(line_extraction_ros src/line_extraction_ros.cpp) -target_link_libraries(line_extraction_ros line line_extraction ${catkin_LIBRARIES}) -add_dependencies(line_extraction_ros laser_line_extraction_generate_messages_cpp) - -add_executable(line_extraction_node src/line_extraction_node.cpp) -target_link_libraries(line_extraction_node line_extraction_ros ${catkin_LIBRARIES}) - -include_directories(include ${catkin_INCLUDE_DIRS}) -include_directories(include ${EIGEN3_INCLUDE_DIRS}) - -# catkin_add_gtest(${PROJECT_NAME}-test test/test_laser_line_extraction.cpp) - -install(TARGETS line_extraction_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(TARGETS line_extraction_ros line_extraction line ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) diff --git a/include/laser_line_extraction/line.h b/include/laser_line_extraction/line.h deleted file mode 100644 index 32f0b1f..0000000 --- a/include/laser_line_extraction/line.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef LINE_EXTRACTION_LINE_H -#define LINE_EXTRACTION_LINE_H - -#include -#include -#include "laser_line_extraction/utilities.h" - -namespace line_extraction -{ - -class Line -{ - -public: - // Constructor / destructor - Line(const CachedData&, const RangeData&, const Params&, std::vector); - Line(double angle, double radius, const boost::array &covariance, - const boost::array &start, const boost::array &end, - const std::vector &indices); - ~Line(); - // Get methods for the line parameters - double getAngle() const; - const boost::array& getCovariance() const; - const boost::array& getEnd() const; - const std::vector& getIndices() const; - double getRadius() const; - const boost::array& 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 indices_; - // Data structures - CachedData c_data_; - RangeData r_data_; - Params params_; - PointParams p_params_; - // Point variances used for least squares - std::vector point_scalar_vars_; - std::vector > point_covs_; - double p_rr_; - // Line parameters - double angle_; - double radius_; - boost::array start_; - boost::array end_; - boost::array 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 diff --git a/include/laser_line_extraction/line_extraction_ros.h b/include/laser_line_extraction/line_extraction_ros.h deleted file mode 100644 index a667342..0000000 --- a/include/laser_line_extraction/line_extraction_ros.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef LINE_EXTRACTION_ROS_H -#define LINE_EXTRACTION_ROS_H - -#include -#include -#include -#include -#include -#include -#include "laser_line_extraction/LineSegment.h" -#include "laser_line_extraction/LineSegmentList.h" -#include "laser_line_extraction/line_extraction.h" -#include "laser_line_extraction/line.h" - -namespace line_extraction -{ - -class LineExtractionROS -{ - -public: - // Constructor / destructor - LineExtractionROS(ros::NodeHandle&, ros::NodeHandle&); - ~LineExtractionROS(); - // Running - void run(); - -private: - // ROS - ros::NodeHandle nh_; - ros::NodeHandle nh_local_; - ros::Subscriber scan_subscriber_; - ros::Publisher line_publisher_; - ros::Publisher marker_publisher_; - // Parameters - std::string frame_id_; - std::string scan_topic_; - bool pub_markers_; - // Line extraction - LineExtraction line_extraction_; - bool data_cached_; // true after first scan used to cache data - // Members - void loadParameters(); - void populateLineSegListMsg(const std::vector&, laser_line_extraction::LineSegmentList&); - void populateMarkerMsg(const std::vector&, visualization_msgs::Marker&); - void cacheData(const sensor_msgs::LaserScan::ConstPtr&); - void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr&); -}; - -} // namespace line_extraction - -#endif diff --git a/laser_line_extraction/CMakeLists.txt b/laser_line_extraction/CMakeLists.txt new file mode 100644 index 0000000..df7324d --- /dev/null +++ b/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( 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() diff --git a/laser_line_extraction/include/laser_line_extraction/line.h b/laser_line_extraction/include/laser_line_extraction/line.h new file mode 100644 index 0000000..38ff748 --- /dev/null +++ b/laser_line_extraction/include/laser_line_extraction/line.h @@ -0,0 +1,67 @@ +#ifndef LINE_EXTRACTION_LINE_H +#define LINE_EXTRACTION_LINE_H + +#include +#include +#include "laser_line_extraction/utilities.h" + +namespace line_extraction +{ + + class Line + { + + public: + // Constructor / destructor + Line(const CachedData &, const RangeData &, const Params &, std::vector); + Line(double angle, double radius, const std::array &covariance, + const std::array &start, const std::array &end, + const std::vector &indices); + ~Line(); + // Get methods for the line parameters + double getAngle() const; + const std::array &getCovariance() const; + const std::array &getEnd() const; + const std::vector &getIndices() const; + double getRadius() const; + const std::array &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 indices_; + // Data structures + CachedData c_data_; + RangeData r_data_; + Params params_; + PointParams p_params_; + // Point variances used for least squares + std::vector point_scalar_vars_; + std::vector> point_covs_; + double p_rr_; + // Line parameters + double angle_; + double radius_; + std::array start_; + std::array end_; + std::array 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 diff --git a/include/laser_line_extraction/line_extraction.h b/laser_line_extraction/include/laser_line_extraction/line_extraction.h similarity index 100% rename from include/laser_line_extraction/line_extraction.h rename to laser_line_extraction/include/laser_line_extraction/line_extraction.h diff --git a/laser_line_extraction/include/laser_line_extraction/line_extraction_ros.h b/laser_line_extraction/include/laser_line_extraction/line_extraction_ros.h new file mode 100644 index 0000000..7e2de9f --- /dev/null +++ b/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 +#include +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#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::SharedPtr scan_subscriber_; + rclcpp::Publisher::SharedPtr line_publisher_; + rclcpp::Publisher::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 &, laser_line_extraction_interfaces::msg::LineSegmentList &); + void populateMarkerMsg(const std::vector &, visualization_msgs::msg::Marker &); + void cacheData(const sensor_msgs::msg::LaserScan::SharedPtr &); + void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr); + }; + +} // namespace line_extraction + +#endif diff --git a/include/laser_line_extraction/utilities.h b/laser_line_extraction/include/laser_line_extraction/utilities.h similarity index 100% rename from include/laser_line_extraction/utilities.h rename to laser_line_extraction/include/laser_line_extraction/utilities.h diff --git a/laser_line_extraction/package.xml b/laser_line_extraction/package.xml new file mode 100644 index 0000000..9baca72 --- /dev/null +++ b/laser_line_extraction/package.xml @@ -0,0 +1,24 @@ + + + + laser_line_extraction + 0.1.0 + A ROS package to extract line segments from LaserScan messages. + Marc Gallant + BSD + ament_cmake + + eigen + laser_line_extraction_interfaces + geometry_msgs + rclcpp + sensor_msgs + visualization_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/laser_line_extraction/src/line.cpp b/laser_line_extraction/src/line.cpp new file mode 100644 index 0000000..87061f4 --- /dev/null +++ b/laser_line_extraction/src/line.cpp @@ -0,0 +1,304 @@ +#include "laser_line_extraction/line.h" +#include + +namespace line_extraction +{ + + /////////////////////////////////////////////////////////////////////////////// + // Constructor / destructor + /////////////////////////////////////////////////////////////////////////////// + Line::Line(const CachedData &c_data, const RangeData &r_data, const Params ¶ms, + std::vector indices) : c_data_(c_data), + r_data_(r_data), + params_(params), + indices_(indices) + { + } + + Line::Line(double angle, double radius, const std::array &covariance, + const std::array &start, const std::array &end, + const std::vector &indices) : angle_(angle), + radius_(radius), + covariance_(covariance), + start_(start), + end_(end), + indices_(indices) + { + } + + Line::~Line() + { + } + + /////////////////////////////////////////////////////////////////////////////// + // Get methods for line parameters + /////////////////////////////////////////////////////////////////////////////// + double Line::getAngle() const + { + return angle_; + } + + const std::array &Line::getCovariance() const + { + return covariance_; + } + + const std::array &Line::getEnd() const + { + return end_; + } + + const std::vector &Line::getIndices() const + { + return indices_; + } + + double Line::getRadius() const + { + return radius_; + } + + const std::array &Line::getStart() const + { + return start_; + } + + /////////////////////////////////////////////////////////////////////////////// + // Utility methods + /////////////////////////////////////////////////////////////////////////////// + double Line::distToPoint(unsigned int index) + { + double p_rad = sqrt(pow(r_data_.xs[index], 2) + pow(r_data_.ys[index], 2)); + double p_ang = atan2(r_data_.ys[index], r_data_.xs[index]); + return fabs(p_rad * cos(p_ang - angle_) - radius_); + } + + double Line::length() const + { + return sqrt(pow(start_[0] - end_[0], 2) + pow(start_[1] - end_[1], 2)); + } + + unsigned int Line::numPoints() const + { + return indices_.size(); + } + + void Line::projectEndpoints() + { + double s = -1.0 / tan(angle_); + double b = radius_ / sin(angle_); + double x = start_[0]; + double y = start_[1]; + start_[0] = (s * y + x - s * b) / (pow(s, 2) + 1); + start_[1] = (pow(s, 2) * y + s * x + b) / (pow(s, 2) + 1); + x = end_[0]; + y = end_[1]; + end_[0] = (s * y + x - s * b) / (pow(s, 2) + 1); + end_[1] = (pow(s, 2) * y + s * x + b) / (pow(s, 2) + 1); + } + + /////////////////////////////////////////////////////////////////////////////// + // Methods for endpoint line fitting + /////////////////////////////////////////////////////////////////////////////// + void Line::endpointFit() + { + start_[0] = r_data_.xs[indices_[0]]; + start_[1] = r_data_.ys[indices_[0]]; + end_[0] = r_data_.xs[indices_.back()]; + end_[1] = r_data_.ys[indices_.back()]; + angleFromEndpoints(); + radiusFromEndpoints(); + } + + void Line::angleFromEndpoints() + { + double slope; + if (fabs(end_[0] - start_[0]) > 1e-9) + { + slope = (end_[1] - start_[1]) / (end_[0] - start_[0]); + angle_ = pi_to_pi(atan(slope) + M_PI / 2); + } + else + { + angle_ = 0.0; + } + } + + void Line::radiusFromEndpoints() + { + radius_ = start_[0] * cos(angle_) + start_[1] * sin(angle_); + if (radius_ < 0) + { + radius_ = -radius_; + angle_ = pi_to_pi(angle_ + M_PI); + } + } + + /////////////////////////////////////////////////////////////////////////////// + // Methods for least squares line fitting + /////////////////////////////////////////////////////////////////////////////// + void Line::leastSqFit() + { + calcPointCovariances(); + double prev_radius = 0.0; + double prev_angle = 0.0; + while (fabs(radius_ - prev_radius) > params_.least_sq_radius_thresh || + fabs(angle_ - prev_angle) > params_.least_sq_angle_thresh) + { + prev_radius = radius_; + prev_angle = angle_; + calcPointScalarCovariances(); + radiusFromLeastSq(); + angleFromLeastSq(); + } + calcCovariance(); + projectEndpoints(); + } + + void Line::angleFromLeastSq() + { + calcPointParameters(); + angle_ += angleIncrement(); + } + + double Line::angleIncrement() + { + const std::vector &a = p_params_.a; + const std::vector &ap = p_params_.ap; + const std::vector &app = p_params_.app; + const std::vector &b = p_params_.b; + const std::vector &bp = p_params_.bp; + const std::vector &bpp = p_params_.bpp; + const std::vector &c = p_params_.c; + const std::vector &s = p_params_.s; + + double numerator = 0; + double denominator = 0; + for (std::size_t i = 0; i < a.size(); ++i) + { + numerator += (b[i] * ap[i] - a[i] * bp[i]) / pow(b[i], 2); + denominator += ((app[i] * b[i] - a[i] * bpp[i]) * b[i] - + 2 * (ap[i] * b[i] - a[i] * bp[i]) * bp[i]) / + pow(b[i], 3); + } + return -(numerator / denominator); + } + + void Line::calcCovariance() + { + covariance_[0] = p_rr_; + + const std::vector &a = p_params_.a; + const std::vector &ap = p_params_.ap; + const std::vector &app = p_params_.app; + const std::vector &b = p_params_.b; + const std::vector &bp = p_params_.bp; + const std::vector &bpp = p_params_.bpp; + const std::vector &c = p_params_.c; + const std::vector &s = p_params_.s; + + double G = 0; + double A = 0; + double B = 0; + double r, phi; + for (std::size_t i = 0; i < a.size(); ++i) + { + r = r_data_.ranges[indices_[i]]; // range + phi = c_data_.bearings[indices_[i]]; // bearing + G += ((app[i] * b[i] - a[i] * bpp[i]) * b[i] - 2 * (ap[i] * b[i] - a[i] * bp[i]) * bp[i]) / pow(b[i], 3); + A += 2 * r * sin(angle_ - phi) / b[i]; + B += 4 * pow(r, 2) * pow(sin(angle_ - phi), 2) / b[i]; + } + covariance_[1] = p_rr_ * A / G; + covariance_[2] = covariance_[1]; + covariance_[3] = pow(1.0 / G, 2) * B; + } + + void Line::calcPointCovariances() + { + point_covs_.clear(); + double r, phi, var_r, var_phi; + for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) + { + r = r_data_.ranges[*cit]; // range + phi = c_data_.bearings[*cit]; // bearing + var_r = params_.range_var; // range variance + var_phi = params_.bearing_var; // bearing variance + std::array Q; + Q[0] = pow(r, 2) * var_phi * pow(sin(phi), 2) + var_r * pow(cos(phi), 2); + Q[1] = -pow(r, 2) * var_phi * sin(2 * phi) / 2.0 + var_r * sin(2 * phi) / 2.0; + Q[2] = Q[1]; + Q[3] = pow(r, 2) * var_phi * pow(cos(phi), 2) + var_r * pow(sin(phi), 2); + point_covs_.push_back(Q); + } + } + + void Line::calcPointParameters() + { + p_params_.a.clear(); + p_params_.ap.clear(); + p_params_.app.clear(); + p_params_.b.clear(); + p_params_.bp.clear(); + p_params_.bpp.clear(); + p_params_.c.clear(); + p_params_.s.clear(); + + double r, phi, var_r, var_phi; + double a, ap, app, b, bp, bpp, c, s; + for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) + { + r = r_data_.ranges[*cit]; // range + phi = c_data_.bearings[*cit]; // bearing + var_r = params_.range_var; // range variance + var_phi = params_.bearing_var; // bearing variance + c = cos(angle_ - phi); + s = sin(angle_ - phi); + a = pow(r * c - radius_, 2); + ap = -2 * r * s * (r * c - radius_); + app = 2 * pow(r, 2) * pow(s, 2) - 2 * r * c * (r * c - radius_); + b = var_r * pow(c, 2) + var_phi * pow(r, 2) * pow(s, 2); + bp = 2 * (pow(r, 2) * var_phi - var_r) * c * s; + bpp = 2 * (pow(r, 2) * var_phi - var_r) * (pow(c, 2) - pow(s, 2)); + p_params_.a.push_back(a); + p_params_.ap.push_back(ap); + p_params_.app.push_back(app); + p_params_.b.push_back(b); + p_params_.bp.push_back(bp); + p_params_.bpp.push_back(bpp); + p_params_.c.push_back(c); + p_params_.s.push_back(s); + } + } + + void Line::calcPointScalarCovariances() + { + point_scalar_vars_.clear(); + double P; + double inverse_P_sum = 0; + for (std::vector>::const_iterator cit = point_covs_.begin(); + cit != point_covs_.end(); ++cit) + { + P = (*cit)[0] * pow(cos(angle_), 2) + 2 * (*cit)[1] * sin(angle_) * cos(angle_) + + (*cit)[3] * pow(sin(angle_), 2); + inverse_P_sum += 1.0 / P; + point_scalar_vars_.push_back(P); + } + p_rr_ = 1.0 / inverse_P_sum; + } + + void Line::radiusFromLeastSq() + { + radius_ = 0; + double r, phi; + for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) + { + r = r_data_.ranges[*cit]; // range + phi = c_data_.bearings[*cit]; // bearing + radius_ += r * cos(angle_ - phi) / point_scalar_vars_[cit - indices_.begin()]; // cit to index + } + + radius_ *= p_rr_; + } + +} // namespace line_extraction diff --git a/laser_line_extraction/src/line_extraction.cpp b/laser_line_extraction/src/line_extraction.cpp new file mode 100644 index 0000000..e1ab120 --- /dev/null +++ b/laser_line_extraction/src/line_extraction.cpp @@ -0,0 +1,362 @@ +#include "laser_line_extraction/line_extraction.h" +#include +#include +#include + +namespace line_extraction +{ + + /////////////////////////////////////////////////////////////////////////////// + // Constructor / destructor + /////////////////////////////////////////////////////////////////////////////// + LineExtraction::LineExtraction() + { + } + + LineExtraction::~LineExtraction() + { + } + + /////////////////////////////////////////////////////////////////////////////// + // Main run function + /////////////////////////////////////////////////////////////////////////////// + void LineExtraction::extractLines(std::vector &lines) + { + // Resets + filtered_indices_ = c_data_.indices; + lines_.clear(); + + // Filter indices + filterCloseAndFarPoints(); + filterOutlierPoints(); + + // Return no lines if not enough points left + if (filtered_indices_.size() <= std::max(params_.min_line_points, static_cast(3))) + { + return; + } + + // Split indices into lines and filter out short and sparse lines + split(filtered_indices_); + filterLines(); + + // Fit each line using least squares and merge colinear lines + for (std::vector::iterator it = lines_.begin(); it != lines_.end(); ++it) + { + it->leastSqFit(); + } + + // If there is more than one line, check if lines should be merged based on the merging criteria + if (lines_.size() > 1) + { + mergeLines(); + } + + lines = lines_; + } + + /////////////////////////////////////////////////////////////////////////////// + // Data setting + /////////////////////////////////////////////////////////////////////////////// + void LineExtraction::setCachedData(const std::vector &bearings, + const std::vector &cos_bearings, + const std::vector &sin_bearings, + const std::vector &indices) + { + c_data_.bearings = bearings; + c_data_.cos_bearings = cos_bearings; + c_data_.sin_bearings = sin_bearings; + c_data_.indices = indices; + } + + void LineExtraction::setRangeData(const std::vector &ranges) + { + r_data_.ranges = ranges; + r_data_.xs.clear(); + r_data_.ys.clear(); + for (std::vector::const_iterator cit = c_data_.indices.begin(); + cit != c_data_.indices.end(); ++cit) + { + r_data_.xs.push_back(c_data_.cos_bearings[*cit] * ranges[*cit]); + r_data_.ys.push_back(c_data_.sin_bearings[*cit] * ranges[*cit]); + } + } + + /////////////////////////////////////////////////////////////////////////////// + // Parameter setting + /////////////////////////////////////////////////////////////////////////////// + void LineExtraction::setBearingVariance(double value) + { + params_.bearing_var = value; + } + + void LineExtraction::setRangeVariance(double value) + { + params_.range_var = value; + } + + void LineExtraction::setLeastSqAngleThresh(double value) + { + params_.least_sq_angle_thresh = value; + } + + void LineExtraction::setLeastSqRadiusThresh(double value) + { + params_.least_sq_radius_thresh = value; + } + + void LineExtraction::setMaxLineGap(double value) + { + params_.max_line_gap = value; + } + + void LineExtraction::setMinLineLength(double value) + { + params_.min_line_length = value; + } + + void LineExtraction::setMinLinePoints(unsigned int value) + { + params_.min_line_points = value; + } + + void LineExtraction::setMinRange(double value) + { + params_.min_range = value; + } + + void LineExtraction::setMaxRange(double value) + { + params_.max_range = value; + } + + void LineExtraction::setMinSplitDist(double value) + { + params_.min_split_dist = value; + } + + void LineExtraction::setOutlierDist(double value) + { + params_.outlier_dist = value; + } + + /////////////////////////////////////////////////////////////////////////////// + // Utility methods + /////////////////////////////////////////////////////////////////////////////// + double LineExtraction::chiSquared(const Eigen::Vector2d &dL, const Eigen::Matrix2d &P_1, + const Eigen::Matrix2d &P_2) + { + return dL.transpose() * (P_1 + P_2).inverse() * dL; + } + + double LineExtraction::distBetweenPoints(unsigned int index_1, unsigned int index_2) + { + return sqrt(pow(r_data_.xs[index_1] - r_data_.xs[index_2], 2) + + pow(r_data_.ys[index_1] - r_data_.ys[index_2], 2)); + } + + /////////////////////////////////////////////////////////////////////////////// + // Filtering points + /////////////////////////////////////////////////////////////////////////////// + void LineExtraction::filterCloseAndFarPoints() + { + std::vector output; + for (std::vector::const_iterator cit = filtered_indices_.begin(); + cit != filtered_indices_.end(); ++cit) + { + const double &range = r_data_.ranges[*cit]; + if (range >= params_.min_range && range <= params_.max_range) + { + output.push_back(*cit); + } + } + filtered_indices_ = output; + } + + void LineExtraction::filterOutlierPoints() + { + if (filtered_indices_.size() < 3) + { + return; + } + + std::vector output; + unsigned int p_i, p_j, p_k; + for (std::size_t i = 0; i < filtered_indices_.size(); ++i) + { + + // Get two closest neighbours + + p_i = filtered_indices_[i]; + if (i == 0) // first point + { + p_j = filtered_indices_[i + 1]; + p_k = filtered_indices_[i + 2]; + } + else if (i == filtered_indices_.size() - 1) // last point + { + p_j = filtered_indices_[i - 1]; + p_k = filtered_indices_[i - 2]; + } + else // middle points + { + p_j = filtered_indices_[i - 1]; + p_k = filtered_indices_[i + 1]; + } + + // Check if point is an outlier + + if (fabs(r_data_.ranges[p_i] - r_data_.ranges[p_j]) > params_.outlier_dist && + fabs(r_data_.ranges[p_i] - r_data_.ranges[p_k]) > params_.outlier_dist) + { + // Check if it is close to line connecting its neighbours + std::vector line_indices; + line_indices.push_back(p_j); + line_indices.push_back(p_k); + Line line(c_data_, r_data_, params_, line_indices); + line.endpointFit(); + if (line.distToPoint(p_i) > params_.min_split_dist) + { + continue; // point is an outlier + } + } + + output.push_back(p_i); + } + + filtered_indices_ = output; + } + + /////////////////////////////////////////////////////////////////////////////// + // Filtering and merging lines + /////////////////////////////////////////////////////////////////////////////// + void LineExtraction::filterLines() + { + std::vector output; + for (std::vector::const_iterator cit = lines_.begin(); cit != lines_.end(); ++cit) + { + if (cit->length() >= params_.min_line_length && cit->numPoints() >= params_.min_line_points) + { + output.push_back(*cit); + } + } + lines_ = output; + } + + void LineExtraction::mergeLines() + { + std::vector merged_lines; + + for (std::size_t i = 1; i < lines_.size(); ++i) + { + // Get L, P_1, P_2 of consecutive lines + Eigen::Vector2d L_1(lines_[i - 1].getRadius(), lines_[i - 1].getAngle()); + Eigen::Vector2d L_2(lines_[i].getRadius(), lines_[i].getAngle()); + Eigen::Matrix2d P_1; + P_1 << lines_[i - 1].getCovariance()[0], lines_[i - 1].getCovariance()[1], + lines_[i - 1].getCovariance()[2], lines_[i - 1].getCovariance()[3]; + Eigen::Matrix2d P_2; + P_2 << lines_[i].getCovariance()[0], lines_[i].getCovariance()[1], + lines_[i].getCovariance()[2], lines_[i].getCovariance()[3]; + + // Merge lines if chi-squared distance is less than 3 + if (chiSquared(L_1 - L_2, P_1, P_2) < 3) + { + // Get merged angle, radius, and covariance + Eigen::Matrix2d P_m = (P_1.inverse() + P_2.inverse()).inverse(); + Eigen::Vector2d L_m = P_m * (P_1.inverse() * L_1 + P_2.inverse() * L_2); + // Populate new line with these merged parameters + std::array cov; + cov[0] = P_m(0, 0); + cov[1] = P_m(0, 1); + cov[2] = P_m(1, 0); + cov[3] = P_m(1, 1); + std::vector indices; + const std::vector &ind_1 = lines_[i - 1].getIndices(); + const std::vector &ind_2 = lines_[i].getIndices(); + indices.resize(ind_1.size() + ind_2.size()); + indices.insert(indices.end(), ind_1.begin(), ind_1.end()); + indices.insert(indices.end(), ind_2.begin(), ind_2.end()); + Line merged_line(L_m[1], L_m[0], cov, lines_[i - 1].getStart(), lines_[i].getEnd(), indices); + // Project the new endpoints + merged_line.projectEndpoints(); + lines_[i] = merged_line; + } + else + { + merged_lines.push_back(lines_[i - 1]); + } + + if (i == lines_.size() - 1) + { + merged_lines.push_back(lines_[i]); + } + } + lines_ = merged_lines; + } + + /////////////////////////////////////////////////////////////////////////////// + // Splitting points into lines + /////////////////////////////////////////////////////////////////////////////// + void LineExtraction::split(const std::vector &indices) + { + // Don't split if only a single point (only occurs when orphaned by gap) + if (indices.size() <= 1) + { + return; + } + + Line line(c_data_, r_data_, params_, indices); + line.endpointFit(); + double dist_max = 0; + double gap_max = 0; + double dist, gap; + int i_max, i_gap; + + // Find the farthest point and largest gap + for (std::size_t i = 1; i < indices.size() - 1; ++i) + { + dist = line.distToPoint(indices[i]); + if (dist > dist_max) + { + dist_max = dist; + i_max = i; + } + gap = distBetweenPoints(indices[i], indices[i + 1]); + if (gap > gap_max) + { + gap_max = gap; + i_gap = i; + } + } + + // Check for gaps at endpoints + double gap_start = distBetweenPoints(indices[0], indices[1]); + if (gap_start > gap_max) + { + gap_max = gap_start; + i_gap = 1; + } + double gap_end = distBetweenPoints(indices.rbegin()[1], indices.rbegin()[0]); + if (gap_end > gap_max) + { + gap_max = gap_end; + i_gap = indices.size() - 1; + } + + // Check if line meets requirements or should be split + if (dist_max < params_.min_split_dist && gap_max < params_.max_line_gap) + { + lines_.push_back(line); + } + else + { + int i_split = dist_max >= params_.min_split_dist ? i_max : i_gap; + std::vector first_split(&indices[0], &indices[i_split - 1]); + std::vector second_split(&indices[i_split], &indices.back()); + split(first_split); + split(second_split); + } + } + +} // namespace line_extraction diff --git a/laser_line_extraction/src/line_extraction_node.cpp b/laser_line_extraction/src/line_extraction_node.cpp new file mode 100644 index 0000000..c6d5a79 --- /dev/null +++ b/laser_line_extraction/src/line_extraction_node.cpp @@ -0,0 +1,24 @@ +#include "laser_line_extraction/line_extraction_ros.h" +#include + +int main(int argc, char **argv) +{ + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; + + // double frequency; + // nh_local.param("frequency", frequency, 25); + // ROS_DEBUG("Frequency set to %0.1f Hz", frequency); + // ros::Rate rate(frequency); + + // while (ros::ok()) + // { + // line_extractor.run(); + // ros::spinOnce(); + // rate.sleep(); + // } + // return 0; +} diff --git a/laser_line_extraction/src/line_extraction_ros.cpp b/laser_line_extraction/src/line_extraction_ros.cpp new file mode 100644 index 0000000..407c95a --- /dev/null +++ b/laser_line_extraction/src/line_extraction_ros.cpp @@ -0,0 +1,237 @@ +#include "laser_line_extraction/line_extraction_ros.h" +#include +#include +using namespace std::chrono_literals; + +#include "rclcpp/rclcpp.hpp" + +namespace line_extraction +{ + + /////////////////////////////////////////////////////////////////////////////// + // Constructor / destructor + /////////////////////////////////////////////////////////////////////////////// + + LineExtractionROS::LineExtractionROS() : Node("line_extraction") + { + loadParameters(); + line_publisher_ = this->create_publisher("line_segments", 1); + scan_subscriber_ = this->create_subscription(scan_topic_, 1, std::bind(&LineExtractionROS::laserScanCallback, this, std::placeholders::_1)); + if (pub_markers_) + { + marker_publisher_ = this->create_publisher("line_markers", 1); + } + timer_ = this->create_wall_timer( + std::chrono::milliseconds((int)(1000.0 / frequency_)), std::bind(&LineExtractionROS::run, this)); + } + + LineExtractionROS::~LineExtractionROS() + { + } + + /////////////////////////////////////////////////////////////////////////////// + // Run + /////////////////////////////////////////////////////////////////////////////// + void LineExtractionROS::run() + { + // Extract the lines + std::vector lines; + line_extraction_.extractLines(lines); + + // Populate message + laser_line_extraction_interfaces::msg::LineSegmentList msg; + populateLineSegListMsg(lines, msg); + + // Publish the lines + line_publisher_->publish(msg); + + // Also publish markers if parameter publish_markers is set to true + if (pub_markers_) + { + visualization_msgs::msg::Marker marker_msg; + populateMarkerMsg(lines, marker_msg); + marker_publisher_->publish(marker_msg); + } + } + + /////////////////////////////////////////////////////////////////////////////// + // Load ROS parameters + /////////////////////////////////////////////////////////////////////////////// + void LineExtractionROS::loadParameters() + { + + RCLCPP_DEBUG(this->get_logger(), "*************************************"); + RCLCPP_DEBUG(this->get_logger(), "PARAMETERS:"); + + // Parameters used by this node + + this->declare_parameter("frequency", 25); + this->get_parameter("frequency", frequency_); + + RCLCPP_DEBUG(this->get_logger(), "frequency: %f", frequency_); + + this->declare_parameter("frame_id", "laser"); + this->get_parameter("frame_id", frame_id_); + + RCLCPP_DEBUG(this->get_logger(), "frame_id: %s", frame_id_.c_str()); + + this->declare_parameter("scan_topic", "scan"); + this->get_parameter("scan_topic", scan_topic_); + + RCLCPP_DEBUG(this->get_logger(), "scan_topic: %s", scan_topic_.c_str()); + + this->declare_parameter("publish_markers", false); + this->get_parameter("publish_markers", pub_markers_); + + RCLCPP_DEBUG(this->get_logger(), "publish_markers: %s", pub_markers_ ? "true" : "false"); + + // Parameters used by the line extraction algorithm + + double bearing_std_dev, range_std_dev, least_sq_angle_thresh, least_sq_radius_thresh, + max_line_gap, min_line_length, min_range, max_range, min_split_dist, outlier_dist; + int min_line_points; + + this->declare_parameter("bearing_std_dev", 1e-3); + this->get_parameter("bearing_std_dev", bearing_std_dev); + line_extraction_.setBearingVariance(bearing_std_dev * bearing_std_dev); + RCLCPP_DEBUG(this->get_logger(), "bearing_std_dev: %f", bearing_std_dev); + + this->declare_parameter("range_std_dev", 0.02); + this->get_parameter("range_std_dev", range_std_dev); + line_extraction_.setRangeVariance(range_std_dev * range_std_dev); + RCLCPP_DEBUG(this->get_logger(), "range_std_dev: %f", range_std_dev); + + this->declare_parameter("least_sq_angle_thresh", 1e-4); + this->get_parameter("least_sq_angle_thresh", least_sq_angle_thresh); + line_extraction_.setLeastSqAngleThresh(least_sq_angle_thresh); + RCLCPP_DEBUG(this->get_logger(), "least_sq_angle_thresh: %f", least_sq_angle_thresh); + + this->declare_parameter("least_sq_radius_thresh", 1e-4); + this->get_parameter("least_sq_radius_thresh", least_sq_radius_thresh); + line_extraction_.setLeastSqRadiusThresh(least_sq_radius_thresh); + RCLCPP_DEBUG(this->get_logger(), "least_sq_radius_thresh: %f", least_sq_radius_thresh); + + this->declare_parameter("max_line_gap", 0.4); + this->get_parameter("max_line_gap", max_line_gap); + line_extraction_.setMaxLineGap(max_line_gap); + RCLCPP_DEBUG(this->get_logger(), "max_line_gap: %f", max_line_gap); + + this->declare_parameter("min_line_length", 0.5); + this->get_parameter("min_line_length", min_line_length); + line_extraction_.setMinLineLength(min_line_length); + RCLCPP_DEBUG(this->get_logger(), "min_line_length: %f", min_line_length); + + this->declare_parameter("min_range", 0.4); + this->get_parameter("min_range", min_range); + line_extraction_.setMinRange(min_range); + RCLCPP_DEBUG(this->get_logger(), "min_range: %f", min_range); + + this->declare_parameter("max_range", 10000.0); + this->get_parameter("max_range", max_range); + line_extraction_.setMaxRange(max_range); + RCLCPP_DEBUG(this->get_logger(), "max_range: %f", max_range); + + this->declare_parameter("min_split_dist", 0.05); + this->get_parameter("min_split_dist", min_split_dist); + line_extraction_.setMinSplitDist(min_split_dist); + RCLCPP_DEBUG(this->get_logger(), "min_split_dist: %f", min_split_dist); + + this->declare_parameter("outlier_dist", 0.05); + this->get_parameter("outlier_dist", outlier_dist); + line_extraction_.setOutlierDist(outlier_dist); + RCLCPP_DEBUG(this->get_logger(), "outlier_dist: %f", outlier_dist); + + this->declare_parameter("min_line_points", 9); + this->get_parameter("min_line_points", min_line_points); + line_extraction_.setMinLinePoints(static_cast(min_line_points)); + RCLCPP_DEBUG(this->get_logger(), "min_line_points: %d", min_line_points); + + RCLCPP_DEBUG(this->get_logger(), "*************************************"); + } + + /////////////////////////////////////////////////////////////////////////////// + // Populate messages + /////////////////////////////////////////////////////////////////////////////// + void LineExtractionROS::populateLineSegListMsg(const std::vector &lines, + laser_line_extraction_interfaces::msg::LineSegmentList &line_list_msg) + { + for (std::vector::const_iterator cit = lines.begin(); cit != lines.end(); ++cit) + { + laser_line_extraction_interfaces::msg::LineSegment line_msg; + line_msg.angle = cit->getAngle(); + line_msg.radius = cit->getRadius(); + line_msg.covariance = cit->getCovariance(); + line_msg.start = cit->getStart(); + line_msg.end = cit->getEnd(); + line_list_msg.line_segments.push_back(line_msg); + } + line_list_msg.header.frame_id = frame_id_; + line_list_msg.header.stamp = this->now(); + } + + void LineExtractionROS::populateMarkerMsg(const std::vector &lines, + visualization_msgs::msg::Marker &marker_msg) + { + marker_msg.ns = "line_extraction"; + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::LINE_LIST; + marker_msg.scale.x = 0.1; + marker_msg.color.r = 1.0; + marker_msg.color.g = 0.0; + marker_msg.color.b = 0.0; + marker_msg.color.a = 1.0; + for (std::vector::const_iterator cit = lines.begin(); cit != lines.end(); ++cit) + { + geometry_msgs::msg::Point p_start; + p_start.x = cit->getStart()[0]; + p_start.y = cit->getStart()[1]; + p_start.z = 0; + marker_msg.points.push_back(p_start); + geometry_msgs::msg::Point p_end; + p_end.x = cit->getEnd()[0]; + p_end.y = cit->getEnd()[1]; + p_end.z = 0; + marker_msg.points.push_back(p_end); + } + marker_msg.header.frame_id = frame_id_; + marker_msg.header.stamp = this->now(); + } + + /////////////////////////////////////////////////////////////////////////////// + // Cache data on first LaserScan message received + /////////////////////////////////////////////////////////////////////////////// + void LineExtractionROS::cacheData(const sensor_msgs::msg::LaserScan::SharedPtr &scan_msg) + { + std::vector bearings, cos_bearings, sin_bearings; + std::vector indices; + const std::size_t num_measurements = std::ceil( + (scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment); + for (std::size_t i = 0; i < num_measurements; ++i) + { + const double b = scan_msg->angle_min + i * scan_msg->angle_increment; + bearings.push_back(b); + cos_bearings.push_back(cos(b)); + sin_bearings.push_back(sin(b)); + indices.push_back(i); + } + + line_extraction_.setCachedData(bearings, cos_bearings, sin_bearings, indices); + RCLCPP_DEBUG(this->get_logger(), "Data has been cached."); + } + + /////////////////////////////////////////////////////////////////////////////// + // Main LaserScan callback + /////////////////////////////////////////////////////////////////////////////// + void LineExtractionROS::laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg) + { + if (!data_cached_) + { + cacheData(scan_msg); + data_cached_ = true; + } + + std::vector scan_ranges_doubles(scan_msg->ranges.begin(), scan_msg->ranges.end()); + line_extraction_.setRangeData(scan_ranges_doubles); + } + +} // namespace line_extraction diff --git a/laser_line_extraction_interfaces/CMakeLists.txt b/laser_line_extraction_interfaces/CMakeLists.txt new file mode 100644 index 0000000..6ef3607 --- /dev/null +++ b/laser_line_extraction_interfaces/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.8) +project(laser_line_extraction_interfaces) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/LineSegment.msg" + "msg/LineSegmentList.msg" + DEPENDENCIES std_msgs +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/laser_line_extraction_interfaces/msg/LineSegment.msg b/laser_line_extraction_interfaces/msg/LineSegment.msg new file mode 100644 index 0000000..cef0365 --- /dev/null +++ b/laser_line_extraction_interfaces/msg/LineSegment.msg @@ -0,0 +1,5 @@ +float64 radius +float64 angle +float64[4] covariance +float64[2] start +float64[2] end diff --git a/msg/LineSegmentList.msg b/laser_line_extraction_interfaces/msg/LineSegmentList.msg similarity index 52% rename from msg/LineSegmentList.msg rename to laser_line_extraction_interfaces/msg/LineSegmentList.msg index e11a7c5..e1e848b 100644 --- a/msg/LineSegmentList.msg +++ b/laser_line_extraction_interfaces/msg/LineSegmentList.msg @@ -1,2 +1,2 @@ -Header header -LineSegment[] line_segments +std_msgs/Header header +LineSegment[] line_segments diff --git a/laser_line_extraction_interfaces/package.xml b/laser_line_extraction_interfaces/package.xml new file mode 100644 index 0000000..838ed75 --- /dev/null +++ b/laser_line_extraction_interfaces/package.xml @@ -0,0 +1,23 @@ + + + + laser_line_extraction_interfaces + 0.1.0 + A ROS package to provide interfaces to the line extraction package. + gns + BSD + + std_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/launch/debug.launch b/launch/debug.launch deleted file mode 100644 index edb9bba..0000000 --- a/launch/debug.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/launch/example.launch b/launch/example.launch deleted file mode 100644 index de6eaec..0000000 --- a/launch/example.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/msg/LineSegment.msg b/msg/LineSegment.msg deleted file mode 100644 index 23248a0..0000000 --- a/msg/LineSegment.msg +++ /dev/null @@ -1,5 +0,0 @@ -float32 radius -float32 angle -float32[4] covariance -float32[2] start -float32[2] end diff --git a/package.xml b/package.xml deleted file mode 100644 index 4f9c325..0000000 --- a/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - laser_line_extraction - 0.1.0 - - A ROS package to extract line segments from LaserScan messages. - - Marc Gallant - BSD - catkin - cmake_modules - eigen - geometry_msgs - message_generation - roscpp - rospy - sensor_msgs - visualization_msgs - geometry_msgs - message_runtime - roscpp - rospy - sensor_msgs - visualization_msgs - diff --git a/src/line.cpp b/src/line.cpp deleted file mode 100644 index dd98bb9..0000000 --- a/src/line.cpp +++ /dev/null @@ -1,304 +0,0 @@ -#include "laser_line_extraction/line.h" - -namespace line_extraction -{ - -/////////////////////////////////////////////////////////////////////////////// -// Constructor / destructor -/////////////////////////////////////////////////////////////////////////////// -Line::Line(const CachedData &c_data, const RangeData &r_data, const Params ¶ms, - std::vector indices): - c_data_(c_data), - r_data_(r_data), - params_(params), - indices_(indices) -{ -} - -Line::Line(double angle, double radius, const boost::array &covariance, - const boost::array &start, const boost::array &end, - const std::vector &indices): - angle_(angle), - radius_(radius), - covariance_(covariance), - start_(start), - end_(end), - indices_(indices) -{ -} - -Line::~Line() -{ -} - -/////////////////////////////////////////////////////////////////////////////// -// Get methods for line parameters -/////////////////////////////////////////////////////////////////////////////// -double Line::getAngle() const -{ - return angle_; -} - -const boost::array& Line::getCovariance() const -{ - return covariance_; -} - -const boost::array& Line::getEnd() const -{ - return end_; -} - -const std::vector& Line::getIndices() const -{ - return indices_; -} - -double Line::getRadius() const -{ - return radius_; -} - -const boost::array& Line::getStart() const -{ - return start_; -} - -/////////////////////////////////////////////////////////////////////////////// -// Utility methods -/////////////////////////////////////////////////////////////////////////////// -double Line::distToPoint(unsigned int index) -{ - double p_rad = sqrt(pow(r_data_.xs[index], 2) + pow(r_data_.ys[index], 2)); - double p_ang = atan2(r_data_.ys[index], r_data_.xs[index]); - return fabs(p_rad * cos(p_ang - angle_) - radius_); -} - -double Line::length() const -{ - return sqrt(pow(start_[0] - end_[0], 2) + pow(start_[1] - end_[1], 2)); -} - -unsigned int Line::numPoints() const -{ - return indices_.size(); -} - -void Line::projectEndpoints() -{ - double s = -1.0 / tan(angle_); - double b = radius_ / sin(angle_); - double x = start_[0]; - double y = start_[1]; - start_[0] = (s * y + x - s * b) / (pow(s, 2) + 1); - start_[1] = (pow(s, 2) * y + s * x + b) / (pow(s, 2) + 1); - x = end_[0]; - y = end_[1]; - end_[0] = (s * y + x - s * b) / (pow(s, 2) + 1); - end_[1] = (pow(s, 2) * y + s * x + b) / (pow(s, 2) + 1); -} - -/////////////////////////////////////////////////////////////////////////////// -// Methods for endpoint line fitting -/////////////////////////////////////////////////////////////////////////////// -void Line::endpointFit() -{ - start_[0] = r_data_.xs[indices_[0]]; - start_[1] = r_data_.ys[indices_[0]]; - end_[0] = r_data_.xs[indices_.back()]; - end_[1] = r_data_.ys[indices_.back()]; - angleFromEndpoints(); - radiusFromEndpoints(); -} - -void Line::angleFromEndpoints() -{ - double slope; - if (fabs(end_[0] - start_[0]) > 1e-9) - { - slope = (end_[1] - start_[1]) / (end_[0] - start_[0]); - angle_ = pi_to_pi(atan(slope) + M_PI/2); - } - else - { - angle_ = 0.0; - } -} - -void Line::radiusFromEndpoints() -{ - radius_ = start_[0] * cos(angle_) + start_[1] * sin(angle_); - if (radius_ < 0) - { - radius_ = -radius_; - angle_ = pi_to_pi(angle_ + M_PI); - } -} - -/////////////////////////////////////////////////////////////////////////////// -// Methods for least squares line fitting -/////////////////////////////////////////////////////////////////////////////// -void Line::leastSqFit() -{ - calcPointCovariances(); - double prev_radius = 0.0; - double prev_angle = 0.0; - while (fabs(radius_ - prev_radius) > params_.least_sq_radius_thresh || - fabs(angle_ - prev_angle) > params_.least_sq_angle_thresh) - { - prev_radius = radius_; - prev_angle = angle_; - calcPointScalarCovariances(); - radiusFromLeastSq(); - angleFromLeastSq(); - } - calcCovariance(); - projectEndpoints(); -} - -void Line::angleFromLeastSq() -{ - calcPointParameters(); - angle_ += angleIncrement(); -} - -double Line::angleIncrement() -{ - const std::vector &a = p_params_.a; - const std::vector &ap = p_params_.ap; - const std::vector &app = p_params_.app; - const std::vector &b = p_params_.b; - const std::vector &bp = p_params_.bp; - const std::vector &bpp = p_params_.bpp; - const std::vector &c = p_params_.c; - const std::vector &s = p_params_.s; - - double numerator = 0; - double denominator = 0; - for (std::size_t i = 0; i < a.size(); ++i) - { - numerator += (b[i] * ap[i] - a[i] * bp[i]) / pow(b[i], 2); - denominator += ((app[i] * b[i] - a[i] * bpp[i]) * b[i] - - 2 * (ap[i] * b[i] - a[i] * bp[i]) * bp[i]) / pow(b[i], 3); - } - return -(numerator/denominator); -} - -void Line::calcCovariance() -{ - covariance_[0] = p_rr_; - - const std::vector &a = p_params_.a; - const std::vector &ap = p_params_.ap; - const std::vector &app = p_params_.app; - const std::vector &b = p_params_.b; - const std::vector &bp = p_params_.bp; - const std::vector &bpp = p_params_.bpp; - const std::vector &c = p_params_.c; - const std::vector &s = p_params_.s; - - double G = 0; - double A = 0; - double B = 0; - double r, phi; - for (std::size_t i = 0; i < a.size(); ++i) - { - r = r_data_.ranges[indices_[i]]; // range - phi = c_data_.bearings[indices_[i]]; // bearing - G += ((app[i] * b[i] - a[i] * bpp[i]) * b[i] - 2 * (ap[i] * b[i] - a[i] * bp[i]) * bp[i]) / pow(b[i], 3); - A += 2 * r * sin(angle_ - phi) / b[i]; - B += 4 * pow(r, 2) * pow(sin(angle_ - phi), 2) / b[i]; - } - covariance_[1] = p_rr_ * A / G; - covariance_[2] = covariance_[1]; - covariance_[3] = pow(1.0 / G, 2) * B; -} - -void Line::calcPointCovariances() -{ - point_covs_.clear(); - double r, phi, var_r, var_phi; - for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) - { - r = r_data_.ranges[*cit]; // range - phi = c_data_.bearings[*cit]; // bearing - var_r = params_.range_var; // range variance - var_phi = params_.bearing_var; // bearing variance - boost::array Q; - Q[0] = pow(r, 2) * var_phi * pow(sin(phi), 2) + var_r * pow(cos(phi), 2); - Q[1] = -pow(r, 2) * var_phi * sin(2 * phi) / 2.0 + var_r * sin(2 * phi) / 2.0; - Q[2] = Q[1]; - Q[3] = pow(r, 2) * var_phi * pow(cos(phi), 2) + var_r * pow(sin(phi), 2); - point_covs_.push_back(Q); - } -} - -void Line::calcPointParameters() -{ - p_params_.a.clear(); - p_params_.ap.clear(); - p_params_.app.clear(); - p_params_.b.clear(); - p_params_.bp.clear(); - p_params_.bpp.clear(); - p_params_.c.clear(); - p_params_.s.clear(); - - double r, phi, var_r, var_phi; - double a, ap, app, b, bp, bpp, c, s; - for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) - { - r = r_data_.ranges[*cit]; // range - phi = c_data_.bearings[*cit]; // bearing - var_r = params_.range_var; // range variance - var_phi = params_.bearing_var; // bearing variance - c = cos(angle_ - phi); - s = sin(angle_ - phi); - a = pow(r * c - radius_, 2); - ap = -2 * r * s * (r * c - radius_); - app = 2 * pow(r, 2) * pow(s, 2) - 2 * r * c * (r * c - radius_); - b = var_r * pow(c, 2) + var_phi * pow(r, 2) * pow(s, 2); - bp = 2 * (pow(r, 2) * var_phi - var_r) * c * s; - bpp = 2 * (pow(r, 2) * var_phi - var_r) * (pow(c, 2) - pow(s, 2)); - p_params_.a.push_back(a); - p_params_.ap.push_back(ap); - p_params_.app.push_back(app); - p_params_.b.push_back(b); - p_params_.bp.push_back(bp); - p_params_.bpp.push_back(bpp); - p_params_.c.push_back(c); - p_params_.s.push_back(s); - } -} - -void Line::calcPointScalarCovariances() -{ - point_scalar_vars_.clear(); - double P; - double inverse_P_sum = 0; - for (std::vector >::const_iterator cit = point_covs_.begin(); - cit != point_covs_.end(); ++cit) - { - P = (*cit)[0] * pow(cos(angle_), 2) + 2 * (*cit)[1] * sin(angle_) * cos(angle_) + - (*cit)[3] * pow(sin(angle_), 2); - inverse_P_sum += 1.0 / P; - point_scalar_vars_.push_back(P); - } - p_rr_ = 1.0 / inverse_P_sum; -} - -void Line::radiusFromLeastSq() -{ - radius_ = 0; - double r, phi; - for (std::vector::const_iterator cit = indices_.begin(); cit != indices_.end(); ++cit) - { - r = r_data_.ranges[*cit]; // range - phi = c_data_.bearings[*cit]; // bearing - radius_ += r * cos(angle_ - phi) / point_scalar_vars_[cit - indices_.begin()]; // cit to index - } - - radius_ *= p_rr_; -} - -} // namespace line_extraction diff --git a/src/line_extraction.cpp b/src/line_extraction.cpp deleted file mode 100644 index 00d42c0..0000000 --- a/src/line_extraction.cpp +++ /dev/null @@ -1,363 +0,0 @@ -#include "laser_line_extraction/line_extraction.h" -#include -#include -#include - -namespace line_extraction -{ - -/////////////////////////////////////////////////////////////////////////////// -// Constructor / destructor -/////////////////////////////////////////////////////////////////////////////// -LineExtraction::LineExtraction() -{ -} - -LineExtraction::~LineExtraction() -{ -} - -/////////////////////////////////////////////////////////////////////////////// -// Main run function -/////////////////////////////////////////////////////////////////////////////// -void LineExtraction::extractLines(std::vector& lines) -{ - // Resets - filtered_indices_ = c_data_.indices; - lines_.clear(); - - // Filter indices - filterCloseAndFarPoints(); - filterOutlierPoints(); - - // Return no lines if not enough points left - if (filtered_indices_.size() <= std::max(params_.min_line_points, static_cast(3))) - { - return; - } - - // Split indices into lines and filter out short and sparse lines - split(filtered_indices_); - filterLines(); - - // Fit each line using least squares and merge colinear lines - for (std::vector::iterator it = lines_.begin(); it != lines_.end(); ++it) - { - it->leastSqFit(); - } - - // If there is more than one line, check if lines should be merged based on the merging criteria - if (lines_.size() > 1) - { - mergeLines(); - } - - lines = lines_; -} - -/////////////////////////////////////////////////////////////////////////////// -// Data setting -/////////////////////////////////////////////////////////////////////////////// -void LineExtraction::setCachedData(const std::vector& bearings, - const std::vector& cos_bearings, - const std::vector& sin_bearings, - const std::vector& indices) -{ - c_data_.bearings = bearings; - c_data_.cos_bearings = cos_bearings; - c_data_.sin_bearings = sin_bearings; - c_data_.indices = indices; -} - -void LineExtraction::setRangeData(const std::vector& ranges) -{ - r_data_.ranges = ranges; - r_data_.xs.clear(); - r_data_.ys.clear(); - for (std::vector::const_iterator cit = c_data_.indices.begin(); - cit != c_data_.indices.end(); ++cit) - { - r_data_.xs.push_back(c_data_.cos_bearings[*cit] * ranges[*cit]); - r_data_.ys.push_back(c_data_.sin_bearings[*cit] * ranges[*cit]); - } -} - -/////////////////////////////////////////////////////////////////////////////// -// Parameter setting -/////////////////////////////////////////////////////////////////////////////// -void LineExtraction::setBearingVariance(double value) -{ - params_.bearing_var = value; -} - -void LineExtraction::setRangeVariance(double value) -{ - params_.range_var = value; -} - -void LineExtraction::setLeastSqAngleThresh(double value) -{ - params_.least_sq_angle_thresh = value; -} - -void LineExtraction::setLeastSqRadiusThresh(double value) -{ - params_.least_sq_radius_thresh = value; -} - -void LineExtraction::setMaxLineGap(double value) -{ - params_.max_line_gap = value; -} - -void LineExtraction::setMinLineLength(double value) -{ - params_.min_line_length = value; -} - -void LineExtraction::setMinLinePoints(unsigned int value) -{ - params_.min_line_points = value; -} - -void LineExtraction::setMinRange(double value) -{ - params_.min_range = value; -} - -void LineExtraction::setMaxRange(double value) -{ - params_.max_range = value; -} - -void LineExtraction::setMinSplitDist(double value) -{ - params_.min_split_dist = value; -} - -void LineExtraction::setOutlierDist(double value) -{ - params_.outlier_dist = value; -} - -/////////////////////////////////////////////////////////////////////////////// -// Utility methods -/////////////////////////////////////////////////////////////////////////////// -double LineExtraction::chiSquared(const Eigen::Vector2d &dL, const Eigen::Matrix2d &P_1, - const Eigen::Matrix2d &P_2) -{ - return dL.transpose() * (P_1 + P_2).inverse() * dL; -} - -double LineExtraction::distBetweenPoints(unsigned int index_1, unsigned int index_2) -{ - return sqrt(pow(r_data_.xs[index_1] - r_data_.xs[index_2], 2) + - pow(r_data_.ys[index_1] - r_data_.ys[index_2], 2)); -} - -/////////////////////////////////////////////////////////////////////////////// -// Filtering points -/////////////////////////////////////////////////////////////////////////////// -void LineExtraction::filterCloseAndFarPoints() -{ - std::vector output; - for (std::vector::const_iterator cit = filtered_indices_.begin(); - cit != filtered_indices_.end(); ++cit) - { - const double& range = r_data_.ranges[*cit]; - if (range >= params_.min_range && range <= params_.max_range) - { - output.push_back(*cit); - } - } - filtered_indices_ = output; -} - -void LineExtraction::filterOutlierPoints() -{ - if (filtered_indices_.size() < 3) - { - return; - } - - std::vector output; - unsigned int p_i, p_j, p_k; - for (std::size_t i = 0; i < filtered_indices_.size(); ++i) - { - - // Get two closest neighbours - - p_i = filtered_indices_[i]; - if (i == 0) // first point - { - p_j = filtered_indices_[i + 1]; - p_k = filtered_indices_[i + 2]; - } - else if (i == filtered_indices_.size() - 1) // last point - { - p_j = filtered_indices_[i - 1]; - p_k = filtered_indices_[i - 2]; - } - else // middle points - { - p_j = filtered_indices_[i - 1]; - p_k = filtered_indices_[i + 1]; - } - - // Check if point is an outlier - - if (fabs(r_data_.ranges[p_i] - r_data_.ranges[p_j]) > params_.outlier_dist && - fabs(r_data_.ranges[p_i] - r_data_.ranges[p_k]) > params_.outlier_dist) - { - // Check if it is close to line connecting its neighbours - std::vector line_indices; - line_indices.push_back(p_j); - line_indices.push_back(p_k); - Line line(c_data_, r_data_, params_, line_indices); - line.endpointFit(); - if (line.distToPoint(p_i) > params_.min_split_dist) - { - continue; // point is an outlier - } - } - - output.push_back(p_i); - } - - filtered_indices_ = output; -} - -/////////////////////////////////////////////////////////////////////////////// -// Filtering and merging lines -/////////////////////////////////////////////////////////////////////////////// -void LineExtraction::filterLines() -{ - std::vector output; - for (std::vector::const_iterator cit = lines_.begin(); cit != lines_.end(); ++cit) - { - if (cit->length() >= params_.min_line_length && cit->numPoints() >= params_.min_line_points) - { - output.push_back(*cit); - } - } - lines_ = output; -} - -void LineExtraction::mergeLines() -{ - std::vector merged_lines; - - for (std::size_t i = 1; i < lines_.size(); ++i) - { - // Get L, P_1, P_2 of consecutive lines - Eigen::Vector2d L_1(lines_[i-1].getRadius(), lines_[i-1].getAngle()); - Eigen::Vector2d L_2(lines_[i].getRadius(), lines_[i].getAngle()); - Eigen::Matrix2d P_1; - P_1 << lines_[i-1].getCovariance()[0], lines_[i-1].getCovariance()[1], - lines_[i-1].getCovariance()[2], lines_[i-1].getCovariance()[3]; - Eigen::Matrix2d P_2; - P_2 << lines_[i].getCovariance()[0], lines_[i].getCovariance()[1], - lines_[i].getCovariance()[2], lines_[i].getCovariance()[3]; - - // Merge lines if chi-squared distance is less than 3 - if (chiSquared(L_1 - L_2, P_1, P_2) < 3) - { - // Get merged angle, radius, and covariance - Eigen::Matrix2d P_m = (P_1.inverse() + P_2.inverse()).inverse(); - Eigen::Vector2d L_m = P_m * (P_1.inverse() * L_1 + P_2.inverse() * L_2); - // Populate new line with these merged parameters - boost::array cov; - cov[0] = P_m(0,0); - cov[1] = P_m(0,1); - cov[2] = P_m(1,0); - cov[3] = P_m(1,1); - std::vector indices; - const std::vector &ind_1 = lines_[i-1].getIndices(); - const std::vector &ind_2 = lines_[i].getIndices(); - indices.resize(ind_1.size() + ind_2.size()); - indices.insert(indices.end(), ind_1.begin(), ind_1.end()); - indices.insert(indices.end(), ind_2.begin(), ind_2.end()); - Line merged_line(L_m[1], L_m[0], cov, lines_[i-1].getStart(), lines_[i].getEnd(), indices); - // Project the new endpoints - merged_line.projectEndpoints(); - lines_[i] = merged_line; - } - else - { - merged_lines.push_back(lines_[i-1]); - } - - if (i == lines_.size() - 1) - { - merged_lines.push_back(lines_[i]); - } - } - lines_ = merged_lines; -} - -/////////////////////////////////////////////////////////////////////////////// -// Splitting points into lines -/////////////////////////////////////////////////////////////////////////////// -void LineExtraction::split(const std::vector& indices) -{ - // Don't split if only a single point (only occurs when orphaned by gap) - if (indices.size() <= 1) - { - return; - } - - Line line(c_data_, r_data_, params_, indices); - line.endpointFit(); - double dist_max = 0; - double gap_max = 0; - double dist, gap; - int i_max, i_gap; - - // Find the farthest point and largest gap - for (std::size_t i = 1; i < indices.size() - 1; ++i) - { - dist = line.distToPoint(indices[i]); - if (dist > dist_max) - { - dist_max = dist; - i_max = i; - } - gap = distBetweenPoints(indices[i], indices[i+1]); - if (gap > gap_max) - { - gap_max = gap; - i_gap = i; - } - } - - // Check for gaps at endpoints - double gap_start = distBetweenPoints(indices[0], indices[1]); - if (gap_start > gap_max) - { - gap_max = gap_start; - i_gap = 1; - } - double gap_end = distBetweenPoints(indices.rbegin()[1], indices.rbegin()[0]); - if (gap_end > gap_max) - { - gap_max = gap_end; - i_gap = indices.size() - 1; - } - - // Check if line meets requirements or should be split - if (dist_max < params_.min_split_dist && gap_max < params_.max_line_gap) - { - lines_.push_back(line); - } - else - { - int i_split = dist_max >= params_.min_split_dist ? i_max : i_gap; - std::vector first_split(&indices[0], &indices[i_split - 1]); - std::vector second_split(&indices[i_split], &indices.back()); - split(first_split); - split(second_split); - } - -} - -} // namespace line_extraction diff --git a/src/line_extraction_node.cpp b/src/line_extraction_node.cpp deleted file mode 100644 index 0b717b5..0000000 --- a/src/line_extraction_node.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "laser_line_extraction/line_extraction_ros.h" -#include - -int main(int argc, char **argv) -{ - - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) - { - ros::console::notifyLoggerLevelsChanged(); - } - - ROS_DEBUG("Starting line_extraction_node."); - - ros::init(argc, argv, "line_extraction_node"); - ros::NodeHandle nh; - ros::NodeHandle nh_local("~"); - line_extraction::LineExtractionROS line_extractor(nh, nh_local); - - double frequency; - nh_local.param("frequency", frequency, 25); - ROS_DEBUG("Frequency set to %0.1f Hz", frequency); - ros::Rate rate(frequency); - - while (ros::ok()) - { - line_extractor.run(); - ros::spinOnce(); - rate.sleep(); - } - return 0; -} - diff --git a/src/line_extraction_ros.cpp b/src/line_extraction_ros.cpp deleted file mode 100644 index 221efa5..0000000 --- a/src/line_extraction_ros.cpp +++ /dev/null @@ -1,220 +0,0 @@ -#include "laser_line_extraction/line_extraction_ros.h" -#include -#include - - -namespace line_extraction -{ - -/////////////////////////////////////////////////////////////////////////////// -// Constructor / destructor -/////////////////////////////////////////////////////////////////////////////// -LineExtractionROS::LineExtractionROS(ros::NodeHandle& nh, ros::NodeHandle& nh_local): - nh_(nh), - nh_local_(nh_local), - data_cached_(false) -{ - loadParameters(); - line_publisher_ = nh_.advertise("line_segments", 1); - scan_subscriber_ = nh_.subscribe(scan_topic_, 1, &LineExtractionROS::laserScanCallback, this); - if (pub_markers_) - { - marker_publisher_ = nh_.advertise("line_markers", 1); - } -} - -LineExtractionROS::~LineExtractionROS() -{ -} - -/////////////////////////////////////////////////////////////////////////////// -// Run -/////////////////////////////////////////////////////////////////////////////// -void LineExtractionROS::run() -{ - // Extract the lines - std::vector lines; - line_extraction_.extractLines(lines); - - // Populate message - laser_line_extraction::LineSegmentList msg; - populateLineSegListMsg(lines, msg); - - // Publish the lines - line_publisher_.publish(msg); - - // Also publish markers if parameter publish_markers is set to true - if (pub_markers_) - { - visualization_msgs::Marker marker_msg; - populateMarkerMsg(lines, marker_msg); - marker_publisher_.publish(marker_msg); - } -} - -/////////////////////////////////////////////////////////////////////////////// -// Load ROS parameters -/////////////////////////////////////////////////////////////////////////////// -void LineExtractionROS::loadParameters() -{ - - ROS_DEBUG("*************************************"); - ROS_DEBUG("PARAMETERS:"); - - // Parameters used by this node - - std::string frame_id, scan_topic; - bool pub_markers; - - nh_local_.param("frame_id", frame_id, "laser"); - frame_id_ = frame_id; - ROS_DEBUG("frame_id: %s", frame_id_.c_str()); - - nh_local_.param("scan_topic", scan_topic, "scan"); - scan_topic_ = scan_topic; - ROS_DEBUG("scan_topic: %s", scan_topic_.c_str()); - - nh_local_.param("publish_markers", pub_markers, false); - pub_markers_ = pub_markers; - ROS_DEBUG("publish_markers: %s", pub_markers ? "true" : "false"); - - // Parameters used by the line extraction algorithm - - double bearing_std_dev, range_std_dev, least_sq_angle_thresh, least_sq_radius_thresh, - max_line_gap, min_line_length, min_range, max_range, min_split_dist, outlier_dist; - int min_line_points; - - nh_local_.param("bearing_std_dev", bearing_std_dev, 1e-3); - line_extraction_.setBearingVariance(bearing_std_dev * bearing_std_dev); - ROS_DEBUG("bearing_std_dev: %f", bearing_std_dev); - - nh_local_.param("range_std_dev", range_std_dev, 0.02); - line_extraction_.setRangeVariance(range_std_dev * range_std_dev); - ROS_DEBUG("range_std_dev: %f", range_std_dev); - - nh_local_.param("least_sq_angle_thresh", least_sq_angle_thresh, 1e-4); - line_extraction_.setLeastSqAngleThresh(least_sq_angle_thresh); - ROS_DEBUG("least_sq_angle_thresh: %f", least_sq_angle_thresh); - - nh_local_.param("least_sq_radius_thresh", least_sq_radius_thresh, 1e-4); - line_extraction_.setLeastSqRadiusThresh(least_sq_radius_thresh); - ROS_DEBUG("least_sq_radius_thresh: %f", least_sq_radius_thresh); - - nh_local_.param("max_line_gap", max_line_gap, 0.4); - line_extraction_.setMaxLineGap(max_line_gap); - ROS_DEBUG("max_line_gap: %f", max_line_gap); - - nh_local_.param("min_line_length", min_line_length, 0.5); - line_extraction_.setMinLineLength(min_line_length); - ROS_DEBUG("min_line_length: %f", min_line_length); - - nh_local_.param("min_range", min_range, 0.4); - line_extraction_.setMinRange(min_range); - ROS_DEBUG("min_range: %f", min_range); - - nh_local_.param("max_range", max_range, 10000.0); - line_extraction_.setMaxRange(max_range); - ROS_DEBUG("max_range: %f", max_range); - - nh_local_.param("min_split_dist", min_split_dist, 0.05); - line_extraction_.setMinSplitDist(min_split_dist); - ROS_DEBUG("min_split_dist: %f", min_split_dist); - - nh_local_.param("outlier_dist", outlier_dist, 0.05); - line_extraction_.setOutlierDist(outlier_dist); - ROS_DEBUG("outlier_dist: %f", outlier_dist); - - nh_local_.param("min_line_points", min_line_points, 9); - line_extraction_.setMinLinePoints(static_cast(min_line_points)); - ROS_DEBUG("min_line_points: %d", min_line_points); - - ROS_DEBUG("*************************************"); -} - -/////////////////////////////////////////////////////////////////////////////// -// Populate messages -/////////////////////////////////////////////////////////////////////////////// -void LineExtractionROS::populateLineSegListMsg(const std::vector &lines, - laser_line_extraction::LineSegmentList &line_list_msg) -{ - for (std::vector::const_iterator cit = lines.begin(); cit != lines.end(); ++cit) - { - laser_line_extraction::LineSegment line_msg; - line_msg.angle = cit->getAngle(); - line_msg.radius = cit->getRadius(); - line_msg.covariance = cit->getCovariance(); - line_msg.start = cit->getStart(); - line_msg.end = cit->getEnd(); - line_list_msg.line_segments.push_back(line_msg); - } - line_list_msg.header.frame_id = frame_id_; - line_list_msg.header.stamp = ros::Time::now(); -} - -void LineExtractionROS::populateMarkerMsg(const std::vector &lines, - visualization_msgs::Marker &marker_msg) -{ - marker_msg.ns = "line_extraction"; - marker_msg.id = 0; - marker_msg.type = visualization_msgs::Marker::LINE_LIST; - marker_msg.scale.x = 0.1; - marker_msg.color.r = 1.0; - marker_msg.color.g = 0.0; - marker_msg.color.b = 0.0; - marker_msg.color.a = 1.0; - for (std::vector::const_iterator cit = lines.begin(); cit != lines.end(); ++cit) - { - geometry_msgs::Point p_start; - p_start.x = cit->getStart()[0]; - p_start.y = cit->getStart()[1]; - p_start.z = 0; - marker_msg.points.push_back(p_start); - geometry_msgs::Point p_end; - p_end.x = cit->getEnd()[0]; - p_end.y = cit->getEnd()[1]; - p_end.z = 0; - marker_msg.points.push_back(p_end); - } - marker_msg.header.frame_id = frame_id_; - marker_msg.header.stamp = ros::Time::now(); -} - -/////////////////////////////////////////////////////////////////////////////// -// Cache data on first LaserScan message received -/////////////////////////////////////////////////////////////////////////////// -void LineExtractionROS::cacheData(const sensor_msgs::LaserScan::ConstPtr &scan_msg) -{ - std::vector bearings, cos_bearings, sin_bearings; - std::vector indices; - const std::size_t num_measurements = std::ceil( - (scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment); - for (std::size_t i = 0; i < num_measurements; ++i) - { - const double b = scan_msg->angle_min + i * scan_msg->angle_increment; - bearings.push_back(b); - cos_bearings.push_back(cos(b)); - sin_bearings.push_back(sin(b)); - indices.push_back(i); - } - - line_extraction_.setCachedData(bearings, cos_bearings, sin_bearings, indices); - ROS_DEBUG("Data has been cached."); -} - -/////////////////////////////////////////////////////////////////////////////// -// Main LaserScan callback -/////////////////////////////////////////////////////////////////////////////// -void LineExtractionROS::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg) -{ - if (!data_cached_) - { - cacheData(scan_msg); - data_cached_ = true; - } - - std::vector scan_ranges_doubles(scan_msg->ranges.begin(), scan_msg->ranges.end()); - line_extraction_.setRangeData(scan_ranges_doubles); -} - -} // namespace line_extraction -