laser_scan_matcher.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Ivan Dryanovski, William Morris
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the CCNY Robotics Lab nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*  This package uses Canonical Scan Matcher [1], written by
00031  *  Andrea Censi
00032  *
00033  *  [1] A. Censi, "An ICP variant using a point-to-line metric"
00034  *  Proceedings of the IEEE International Conference
00035  *  on Robotics and Automation (ICRA), 2008
00036  */
00037 
00038 #ifndef LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H
00039 #define LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H
00040 
00041 #include <ros/ros.h>
00042 #include <sensor_msgs/Imu.h>
00043 #include <sensor_msgs/LaserScan.h>
00044 #include <geometry_msgs/PoseStamped.h>
00045 #include <geometry_msgs/Pose2D.h>
00046 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00047 #include <geometry_msgs/PoseWithCovariance.h>
00048 #include <geometry_msgs/TwistStamped.h>
00049 #include <nav_msgs/Odometry.h>
00050 #include <tf/transform_datatypes.h>
00051 #include <tf/transform_listener.h>
00052 #include <tf/transform_broadcaster.h>
00053 #include <pcl/point_types.h>
00054 #include <pcl/point_cloud.h>
00055 #include <pcl/filters/voxel_grid.h>
00056 #include <pcl_ros/point_cloud.h>
00057 
00058 #include <csm/csm_all.h>  // csm defines min and max, but Eigen complains
00059 #undef min
00060 #undef max
00061 
00062 namespace scan_tools
00063 {
00064 
00065 class LaserScanMatcher
00066 {
00067   public:
00068 
00069     LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private);
00070     ~LaserScanMatcher();
00071 
00072   private:
00073 
00074     typedef pcl::PointXYZ           PointT;
00075     typedef pcl::PointCloud<PointT> PointCloudT;
00076 
00077     // **** ros
00078 
00079     ros::NodeHandle nh_;
00080     ros::NodeHandle nh_private_;
00081 
00082     ros::Subscriber scan_subscriber_;
00083     ros::Subscriber cloud_subscriber_;
00084     ros::Subscriber odom_subscriber_;
00085     ros::Subscriber imu_subscriber_;
00086     ros::Subscriber vel_subscriber_;
00087 
00088     tf::TransformListener    tf_listener_;
00089     tf::TransformBroadcaster tf_broadcaster_;
00090 
00091     tf::Transform base_to_laser_; // static, cached
00092     tf::Transform laser_to_base_; // static, cached, calculated from base_to_laser_
00093 
00094     ros::Publisher  pose_publisher_;
00095     ros::Publisher  pose_stamped_publisher_;
00096     ros::Publisher  pose_with_covariance_publisher_;
00097     ros::Publisher  pose_with_covariance_stamped_publisher_;
00098 
00099     // **** parameters
00100 
00101     std::string base_frame_;
00102     std::string fixed_frame_;
00103     double cloud_range_min_;
00104     double cloud_range_max_;
00105     double cloud_res_;
00106     bool publish_tf_;
00107     bool publish_pose_;
00108     bool publish_pose_with_covariance_;
00109     bool publish_pose_stamped_;
00110     bool publish_pose_with_covariance_stamped_;
00111     std::vector<double> position_covariance_;
00112     std::vector<double> orientation_covariance_;
00113 
00114     bool use_cloud_input_;
00115 
00116     double kf_dist_linear_;
00117     double kf_dist_linear_sq_;
00118     double kf_dist_angular_;
00119 
00120     // **** What predictions are available to speed up the ICP?
00121     // 1) imu - [theta] from imu yaw angle - /imu topic
00122     // 2) odom - [x, y, theta] from wheel odometry - /odom topic
00123     // 3) velocity [vx, vy, vtheta], usually from ab-filter - /vel.
00124     // If more than one is enabled, priority is imu > odom > velocity
00125 
00126     bool use_imu_;
00127     bool use_odom_;
00128     bool use_vel_;
00129     bool stamped_vel_;
00130 
00131     // **** state variables
00132 
00133     boost::mutex mutex_;
00134 
00135     bool initialized_;
00136     bool received_imu_;
00137     bool received_odom_;
00138     bool received_vel_;
00139 
00140     tf::Transform f2b_;    // fixed-to-base tf (pose of base frame in fixed frame)
00141     tf::Transform f2b_kf_; // pose of the last keyframe scan in fixed frame
00142 
00143     ros::Time last_icp_time_;
00144 
00145     sensor_msgs::Imu latest_imu_msg_;
00146     sensor_msgs::Imu last_used_imu_msg_;
00147     nav_msgs::Odometry latest_odom_msg_;
00148     nav_msgs::Odometry last_used_odom_msg_;
00149 
00150     geometry_msgs::Twist latest_vel_msg_;
00151 
00152     std::vector<double> a_cos_;
00153     std::vector<double> a_sin_;
00154 
00155     sm_params input_;
00156     sm_result output_;
00157     LDP prev_ldp_scan_;
00158 
00159     // **** methods
00160 
00161     void initParams();
00162     void processScan(LDP& curr_ldp_scan, const ros::Time& time);
00163 
00164     void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
00165                               LDP& ldp);
00166     void PointCloudToLDP(const PointCloudT::ConstPtr& cloud,
00167                                LDP& ldp);
00168 
00169     void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
00170     void cloudCallback (const PointCloudT::ConstPtr& cloud);
00171 
00172     void odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg);
00173     void imuCallback (const sensor_msgs::Imu::ConstPtr& imu_msg);
00174     void velCallback (const geometry_msgs::Twist::ConstPtr& twist_msg);
00175     void velStmpCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg);
00176 
00177     void createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
00178     bool getBaseToLaserTf (const std::string& frame_id);
00179 
00180     bool newKeyframeNeeded(const tf::Transform& d);
00181 
00182     void getPrediction(double& pr_ch_x, double& pr_ch_y,
00183                        double& pr_ch_a, double dt);
00184 
00185     void createTfFromXYTheta(double x, double y, double theta, tf::Transform& t);
00186 };
00187 
00188 } // namespace scan_tools
00189 
00190 #endif // LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H


laser_scan_matcher
Author(s): Ivan Dryanovski , William Morris, Andrea Censi
autogenerated on Thu Jun 6 2019 22:03:30