diff --git a/laser_slam_ros/include/laser_slam_ros/common.hpp b/laser_slam_ros/include/laser_slam_ros/common.hpp index f83e7b5..58147f8 100644 --- a/laser_slam_ros/include/laser_slam_ros/common.hpp +++ b/laser_slam_ros/include/laser_slam_ros/common.hpp @@ -52,6 +52,11 @@ struct LaserSlamWorkerParams { bool publish_full_map; bool publish_distant_map; double map_publication_rate_hz; + + // Odometry noiser + bool enable_drift; + 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 +94,20 @@ 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.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); + 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 f86cc46..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 @@ -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,7 +78,15 @@ 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); + + 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? @@ -97,7 +108,18 @@ class LaserSlamWorker { bool getLaserTracksServiceCall(laser_slam_ros::GetLaserTrackSrv::Request& request, laser_slam_ros::GetLaserTrackSrv::Response& response); - private: + // Odometry noiser + bool enable_drift_ = false; + tf::Transform T_W_BLast_; + tf::Transform T_W_BdLast_; + + 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_; + LaserSlamWorkerParams params_; unsigned int worker_id_; @@ -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 7323fd4..d61ccad 100644 --- a/laser_slam_ros/src/laser_slam_worker.cpp +++ b/laser_slam_ros/src/laser_slam_worker.cpp @@ -56,6 +56,23 @@ void LaserSlamWorker::init( trajectory_pub_ = nh.advertise(params_.trajectory_pub_topic, kPublisherQueueSize, true); + // Odometry noiser + enable_drift_ = params.enable_drift; + 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, kPublisherQueueSize); @@ -96,7 +113,46 @@ 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 + // B = Sensor frame. Bd = Drifted sensor frame. W = Odom frame. + if(enable_drift_) { + tf::Transform T_W_B = tf_transform; + + // 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(); + + // 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_); + 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. + + // Compute drifted sensor frame T_W_Bd. + tf::Transform T_W_Bd = T_W_BdLast_ * T_BdLast_Bd; + tf_transform.setData(T_W_Bd); + + // Store values for next iteration. + T_W_BdLast_ = T_W_Bd; + T_W_BLast_ = T_W_B; + } else { + T_W_BLast_ = tf_transform; + T_W_BdLast_ = tf_transform; + } bool process_scan = false; SE3 current_pose; @@ -348,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;