├── CMakeLists.txt ├── README.md ├── package.xml ├── include └── ros2_laser_scan_matcher │ └── laser_scan_matcher.h └── src └── laser_scan_matcher.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ros2_laser_scan_matcher) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(sensor_msgs REQUIRED) 7 | find_package(rclcpp REQUIRED) 8 | find_package(nav_msgs REQUIRED) 9 | find_package(tf2_ros REQUIRED) 10 | find_package(tf2 REQUIRED) 11 | find_package(tf2_geometry_msgs REQUIRED) 12 | find_package(rcutils REQUIRED) 13 | find_package(PkgConfig) 14 | find_package(csm REQUIRED) 15 | find_package(Eigen3 REQUIRED) 16 | 17 | find_package(Boost COMPONENTS thread REQUIRED) 18 | 19 | include_directories( 20 | include 21 | ${EIGEN3_INCLUDE_DIRS} 22 | ) 23 | 24 | 25 | add_executable(laser_scan_matcher src/laser_scan_matcher.cpp) 26 | 27 | target_include_directories(laser_scan_matcher 28 | PRIVATE 29 | "include" 30 | ) 31 | 32 | ament_target_dependencies(laser_scan_matcher 33 | rclcpp 34 | sensor_msgs 35 | rcutils 36 | Eigen3 37 | tf2_ros 38 | tf2 39 | tf2_geometry_msgs 40 | csm 41 | Boost 42 | nav_msgs 43 | ) 44 | 45 | install(TARGETS laser_scan_matcher DESTINATION lib/${PROJECT_NAME}) 46 | 47 | 48 | ament_package() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Laser Scan Matcher for ROS2 2 | Ported to ros2 version of laser-scan-matcher by [scan_tools](https://github.com/ccny-ros-pkg/scan_tools). 3 | 4 | 5 | ## Installation 6 | * Install modified version of [csmlib](https://github.com/AlexKaravaev/csm) 7 | 8 | ## Topics 9 | 10 | ### Subscribed topics 11 | - `/scan` ([sensor_msgs/LaserScan](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/LaserScan.html)) 12 | - `/tf` ([tf2_msgs/TFMessage](http://docs.ros.org/melodic/api/tf2_msgs/html/msg/TFMessage.html)) 13 | ### Published topics 14 | - `/tf` ([tf2_msgs/TFMessage](http://docs.ros.org/melodic/api/tf2_msgs/html/msg/TFMessage.html)) with transform odom->base_link 15 | - `/odom` ([nav_msgs/Odometry](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg)) Optional. Parameter publish_odom must be set to the name of the topic. If topic is empty, odom will not be published. 16 | 17 | ## Will be released features: 18 | - [x] Support of pure laserscan 19 | - [ ] Support of IMU 20 | - [ ] Support of odometry 21 | - [ ] Support of PointCloud msgs 22 | 23 | ## Example usage: 24 | `ros2 run ros2_laser_scan_matcher laser_scan_matcher --ros-args -p publish_odom:=/odom -p publish_tf:=true` 25 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros2_laser_scan_matcher 5 | 0.0.0 6 | TODO: Package description 7 | alexander.karavaev 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | nav2_common 13 | sensor_msgs 14 | csm 15 | csm 16 | rclcpp 17 | tf2_geometry_msgs 18 | geometry_msgs 19 | message_filters 20 | nav_msgs 21 | sensor_msgs 22 | std_srvs 23 | tf2_ros 24 | tf2 25 | nav2_util 26 | nav2_msgs 27 | launch_ros 28 | launch_testing 29 | common_interfaces 30 | 31 | ament_lint_auto 32 | ament_lint_common 33 | 34 | 35 | ament_cmake 36 | 37 | 38 | -------------------------------------------------------------------------------- /include/ros2_laser_scan_matcher/laser_scan_matcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Ivan Dryanovski, William Morris 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the CCNY Robotics Lab nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* This package uses Canonical Scan Matcher [1], written by 31 | * Andrea Censi 32 | * 33 | * [1] A. Censi, "An ICP variant using a point-to-line metric" 34 | * Proceedings of the IEEE International Conference 35 | * on Robotics and Automation (ICRA), 2008 36 | */ 37 | 38 | #ifndef LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H 39 | #define LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include "rclcpp/rclcpp.hpp" 51 | 52 | #include // csm defines min and max, but Eigen complains 53 | #include 54 | 55 | 56 | namespace scan_tools 57 | { 58 | class LaserScanMatcher: public rclcpp::Node 59 | { 60 | public: 61 | LaserScanMatcher(); 62 | ~LaserScanMatcher(); 63 | 64 | void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg); 65 | 66 | private: 67 | // Ros handle 68 | 69 | rclcpp::Subscription::SharedPtr scan_filter_sub_; 70 | 71 | std::shared_ptr tf_; 72 | std::shared_ptr tfB_; 73 | tf2::Transform base_to_laser_; // static, cached 74 | tf2::Transform laser_to_base_; 75 | std::shared_ptr tf_buffer_; 76 | rclcpp::Publisher::SharedPtr odom_publisher_; 77 | // Coordinate parameters 78 | std::string map_frame_; 79 | std::string base_frame_; 80 | std::string odom_frame_; 81 | std::string laser_frame_; 82 | std::string odom_topic_; 83 | 84 | // Keyframe parameters 85 | double kf_dist_linear_; 86 | double kf_dist_linear_sq_; 87 | double kf_dist_angular_; 88 | 89 | bool initialized_; 90 | bool publish_odom_; 91 | bool publish_tf_; 92 | 93 | tf2::Transform f2b_; // fixed-to-base tf (pose of base frame in fixed frame) 94 | tf2::Transform prev_f2b_; // previous fixed-to-base tf (for odometry calculation) 95 | tf2::Transform f2b_kf_; // pose of the last keyframe scan in fixed frame 96 | 97 | 98 | tf2::Transform odom_to_base_tf; 99 | 100 | sm_params input_; 101 | sm_result output_; 102 | LDP prev_ldp_scan_; 103 | 104 | // Grid map parameters 105 | double resolution_; 106 | 107 | std::vector a_cos_; 108 | std::vector a_sin_; 109 | 110 | 111 | rclcpp::Time last_icp_time_; 112 | 113 | bool getBaseToLaserTf (const std::string& frame_id); 114 | 115 | bool processScan(LDP& curr_ldp_scan, const rclcpp::Time& time); 116 | void laserScanToLDP(const sensor_msgs::msg::LaserScan::SharedPtr& scan, LDP& ldp); 117 | void createTfFromXYTheta(double x, double y, double theta, tf2::Transform& t); 118 | 119 | bool newKeyframeNeeded(const tf2::Transform& d); 120 | 121 | void add_parameter( 122 | const std::string & name, const rclcpp::ParameterValue & default_value, 123 | const std::string & description = "", const std::string & additional_constraints = "", 124 | bool read_only = false); 125 | void createCache (const sensor_msgs::msg::LaserScan::SharedPtr& scan_msg); 126 | 127 | 128 | }; // LaserScanMatcher 129 | 130 | } // namespace scan_tools 131 | 132 | #endif // LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H 133 | -------------------------------------------------------------------------------- /src/laser_scan_matcher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Ivan Dryanovski, William Morris 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the CCNY Robotics Lab nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* This package uses Canonical Scan Matcher [1], written by 31 | * Andrea Censi 32 | * 33 | * [1] A. Censi, "An ICP variant using a point-to-line metric" 34 | * Proceedings of the IEEE International Conference 35 | * on Robotics and Automation (ICRA), 2008 36 | */ 37 | 38 | #include "ros2_laser_scan_matcher/laser_scan_matcher.h" 39 | 40 | #undef min 41 | #undef max 42 | 43 | namespace scan_tools 44 | { 45 | 46 | void LaserScanMatcher::add_parameter( 47 | const std::string & name, const rclcpp::ParameterValue & default_value, 48 | const std::string & description, const std::string & additional_constraints, 49 | bool read_only) 50 | { 51 | auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); 52 | 53 | descriptor.name = name; 54 | descriptor.description = description; 55 | descriptor.additional_constraints = additional_constraints; 56 | descriptor.read_only = read_only; 57 | 58 | declare_parameter(descriptor.name, default_value, descriptor); 59 | } 60 | 61 | 62 | LaserScanMatcher::LaserScanMatcher() : Node("laser_scan_matcher"), initialized_(false) 63 | { 64 | tf_buffer_ = std::make_shared(get_clock()); 65 | // Initiate parameters 66 | 67 | RCLCPP_INFO(get_logger(), "Creating laser_scan_matcher"); 68 | add_parameter("publish_odom", rclcpp::ParameterValue(std::string("")), 69 | "If publish odometry from laser_scan. Empty if not, otherwise name of the topic"); 70 | add_parameter("publish_tf", rclcpp::ParameterValue(false), 71 | " If publish tf odom->base_link"); 72 | 73 | add_parameter("base_frame", rclcpp::ParameterValue(std::string("base_link")), 74 | "Which frame to use for the robot base"); 75 | add_parameter("odom_frame", rclcpp::ParameterValue(std::string("odom")), 76 | "Which frame to use for the odom"); 77 | add_parameter("map_frame", rclcpp::ParameterValue(std::string("map")), 78 | "Which frame to use for the map"); 79 | add_parameter("laser_frame", rclcpp::ParameterValue(std::string("laser")), 80 | "Which frame to use for the laser"); 81 | add_parameter("kf_dist_linear", rclcpp::ParameterValue(0.10), 82 | "When to generate keyframe scan."); 83 | add_parameter("kf_dist_angular", rclcpp::ParameterValue(10.0* (M_PI/180.0)), 84 | "When to generate keyframe scan."); 85 | 86 | 87 | 88 | // CSM parameters - comments copied from algos.h (by Andrea Censi) 89 | add_parameter("max_angular_correction_deg", rclcpp::ParameterValue(45.0), 90 | "Maximum angular displacement between scansr."); 91 | 92 | add_parameter("max_linear_correction", rclcpp::ParameterValue(0.5), 93 | "Maximum translation between scans (m)."); 94 | 95 | add_parameter("max_iterations", rclcpp::ParameterValue(10), 96 | "Maximum ICP cycle iterationsr."); 97 | 98 | add_parameter("epsilon_xy", rclcpp::ParameterValue(0.000001), 99 | "A threshold for stopping (m)."); 100 | 101 | add_parameter("epsilon_theta", rclcpp::ParameterValue(0.000001), 102 | "A threshold for stopping (rad)."); 103 | 104 | add_parameter("max_correspondence_dist", rclcpp::ParameterValue(0.3), 105 | "Maximum distance for a correspondence to be valid."); 106 | 107 | add_parameter("sigma", rclcpp::ParameterValue(0.010), 108 | "Noise in the scan (m)."); 109 | 110 | add_parameter("use_corr_tricks", rclcpp::ParameterValue(1), 111 | "Use smart tricks for finding correspondences."); 112 | 113 | add_parameter("restart", rclcpp::ParameterValue(0), 114 | "Restart if error is over threshold."); 115 | 116 | add_parameter("restart_threshold_mean_error", rclcpp::ParameterValue(0.01), 117 | "Threshold for restarting."); 118 | 119 | add_parameter("restart_dt", rclcpp::ParameterValue(1.0), 120 | "Displacement for restarting. (m)."); 121 | 122 | add_parameter("restart_dtheta", rclcpp::ParameterValue(0.1), 123 | "Displacement for restarting. (rad)."); 124 | 125 | add_parameter("clustering_threshold", rclcpp::ParameterValue(0.25), 126 | "Max distance for staying in the same clustering."); 127 | 128 | add_parameter("orientation_neighbourhood", rclcpp::ParameterValue(20), 129 | "Number of neighbour rays used to estimate the orientation."); 130 | 131 | add_parameter("use_point_to_line_distance", rclcpp::ParameterValue(1), 132 | "If 0, it's vanilla ICP."); 133 | 134 | add_parameter("do_alpha_test", rclcpp::ParameterValue(0), 135 | " Discard correspondences based on the angles."); 136 | 137 | add_parameter("do_alpha_test_thresholdDeg", rclcpp::ParameterValue(20.0), 138 | "Discard correspondences based on the angles - threshold angle, in degrees."); 139 | 140 | add_parameter("outliers_maxPerc", rclcpp::ParameterValue(0.9), 141 | "Percentage of correspondences to consider: if 0.9, \ 142 | always discard the top 10% of correspondences with more error"); 143 | 144 | 145 | // Parameters describing a simple adaptive algorithm for discarding. 146 | // 1) Order the errors. 147 | // 2) Choose the percentile according to outliers_adaptive_order. 148 | // (if it is 0.7, get the 70% percentile) 149 | // 3) Define an adaptive threshold multiplying outliers_adaptive_mult 150 | // with the value of the error at the chosen percentile. 151 | // 4) Discard correspondences over the threshold. 152 | // This is useful to be conservative; yet remove the biggest errors. 153 | add_parameter("outliers_adaptive_order", rclcpp::ParameterValue(0.7), 154 | ""); 155 | 156 | add_parameter("outliers_adaptive_mult", rclcpp::ParameterValue(2.0), 157 | ""); 158 | 159 | // If you already have a guess of the solution, you can compute the polar 160 | // angle 161 | // of the points of one scan in the new position. If the polar angle is not a 162 | // monotone 163 | // function of the readings index, it means that the surface is not visible in 164 | // the 165 | // next position. If it is not visible, then we don't use it for matching. 166 | add_parameter("do_visibility_test", rclcpp::ParameterValue(0), 167 | ""); 168 | 169 | add_parameter("outliers_remove_doubles", rclcpp::ParameterValue(1), 170 | "No two points in laser_sens can have the same corr."); 171 | 172 | add_parameter("do_compute_covariance", rclcpp::ParameterValue(0), 173 | "If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov"); 174 | 175 | add_parameter("debug_verify_tricks", rclcpp::ParameterValue(0), 176 | " Checks that find_correspondences_tricks gives the right answer."); 177 | 178 | add_parameter("use_ml_weights", rclcpp::ParameterValue(0), 179 | "If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to \ 180 | compute the incidence beta, and the factor (1/cos^2(beta)) used to weight the \ 181 | correspondence."); 182 | 183 | add_parameter("use_sigma_weights", rclcpp::ParameterValue(0), 184 | " If 1, the field 'readings_sigma' in the second scan is used to weight the correspondence by 1/sigma^2"); 185 | 186 | 187 | map_frame_ = this->get_parameter("map_frame").as_string(); 188 | base_frame_ = this->get_parameter("base_frame").as_string(); 189 | odom_frame_ = this->get_parameter("odom_frame").as_string(); 190 | laser_frame_ = this->get_parameter("laser_frame").as_string(); 191 | kf_dist_linear_ = this->get_parameter("kf_dist_linear").as_double(); 192 | kf_dist_angular_ = this->get_parameter("kf_dist_angular").as_double(); 193 | odom_topic_ = this->get_parameter("publish_odom").as_string(); 194 | publish_tf_ = this->get_parameter("publish_tf").as_bool(); 195 | 196 | publish_odom_ = (odom_topic_ != ""); 197 | kf_dist_linear_sq_ = kf_dist_linear_ * kf_dist_linear_; 198 | 199 | input_.max_angular_correction_deg = this->get_parameter("max_angular_correction_deg").as_double(); 200 | input_.max_linear_correction = this->get_parameter("max_linear_correction").as_double(); 201 | input_.max_iterations = this->get_parameter("max_iterations").as_int(); 202 | input_.epsilon_xy = this->get_parameter("epsilon_xy").as_double(); 203 | input_.epsilon_theta = this->get_parameter("epsilon_theta").as_double(); 204 | input_.max_correspondence_dist = this->get_parameter("max_correspondence_dist").as_double(); 205 | input_.sigma = this->get_parameter("sigma").as_double(); 206 | input_.use_corr_tricks = this->get_parameter("use_corr_tricks").as_int(); 207 | input_.restart = this->get_parameter("restart").as_int(); 208 | input_.restart_threshold_mean_error = this->get_parameter("restart_threshold_mean_error").as_double(); 209 | input_.restart_dt = this->get_parameter("restart_dt").as_double(); 210 | input_.restart_dtheta = this->get_parameter("restart_dtheta").as_double(); 211 | input_.clustering_threshold = this->get_parameter("clustering_threshold").as_double(); 212 | input_.orientation_neighbourhood = this->get_parameter("orientation_neighbourhood").as_int(); 213 | input_.use_point_to_line_distance = this->get_parameter("use_point_to_line_distance").as_int(); 214 | input_.do_alpha_test = this->get_parameter("do_alpha_test").as_int(); 215 | input_.do_alpha_test_thresholdDeg = this->get_parameter("do_alpha_test_thresholdDeg").as_double(); 216 | input_.outliers_maxPerc = this->get_parameter("outliers_maxPerc").as_double(); 217 | input_.outliers_adaptive_order = this->get_parameter("outliers_adaptive_order").as_double(); 218 | input_.outliers_adaptive_mult = this->get_parameter("outliers_adaptive_mult").as_double(); 219 | input_.do_visibility_test = this->get_parameter("do_visibility_test").as_int(); 220 | input_.outliers_remove_doubles = this->get_parameter("outliers_remove_doubles").as_int(); 221 | input_.do_compute_covariance = this->get_parameter("do_compute_covariance").as_int(); 222 | input_.debug_verify_tricks = this->get_parameter("debug_verify_tricks").as_int(); 223 | input_.use_ml_weights = this->get_parameter("use_ml_weights").as_int(); 224 | input_.use_sigma_weights = this->get_parameter("use_sigma_weights").as_int(); 225 | 226 | 227 | double transform_publish_period; 228 | double tmp; 229 | 230 | // State variables 231 | 232 | f2b_.setIdentity(); 233 | prev_f2b_.setIdentity(); 234 | f2b_kf_.setIdentity(); 235 | input_.laser[0] = 0.0; 236 | input_.laser[1] = 0.0; 237 | input_.laser[2] = 0.0; 238 | 239 | // Initialize output_ vectors as Null for error-checking 240 | output_.cov_x_m = 0; 241 | output_.dx_dy1_m = 0; 242 | output_.dx_dy2_m = 0; 243 | 244 | 245 | // Subscribers 246 | this->scan_filter_sub_ = this->create_subscription("scan", rclcpp::SensorDataQoS(), std::bind(&LaserScanMatcher::scanCallback, this, std::placeholders::_1)); 247 | tf_ = std::make_shared(*tf_buffer_); 248 | if (publish_tf_) 249 | tfB_ = std::make_shared(*this); 250 | if(publish_odom_){ 251 | odom_publisher_ = this->create_publisher(odom_topic_, rclcpp::SystemDefaultsQoS()); 252 | } 253 | } 254 | 255 | LaserScanMatcher::~LaserScanMatcher() 256 | { 257 | 258 | } 259 | 260 | 261 | 262 | void LaserScanMatcher::createCache (const sensor_msgs::msg::LaserScan::SharedPtr& scan_msg) 263 | { 264 | a_cos_.clear(); 265 | a_sin_.clear(); 266 | 267 | for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i) 268 | { 269 | double angle = scan_msg->angle_min + i * scan_msg->angle_increment; 270 | a_cos_.push_back(cos(angle)); 271 | a_sin_.push_back(sin(angle)); 272 | } 273 | 274 | input_.min_reading = scan_msg->range_min; 275 | input_.max_reading = scan_msg->range_max; 276 | } 277 | 278 | 279 | void LaserScanMatcher::scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg) 280 | 281 | { 282 | 283 | if (!initialized_) 284 | { 285 | createCache(scan_msg); // caches the sin and cos of all angles 286 | 287 | // cache the static tf from base to laser 288 | if (!getBaseToLaserTf(laser_frame_)) 289 | { 290 | RCLCPP_WARN(get_logger(),"Skipping scan"); 291 | return; 292 | } 293 | 294 | laserScanToLDP(scan_msg, prev_ldp_scan_); 295 | last_icp_time_ = scan_msg->header.stamp; 296 | initialized_ = true; 297 | } 298 | 299 | LDP curr_ldp_scan; 300 | laserScanToLDP(scan_msg, curr_ldp_scan); 301 | processScan(curr_ldp_scan, scan_msg->header.stamp); 302 | } 303 | 304 | bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id) 305 | { 306 | rclcpp::Time t = now(); 307 | 308 | tf2::Stamped base_to_laser_tf; 309 | geometry_msgs::msg::TransformStamped laser_pose_msg; 310 | try 311 | { 312 | laser_pose_msg = tf_buffer_->lookupTransform(base_frame_, frame_id, t,rclcpp::Duration(10,0)); 313 | base_to_laser_tf.setOrigin(tf2::Vector3(laser_pose_msg.transform.translation.x,\ 314 | laser_pose_msg.transform.translation.y,\ 315 | laser_pose_msg.transform.translation.z)); 316 | tf2::Quaternion q(laser_pose_msg.transform.rotation.x,\ 317 | laser_pose_msg.transform.rotation.y,\ 318 | laser_pose_msg.transform.rotation.z,\ 319 | laser_pose_msg.transform.rotation.w); 320 | base_to_laser_tf.setRotation(q); 321 | 322 | } 323 | catch (tf2::TransformException ex) 324 | { 325 | RCLCPP_INFO(get_logger(),"Could not get initial transform from base to laser frame, %s", ex.what()); 326 | return false; 327 | } 328 | 329 | base_to_laser_ = base_to_laser_tf; 330 | laser_to_base_ = base_to_laser_.inverse(); 331 | 332 | return true; 333 | } 334 | 335 | 336 | bool LaserScanMatcher::processScan(LDP& curr_ldp_scan, const rclcpp::Time& time) 337 | { 338 | 339 | 340 | // CSM is used in the following way: 341 | // The scans are always in the laser frame 342 | // The reference scan (prevLDPcan_) has a pose of [0, 0, 0] 343 | // The new scan (currLDPScan) has a pose equal to the movement 344 | // of the laser in the laser frame since the last scan 345 | // The computed correction is then propagated using the tf machinery 346 | 347 | prev_ldp_scan_->odometry[0] = 0.0; 348 | prev_ldp_scan_->odometry[1] = 0.0; 349 | prev_ldp_scan_->odometry[2] = 0.0; 350 | 351 | prev_ldp_scan_->estimate[0] = 0.0; 352 | prev_ldp_scan_->estimate[1] = 0.0; 353 | prev_ldp_scan_->estimate[2] = 0.0; 354 | 355 | prev_ldp_scan_->true_pose[0] = 0.0; 356 | prev_ldp_scan_->true_pose[1] = 0.0; 357 | prev_ldp_scan_->true_pose[2] = 0.0; 358 | 359 | input_.laser_ref = prev_ldp_scan_; 360 | input_.laser_sens = curr_ldp_scan; 361 | 362 | // estimated change since last scan 363 | // the predicted change of the laser's position, in the laser frame 364 | 365 | tf2::Transform pr_ch_l; 366 | 367 | double dt = (now() - last_icp_time_).nanoseconds()/1e+9; 368 | double pr_ch_x, pr_ch_y, pr_ch_a; 369 | 370 | 371 | // the predicted change of the laser's position, in the fixed frame 372 | 373 | tf2::Transform pr_ch; 374 | createTfFromXYTheta(0.0,0.0,0.0, pr_ch); 375 | 376 | // account for the change since the last kf, in the fixed frame 377 | 378 | pr_ch = pr_ch * (f2b_ * f2b_kf_.inverse()); 379 | 380 | // the predicted change of the laser's position, in the laser frame 381 | 382 | 383 | pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ; 384 | input_.first_guess[0] = pr_ch_l.getOrigin().getX(); 385 | input_.first_guess[1] = pr_ch_l.getOrigin().getY(); 386 | input_.first_guess[2] = tf2::getYaw(pr_ch_l.getRotation()); 387 | 388 | 389 | // If they are non-Null, free covariance gsl matrices to avoid leaking memory 390 | if (output_.cov_x_m) 391 | { 392 | gsl_matrix_free(output_.cov_x_m); 393 | output_.cov_x_m = 0; 394 | } 395 | if (output_.dx_dy1_m) 396 | { 397 | gsl_matrix_free(output_.dx_dy1_m); 398 | output_.dx_dy1_m = 0; 399 | } 400 | if (output_.dx_dy2_m) 401 | { 402 | gsl_matrix_free(output_.dx_dy2_m); 403 | output_.dx_dy2_m = 0; 404 | } 405 | 406 | // Scan matching - using point to line icp from CSM 407 | 408 | sm_icp(&input_, &output_); 409 | tf2::Transform corr_ch; 410 | 411 | if (output_.valid) 412 | { 413 | // the correction of the laser's position, in the laser frame 414 | tf2::Transform corr_ch_l; 415 | createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l); 416 | 417 | // the correction of the base's position, in the base frame 418 | corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_; 419 | 420 | // update the pose in the world frame 421 | f2b_ = f2b_kf_ * corr_ch; 422 | 423 | } 424 | 425 | else 426 | { 427 | corr_ch.setIdentity(); 428 | RCLCPP_WARN(get_logger(),"Error in scan matching"); 429 | return false; 430 | } 431 | 432 | 433 | if (publish_odom_) 434 | { 435 | // stamped Pose message 436 | nav_msgs::msg::Odometry odom_msg; 437 | 438 | odom_msg.header.stamp = time; 439 | odom_msg.header.frame_id = odom_frame_; 440 | odom_msg.child_frame_id = base_frame_; 441 | odom_msg.pose.pose.position.x = f2b_.getOrigin().x(); 442 | odom_msg.pose.pose.position.y = f2b_.getOrigin().y(); 443 | odom_msg.pose.pose.position.z = f2b_.getOrigin().z(); 444 | 445 | odom_msg.pose.pose.orientation.x = f2b_.getRotation().x(); 446 | odom_msg.pose.pose.orientation.y = f2b_.getRotation().y(); 447 | odom_msg.pose.pose.orientation.z = f2b_.getRotation().z(); 448 | odom_msg.pose.pose.orientation.w = f2b_.getRotation().w(); 449 | 450 | // Get pose difference in base frame and calculate velocities 451 | auto pose_difference = prev_f2b_.inverse() * f2b_; 452 | odom_msg.twist.twist.linear.x = pose_difference.getOrigin().getX()/dt; 453 | odom_msg.twist.twist.linear.y = pose_difference.getOrigin().getY()/dt; 454 | odom_msg.twist.twist.angular.z = tf2::getYaw(pose_difference.getRotation())/dt; 455 | 456 | prev_f2b_ = f2b_; 457 | 458 | odom_publisher_->publish(odom_msg); 459 | } 460 | 461 | 462 | if (publish_tf_) 463 | { 464 | geometry_msgs::msg::TransformStamped tf_msg; 465 | tf_msg.transform.translation.x = f2b_.getOrigin().x(); 466 | tf_msg.transform.translation.y = f2b_.getOrigin().y(); 467 | tf_msg.transform.translation.z = f2b_.getOrigin().z(); 468 | tf_msg.transform.rotation.x = f2b_.getRotation().x(); 469 | tf_msg.transform.rotation.y = f2b_.getRotation().y(); 470 | tf_msg.transform.rotation.z = f2b_.getRotation().z(); 471 | tf_msg.transform.rotation.w = f2b_.getRotation().w(); 472 | 473 | tf_msg.header.stamp = time; 474 | tf_msg.header.frame_id = odom_frame_; 475 | tf_msg.child_frame_id = base_frame_; 476 | //tf2::Stamped transform_msg (f2b_, time, map_frame_, base_frame_); 477 | tfB_->sendTransform (tf_msg); 478 | } 479 | 480 | // **** swap old and new 481 | if (newKeyframeNeeded(corr_ch)) 482 | { 483 | // generate a keyframe 484 | ld_free(prev_ldp_scan_); 485 | prev_ldp_scan_ = curr_ldp_scan; 486 | f2b_kf_ = f2b_; 487 | 488 | } 489 | else 490 | { 491 | ld_free(curr_ldp_scan); 492 | 493 | } 494 | last_icp_time_ = now(); 495 | return true; 496 | } 497 | 498 | bool LaserScanMatcher::newKeyframeNeeded(const tf2::Transform& d) 499 | { 500 | if (fabs(tf2::getYaw(d.getRotation())) > kf_dist_angular_) 501 | return true; 502 | 503 | double x = d.getOrigin().getX(); 504 | double y = d.getOrigin().getY(); 505 | if (x * x + y * y > kf_dist_linear_sq_) 506 | return true; 507 | 508 | return false; 509 | } 510 | 511 | void LaserScanMatcher::laserScanToLDP(const sensor_msgs::msg::LaserScan::SharedPtr& scan, LDP& ldp) 512 | { 513 | unsigned int n = scan->ranges.size(); 514 | ldp = ld_alloc_new(n); 515 | 516 | for (unsigned int i = 0; i < n; i++) 517 | { 518 | // Calculate position in laser frame 519 | double r = scan->ranges[i]; 520 | if ((r > scan->range_min) && (r < scan->range_max)) 521 | { 522 | // Fill in laser scan data 523 | ldp->valid[i] = 1; 524 | ldp->readings[i] = r; 525 | } 526 | else 527 | { 528 | ldp->valid[i] = 0; 529 | ldp->readings[i] = -1; // for invalid range 530 | } 531 | ldp->theta[i] = scan->angle_min + i * scan->angle_increment; 532 | ldp->cluster[i] = -1; 533 | } 534 | 535 | ldp->min_theta = ldp->theta[0]; 536 | ldp->max_theta = ldp->theta[n - 1]; 537 | 538 | ldp->odometry[0] = 0.0; 539 | ldp->odometry[1] = 0.0; 540 | ldp->odometry[2] = 0.0; 541 | 542 | ldp->true_pose[0] = 0.0; 543 | ldp->true_pose[1] = 0.0; 544 | ldp->true_pose[2] = 0.0; 545 | } 546 | 547 | 548 | void LaserScanMatcher::createTfFromXYTheta(double x, double y, double theta, tf2::Transform& t) 549 | { 550 | t.setOrigin(tf2::Vector3(x, y, 0.0)); 551 | tf2::Quaternion q; 552 | q.setRPY(0.0, 0.0, theta); 553 | t.setRotation(q); 554 | } 555 | 556 | 557 | } // namespace scan_tools 558 | 559 | int main(int argc, char ** argv) 560 | { 561 | rclcpp::init(argc, argv); 562 | auto node = std::make_shared(); 563 | rclcpp::spin(node->get_node_base_interface()); 564 | rclcpp::shutdown(); 565 | 566 | return 0; 567 | } 568 | --------------------------------------------------------------------------------