From 70e7c18a2a368970fb8654860bb7b0c8219b0788 Mon Sep 17 00:00:00 2001 From: Nikhilesh Alatur Date: Mon, 2 Nov 2020 11:30:04 +0100 Subject: [PATCH 1/2] Add noise to odometry, if desired. --- .../include/laser_slam_ros/common.hpp | 22 ++++ .../laser_slam_ros/laser_slam_worker.hpp | 23 ++++ laser_slam_ros/src/laser_slam_worker.cpp | 109 ++++++++++++++++++ 3 files changed, 154 insertions(+) diff --git a/laser_slam_ros/include/laser_slam_ros/common.hpp b/laser_slam_ros/include/laser_slam_ros/common.hpp index 73cf5c1..93e6169 100644 --- a/laser_slam_ros/include/laser_slam_ros/common.hpp +++ b/laser_slam_ros/include/laser_slam_ros/common.hpp @@ -52,6 +52,13 @@ struct LaserSlamWorkerParams { bool publish_full_map; bool publish_distant_map; double map_publication_rate_hz; + + //#####ODOMETRY NOISER############### + bool enable_drift; + std::string sensor_drifted_frame; + float noise_x_mean, noise_y_mean, noise_z_mean, noise_yaw_mean, noise_att_mean; + float noise_x_stddev, noise_y_stddev, noise_z_stddev, noise_yaw_stddev, noise_att_stddev; + //################################### }; // struct LaserSlamWorkerParams static LaserSlamWorkerParams getLaserSlamWorkerParams(const ros::NodeHandle& nh, @@ -89,6 +96,21 @@ static LaserSlamWorkerParams getLaserSlamWorkerParams(const ros::NodeHandle& nh, nh.getParam(ns + "/distant_map_pub_topic", params.distant_map_pub_topic); nh.getParam(ns + "/get_laser_track_srv_topic", params.get_laser_track_srv_topic); + //#####ODOMETRY NOISER############### + nh.getParam(ns + "/enable_drift", params.enable_drift); + nh.getParam(ns + "/sensor_drifted_frame", params.sensor_drifted_frame); + nh.getParam(ns + "/noise_x_mean", params.noise_x_mean); + nh.getParam(ns + "/noise_y_mean", params.noise_y_mean); + nh.getParam(ns + "/noise_z_mean", params.noise_z_mean); + nh.getParam(ns + "/noise_yaw_mean", params.noise_yaw_mean); + nh.getParam(ns + "/noise_att_mean", params.noise_att_mean); + nh.getParam(ns + "/noise_x_stddev", params.noise_x_stddev); + nh.getParam(ns + "/noise_y_stddev", params.noise_y_stddev); + nh.getParam(ns + "/noise_z_stddev", params.noise_z_stddev); + nh.getParam(ns + "/noise_yaw_stddev", params.noise_yaw_stddev); + nh.getParam(ns + "/noise_att_stddev", params.noise_att_stddev); + //################################### + return params; } diff --git a/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp b/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp index e5dd3c2..9f5c8bb 100644 --- a/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp +++ b/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp @@ -10,10 +10,13 @@ #include #include #include +#include #include "laser_slam_ros/GetLaserTrackSrv.h" #include "laser_slam_ros/common.hpp" +#include + namespace laser_slam_ros { class LaserSlamWorker { @@ -75,6 +78,8 @@ class LaserSlamWorker { bool exportTrajectoryServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + bool exportDriftServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + private: // Convert a tf::StampedTransform to a laser_slam::Pose. laser_slam::Pose tfTransformToPose(const tf::StampedTransform& tf_transform); @@ -97,6 +102,23 @@ class LaserSlamWorker { bool getLaserTracksServiceCall(laser_slam_ros::GetLaserTrackSrv::Request& request, laser_slam_ros::GetLaserTrackSrv::Response& response); + //###############ODOMETRY NOISER########################## + bool enable_drift_ = false; + tf::Transform T_W_BLast_; + tf::Transform T_W_BdLast_; + tf::TransformBroadcaster br_; + std::vector T_B_Bd_vec; + + float noise_x_mean_, noise_y_mean_, noise_z_mean_, noise_yaw_mean_, noise_attitude_mean_; + float noise_x_stddev_, noise_y_stddev_, noise_z_stddev_, noise_yaw_stddev_, noise_attitude_stddev_; + std::default_random_engine generator_; + std::normal_distribution dist_x_; + std::normal_distribution dist_y_; + std::normal_distribution dist_z_; + std::normal_distribution dist_yaw_; + std::normal_distribution dist_att_; + //######################################################## + private: LaserSlamWorkerParams params_; @@ -127,6 +149,7 @@ class LaserSlamWorker { // Services. ros::ServiceServer get_laser_track_srv_; ros::ServiceServer export_trajectory_srv_; + ros::ServiceServer export_drift_srv_; tf::TransformListener tf_listener_; diff --git a/laser_slam_ros/src/laser_slam_worker.cpp b/laser_slam_ros/src/laser_slam_worker.cpp index f399a65..2d15289 100644 --- a/laser_slam_ros/src/laser_slam_worker.cpp +++ b/laser_slam_ros/src/laser_slam_worker.cpp @@ -54,6 +54,18 @@ void LaserSlamWorker::init( trajectory_pub_ = nh.advertise(params_.trajectory_pub_topic, kPublisherQueueSize, true); + //################### ODOMETRY NOISER################### + T_W_BdLast_.setIdentity(); + T_W_BLast_.setIdentity(); + T_B_Bd_vec.clear(); + enable_drift_ = params.enable_drift; + dist_x_.param(std::normal_distribution::param_type(params_.noise_x_mean, params_.noise_x_stddev)); + dist_y_.param(std::normal_distribution::param_type(params_.noise_y_mean, params_.noise_y_stddev)); + dist_z_.param(std::normal_distribution::param_type(params_.noise_z_mean, params_.noise_z_stddev)); + dist_yaw_.param(std::normal_distribution::param_type(params_.noise_yaw_mean, params_.noise_yaw_stddev)); + dist_att_.param(std::normal_distribution::param_type(params_.noise_att_mean, params_.noise_att_stddev)); + //###################################################### + if (params_.publish_local_map) { local_map_pub_ = nh.advertise(params_.local_map_pub_topic, kPublisherQueueSize); @@ -66,6 +78,9 @@ void LaserSlamWorker::init( export_trajectory_srv_ = nh.advertiseService( "export_trajectory", &LaserSlamWorker::exportTrajectoryServiceCall, this); + export_drift_srv_ = nh.advertiseService( + "export_drift", + &LaserSlamWorker::exportDriftServiceCall, this); voxel_filter_.setLeafSize(params_.voxel_size_m, params_.voxel_size_m, params_.voxel_size_m); @@ -103,6 +118,58 @@ void LaserSlamWorker::scanCallback(const sensor_msgs::PointCloud2& cloud_msg_in) tf_listener_.lookupTransform(params_.odom_frame, params_.sensor_frame, cloud_msg_in.header.stamp, tf_transform); + //######################### ADD DRIFT ############################### + tf::Transform T_B_Bd; // B = Sensor frame. Bd = Drifted sensor frame. + std::string stamp; + if(enable_drift_) + { + tf::Transform T_W_B = tf_transform; // W = Odom frame. + + // 1.) Compute odometry since last update T_BLast_B. + tf::Transform T_BLast_B = (T_W_BLast_.inverse())*T_W_B; + tf::Vector3 transl_gt = T_BLast_B.getOrigin(); + tf::Quaternion quat_gt = T_BLast_B.getRotation(); + + // 2.) Add noise in local frame. T_BdLast_Bd = adddNoise(T_BLast_B) + // Sample noise. + float noise_x = dist_x_(generator_); + float noise_y = dist_y_(generator_); + float noise_z = dist_z_(generator_); + float noise_yaw = dist_yaw_(generator_); + float noise_roll = dist_att_(generator_); + float noise_pitch = dist_att_(generator_); + tf::Quaternion quat_noise; + quat_noise.setRPY(noise_roll, noise_pitch, noise_yaw); + + transl_gt.setX(noise_x + T_BLast_B.getOrigin().x()); + transl_gt.setY(noise_y + T_BLast_B.getOrigin().y()); + transl_gt.setZ(noise_z + T_BLast_B.getOrigin().z()); + tf::Transform T_BdLast_Bd = T_BLast_B; + T_BdLast_Bd.setOrigin(transl_gt); // Add translation drift. + T_BdLast_Bd.setRotation(quat_gt*quat_noise); // Add rotation drift. + + // 3.) Compute drifted sensor frame T_W_Bd. + tf::Transform T_W_Bd = T_W_BdLast_*T_BdLast_Bd; + tf_transform.setData(T_W_Bd); + tf_transform.child_frame_id_ = params_.sensor_drifted_frame; + + // 4.) Compute relation to actual sensor frame T_B_Bd. + tf::Transform T_B_Bd = (T_W_B.inverse())*T_W_Bd; + tf::StampedTransform T_B_Bd_stamped = tf::StampedTransform(T_B_Bd, tf_transform.stamp_, params_.sensor_frame, params_.sensor_drifted_frame); + br_.sendTransform(T_B_Bd_stamped); // Publish for visualization. + T_B_Bd_vec.push_back(T_B_Bd_stamped); // For post-processing. + + // 5.) Store values for next iteration. + T_W_BdLast_ = T_W_Bd; + T_W_BLast_ = T_W_B; + + // 6.) ToDo(alaturn) Store values for post-processing. + // float drift_abs = T_B_Bd.getOrigin().length(); + // float drift_rel = (drift_abs/path_length_)*100.0; + } + + //################################################################### + bool process_scan = false; SE3 current_pose; @@ -602,4 +669,46 @@ bool LaserSlamWorker::exportTrajectoryServiceCall(std_srvs::Empty::Request& req, return true; } +bool LaserSlamWorker::exportDriftServiceCall(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res) { + + if(enable_drift_) + { + std::string filename = "/tmp/online_matcher/drift_values.csv"; // ToDo(alaturn) Ensure that directory exists. + std::ofstream output_file; + output_file.open(filename.c_str(), std::ofstream::out | std::ofstream::trunc); + output_file << "Sensor Frame: "<stamp_.toSec()) << " "; // Timestamp. + output_file << "t_x: " << it->getOrigin().x() << " "; + output_file << "t_y: " << it->getOrigin().y() << " "; + output_file << "t_z: " << it->getOrigin().z() << " "; + output_file << "R_11: " << it->getBasis().getRow(0).x() << " "; + output_file << "R_12: " << it->getBasis().getRow(0).y() << " "; + output_file << "R_13: " << it->getBasis().getRow(0).z() << " "; + output_file << "R_21: " << it->getBasis().getRow(1).x() << " "; + output_file << "R_22: " << it->getBasis().getRow(1).y() << " "; + output_file << "R_23: " << it->getBasis().getRow(1).z() << " "; + output_file << "R_31: " << it->getBasis().getRow(2).x() << " "; + output_file << "R_32: " << it->getBasis().getRow(2).y() << " "; + output_file << "R_33: " << it->getBasis().getRow(2).z() << " "; + output_file << std::endl; + // output_file << "path_length: " << -1 << " "; + // output_file << "absolute_drift_m: " << -1 << " "; + // output_file << "relative_drift: " << -1 << " "; + } + output_file.close(); + LOG(INFO) << "Exported " << i << " drifts to " << filename << "."; + } + else{ + LOG(INFO) << "No drift was added. Nothing to export!"; + } + return true; +} + + } // namespace laser_slam_ros From 718e5f284a6ca18a6adc4e9db75fed19fdcb1f15 Mon Sep 17 00:00:00 2001 From: Andrei Cramariuc Date: Sun, 22 Nov 2020 23:29:22 +0100 Subject: [PATCH 2/2] Integrated drift module better into segmap for evaluation --- .../include/laser_slam_ros/common.hpp | 7 +- .../laser_slam_ros/laser_slam_worker.hpp | 14 +- laser_slam_ros/src/laser_slam_worker.cpp | 124 ++++++------------ 3 files changed, 49 insertions(+), 96 deletions(-) diff --git a/laser_slam_ros/include/laser_slam_ros/common.hpp b/laser_slam_ros/include/laser_slam_ros/common.hpp index 5bf3096..58147f8 100644 --- a/laser_slam_ros/include/laser_slam_ros/common.hpp +++ b/laser_slam_ros/include/laser_slam_ros/common.hpp @@ -53,12 +53,10 @@ struct LaserSlamWorkerParams { bool publish_distant_map; double map_publication_rate_hz; - //#####ODOMETRY NOISER############### + // Odometry noiser bool enable_drift; - std::string sensor_drifted_frame; float noise_x_mean, noise_y_mean, noise_z_mean, noise_yaw_mean, noise_att_mean; float noise_x_stddev, noise_y_stddev, noise_z_stddev, noise_yaw_stddev, noise_att_stddev; - //################################### }; // struct LaserSlamWorkerParams static LaserSlamWorkerParams getLaserSlamWorkerParams(const ros::NodeHandle& nh, @@ -97,8 +95,7 @@ static LaserSlamWorkerParams getLaserSlamWorkerParams(const ros::NodeHandle& nh, nh.getParam(ns + "/get_laser_track_srv_topic", params.get_laser_track_srv_topic); //#####ODOMETRY NOISER############### - nh.getParam(ns + "/enable_drift", params.enable_drift); - nh.getParam(ns + "/sensor_drifted_frame", params.sensor_drifted_frame); + nh.param(ns + "/enable_drift", params.enable_drift, false); nh.getParam(ns + "/noise_x_mean", params.noise_x_mean); nh.getParam(ns + "/noise_y_mean", params.noise_y_mean); nh.getParam(ns + "/noise_z_mean", params.noise_z_mean); diff --git a/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp b/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp index ba05043..e439242 100644 --- a/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp +++ b/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp @@ -80,7 +80,13 @@ class LaserSlamWorker { bool exportDriftServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + laser_slam::SE3 getGrountTruthPose() { + return tfTransformToSE3(T_W_BLast_); + } + private: + // Convert a tf::Transform to a laser_slam::SE3. + laser_slam::SE3 tfTransformToSE3(const tf::Transform& tf_transform); // Convert a tf::StampedTransform to a laser_slam::Pose. laser_slam::Pose tfTransformToPose(const tf::StampedTransform& tf_transform); // TODO: common.hpp? @@ -102,24 +108,18 @@ class LaserSlamWorker { bool getLaserTracksServiceCall(laser_slam_ros::GetLaserTrackSrv::Request& request, laser_slam_ros::GetLaserTrackSrv::Response& response); - //###############ODOMETRY NOISER########################## + // Odometry noiser bool enable_drift_ = false; tf::Transform T_W_BLast_; tf::Transform T_W_BdLast_; - tf::TransformBroadcaster br_; - std::vector T_B_Bd_vec; - float noise_x_mean_, noise_y_mean_, noise_z_mean_, noise_yaw_mean_, noise_attitude_mean_; - float noise_x_stddev_, noise_y_stddev_, noise_z_stddev_, noise_yaw_stddev_, noise_attitude_stddev_; std::default_random_engine generator_; std::normal_distribution dist_x_; std::normal_distribution dist_y_; std::normal_distribution dist_z_; std::normal_distribution dist_yaw_; std::normal_distribution dist_att_; - //######################################################## - private: LaserSlamWorkerParams params_; unsigned int worker_id_; diff --git a/laser_slam_ros/src/laser_slam_worker.cpp b/laser_slam_ros/src/laser_slam_worker.cpp index 74bffaf..d61ccad 100644 --- a/laser_slam_ros/src/laser_slam_worker.cpp +++ b/laser_slam_ros/src/laser_slam_worker.cpp @@ -56,17 +56,22 @@ void LaserSlamWorker::init( trajectory_pub_ = nh.advertise(params_.trajectory_pub_topic, kPublisherQueueSize, true); - //################### ODOMETRY NOISER################### - T_W_BdLast_.setIdentity(); - T_W_BLast_.setIdentity(); - T_B_Bd_vec.clear(); + // Odometry noiser enable_drift_ = params.enable_drift; - dist_x_.param(std::normal_distribution::param_type(params_.noise_x_mean, params_.noise_x_stddev)); - dist_y_.param(std::normal_distribution::param_type(params_.noise_y_mean, params_.noise_y_stddev)); - dist_z_.param(std::normal_distribution::param_type(params_.noise_z_mean, params_.noise_z_stddev)); - dist_yaw_.param(std::normal_distribution::param_type(params_.noise_yaw_mean, params_.noise_yaw_stddev)); - dist_att_.param(std::normal_distribution::param_type(params_.noise_att_mean, params_.noise_att_stddev)); - //###################################################### + if (enable_drift_) { + T_W_BdLast_.setIdentity(); + T_W_BLast_.setIdentity(); + dist_x_.param(std::normal_distribution::param_type( + params_.noise_x_mean, params_.noise_x_stddev)); + dist_y_.param(std::normal_distribution::param_type( + params_.noise_y_mean, params_.noise_y_stddev)); + dist_z_.param(std::normal_distribution::param_type( + params_.noise_z_mean, params_.noise_z_stddev)); + dist_yaw_.param(std::normal_distribution::param_type( + params_.noise_yaw_mean, params_.noise_yaw_stddev)); + dist_att_.param(std::normal_distribution::param_type( + params_.noise_att_mean, params_.noise_att_stddev)); + } if (params_.publish_local_map) { local_map_pub_ = nh.advertise(params_.local_map_pub_topic, @@ -83,9 +88,6 @@ void LaserSlamWorker::init( export_trajectory_srv_ = nh.advertiseService( "export_trajectory", &LaserSlamWorker::exportTrajectoryServiceCall, this); - export_drift_srv_ = nh.advertiseService( - "export_drift", - &LaserSlamWorker::exportDriftServiceCall, this); voxel_filter_.setLeafSize(params_.voxel_size_m, params_.voxel_size_m, params_.voxel_size_m); @@ -111,22 +113,19 @@ void LaserSlamWorker::scanCallback(const sensor_msgs::PointCloud2& cloud_msg_in) // Get the tf transform. tf::StampedTransform tf_transform; tf_listener_->lookupTransform(params_.odom_frame, params_.sensor_frame, - cloud_msg_in.header.stamp, tf_transform); + cloud_msg_in.header.stamp, tf_transform); - //######################### ADD DRIFT ############################### - tf::Transform T_B_Bd; // B = Sensor frame. Bd = Drifted sensor frame. - std::string stamp; - if(enable_drift_) - { - tf::Transform T_W_B = tf_transform; // W = Odom frame. + // Add drift + // B = Sensor frame. Bd = Drifted sensor frame. W = Odom frame. + if(enable_drift_) { + tf::Transform T_W_B = tf_transform; - // 1.) Compute odometry since last update T_BLast_B. - tf::Transform T_BLast_B = (T_W_BLast_.inverse())*T_W_B; + // Compute odometry since last update T_BLast_B. + tf::Transform T_BLast_B = (T_W_BLast_.inverse()) * T_W_B; tf::Vector3 transl_gt = T_BLast_B.getOrigin(); tf::Quaternion quat_gt = T_BLast_B.getRotation(); - // 2.) Add noise in local frame. T_BdLast_Bd = adddNoise(T_BLast_B) - // Sample noise. + // Add noise in local frame. T_BdLast_Bd = adddNoise(T_BLast_B) float noise_x = dist_x_(generator_); float noise_y = dist_y_(generator_); float noise_z = dist_z_(generator_); @@ -141,30 +140,20 @@ void LaserSlamWorker::scanCallback(const sensor_msgs::PointCloud2& cloud_msg_in) transl_gt.setZ(noise_z + T_BLast_B.getOrigin().z()); tf::Transform T_BdLast_Bd = T_BLast_B; T_BdLast_Bd.setOrigin(transl_gt); // Add translation drift. - T_BdLast_Bd.setRotation(quat_gt*quat_noise); // Add rotation drift. + T_BdLast_Bd.setRotation(quat_gt * quat_noise); // Add rotation drift. - // 3.) Compute drifted sensor frame T_W_Bd. - tf::Transform T_W_Bd = T_W_BdLast_*T_BdLast_Bd; + // Compute drifted sensor frame T_W_Bd. + tf::Transform T_W_Bd = T_W_BdLast_ * T_BdLast_Bd; tf_transform.setData(T_W_Bd); - tf_transform.child_frame_id_ = params_.sensor_drifted_frame; - // 4.) Compute relation to actual sensor frame T_B_Bd. - tf::Transform T_B_Bd = (T_W_B.inverse())*T_W_Bd; - tf::StampedTransform T_B_Bd_stamped = tf::StampedTransform(T_B_Bd, tf_transform.stamp_, params_.sensor_frame, params_.sensor_drifted_frame); - br_.sendTransform(T_B_Bd_stamped); // Publish for visualization. - T_B_Bd_vec.push_back(T_B_Bd_stamped); // For post-processing. - - // 5.) Store values for next iteration. + // Store values for next iteration. T_W_BdLast_ = T_W_Bd; T_W_BLast_ = T_W_B; - - // 6.) ToDo(alaturn) Store values for post-processing. - // float drift_abs = T_B_Bd.getOrigin().length(); - // float drift_rel = (drift_abs/path_length_)*100.0; + } else { + T_W_BLast_ = tf_transform; + T_W_BdLast_ = tf_transform; } - //################################################################### - bool process_scan = false; SE3 current_pose; @@ -415,7 +404,16 @@ void LaserSlamWorker::publishTrajectories() { if (trajectory.size() > 0u) publishTrajectory(trajectory, trajectory_pub_); } -// TODO can we move? +SE3 LaserSlamWorker::tfTransformToSE3(const tf::Transform& tf_transform) { + SE3::Position pos(tf_transform.getOrigin().getX(), tf_transform.getOrigin().getY(), + tf_transform.getOrigin().getZ()); + SE3::Rotation::Implementation rot(tf_transform.getRotation().getW(), + tf_transform.getRotation().getX(), + tf_transform.getRotation().getY(), + tf_transform.getRotation().getZ()); + return SE3(pos, rot); +} + Pose LaserSlamWorker::tfTransformToPose(const tf::StampedTransform& tf_transform) { // Register new pose. Pose pose; @@ -642,46 +640,4 @@ bool LaserSlamWorker::exportTrajectoryServiceCall(std_srvs::Empty::Request& req, return true; } -bool LaserSlamWorker::exportDriftServiceCall(std_srvs::Empty::Request& req, - std_srvs::Empty::Response& res) { - - if(enable_drift_) - { - std::string filename = "/tmp/online_matcher/drift_values.csv"; // ToDo(alaturn) Ensure that directory exists. - std::ofstream output_file; - output_file.open(filename.c_str(), std::ofstream::out | std::ofstream::trunc); - output_file << "Sensor Frame: "<stamp_.toSec()) << " "; // Timestamp. - output_file << "t_x: " << it->getOrigin().x() << " "; - output_file << "t_y: " << it->getOrigin().y() << " "; - output_file << "t_z: " << it->getOrigin().z() << " "; - output_file << "R_11: " << it->getBasis().getRow(0).x() << " "; - output_file << "R_12: " << it->getBasis().getRow(0).y() << " "; - output_file << "R_13: " << it->getBasis().getRow(0).z() << " "; - output_file << "R_21: " << it->getBasis().getRow(1).x() << " "; - output_file << "R_22: " << it->getBasis().getRow(1).y() << " "; - output_file << "R_23: " << it->getBasis().getRow(1).z() << " "; - output_file << "R_31: " << it->getBasis().getRow(2).x() << " "; - output_file << "R_32: " << it->getBasis().getRow(2).y() << " "; - output_file << "R_33: " << it->getBasis().getRow(2).z() << " "; - output_file << std::endl; - // output_file << "path_length: " << -1 << " "; - // output_file << "absolute_drift_m: " << -1 << " "; - // output_file << "relative_drift: " << -1 << " "; - } - output_file.close(); - LOG(INFO) << "Exported " << i << " drifts to " << filename << "."; - } - else{ - LOG(INFO) << "No drift was added. Nothing to export!"; - } - return true; -} - - } // namespace laser_slam_ros