mpai227

laser_scan_matcher_odom.h

Jul 22nd, 2015
916
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2.  
  3. /*
  4. * Copyright (c) 2011, Ivan Dryanovski, William Morris
  5. * All rights reserved.
  6. *
  7. * *** Modified also to publish horizontal velocities: ***
  8. * *** WARNING: This is not the original laser_scan_matcher from CCNY, ***
  9. * *** it's been slightly modified in a dirty fashion. ***
  10. *
  11. * Redistribution and use in source and binary forms, with or without
  12. * modification, are permitted provided that the following conditions are met:
  13. *
  14. * * Redistributions of source code must retain the above copyright
  15. * notice, this list of conditions and the following disclaimer.
  16. * * Redistributions in binary form must reproduce the above copyright
  17. * notice, this list of conditions and the following disclaimer in the
  18. * documentation and/or other materials provided with the distribution.
  19. * * Neither the name of the CCNY Robotics Lab nor the names of its
  20. * contributors may be used to endorse or promote products derived from
  21. * this software without specific prior written permission.
  22. *
  23. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  24. * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  25. * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  26. * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  27. * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  28. * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  29. * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  30. * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  31. * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  32. * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  33. * POSSIBILITY OF SUCH DAMAGE.
  34. */
  35.  
  36. /* This package uses Canonical Scan Matcher [1], written by
  37. * Andrea Censi
  38. *
  39. * [1] A. Censi, "An ICP variant using a point-to-line metric"
  40. * Proceedings of the IEEE International Conference
  41. * on Robotics and Automation (ICRA), 2008
  42. */
  43.  
  44. #ifndef LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H
  45. #define LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H
  46.  
  47. #include <ros/ros.h>
  48. #include <sensor_msgs/Imu.h>
  49. #include <sensor_msgs/LaserScan.h>
  50. #include <geometry_msgs/Pose2D.h>
  51. #include <geometry_msgs/Twist.h>
  52. #include <nav_msgs/Odometry.h>
  53. #include <tf/transform_datatypes.h>
  54. #include <tf/transform_listener.h>
  55. #include <tf/transform_broadcaster.h>
  56.  
  57. #include <pcl/point_types.h>
  58. #include <pcl/point_cloud.h>
  59. #include <pcl_ros/point_cloud.h>
  60. #include <ros/time.h>
  61.  
  62. #include <csm/csm_all.h> // csm defines min and max, but Eigen complains
  63. #undef min
  64. #undef max
  65.  
  66. namespace scan_tools1
  67. {
  68.  
  69. // inputs
  70. const std::string scan_topic_ = "scan";
  71. const std::string cloud_topic_ = "cloud";
  72. const std::string odom_topic_ = "odom";
  73. const std::string imu_topic_ = "imu";
  74.  
  75. // outputs
  76. const std::string pose_topic_ = "pose2D";
  77.  
  78. typedef pcl::PointXYZ PointT;
  79. typedef pcl::PointCloud<PointT> PointCloudT;
  80.  
  81. class LaserScanMatcher
  82. {
  83. private:
  84.  
  85. // **** ros
  86.  
  87. ros::NodeHandle nh_;
  88. ros::NodeHandle nh_private_;
  89.  
  90. ros::Subscriber scan_subscriber_;
  91. ros::Subscriber cloud_subscriber_;
  92. ros::Subscriber odom_subscriber_;
  93. ros::Subscriber imu_subscriber_;
  94.  
  95. tf::TransformListener tf_listener_;
  96. tf::TransformBroadcaster tf_broadcaster_;
  97.  
  98. tf::Transform base_to_laser_;
  99. tf::Transform laser_to_base_;
  100.  
  101. ros::Publisher test_pub_;
  102. ros::Publisher pose_publisher_;
  103. ros::Publisher vel_publisher_;
  104.  
  105. // **** parameters
  106.  
  107. std::string base_frame_;
  108. std::string fixed_frame_;
  109. double cloud_range_min_;
  110. double cloud_range_max_;
  111. bool publish_tf_;
  112. bool publish_pose_;
  113. bool publish_vel_;
  114.  
  115. bool use_cloud_input_;
  116.  
  117. // **** What predictions are available to speed up the ICP?
  118. // 1) imu - [theta] from imu yaw angle - /odom topic
  119. // 2) odom - [x, y, theta] from wheel odometry - /imu topic
  120. // 3) alpha_beta - [x, y, theta] from simple tracking filter - no topic req.
  121. // If more than one is enabled, priority is imu > odom > alpha_beta
  122.  
  123. bool use_imu_;
  124. bool use_odom_;
  125. bool use_alpha_beta_;
  126.  
  127. double alpha_;
  128. double beta_;
  129.  
  130. // **** state variables
  131.  
  132. bool initialized_;
  133.  
  134. bool received_imu_;
  135. bool received_odom_;
  136.  
  137. boost::mutex mutex_;
  138.  
  139. tf::Transform w2b_; // world-to-base tf (pose of base frame)
  140.  
  141. double v_x_; // velocity estimated by the alpha-beta tracker
  142. double v_y_;
  143. double v_a_;
  144.  
  145. double vel_x_, vel_y_, vel_a_; // simple velocity estimates
  146. double last_vel_x_, last_vel_y_, last_vel_a_;
  147.  
  148. ros::Time last_icp_time_;
  149.  
  150. double latest_imu_yaw_;
  151. double last_imu_yaw_;
  152.  
  153. nav_msgs::Odometry latest_odom_;
  154. nav_msgs::Odometry last_odom_;
  155. nav_msgs::Odometry odom;
  156.  
  157. std::vector<double> a_cos_;
  158. std::vector<double> a_sin_;
  159.  
  160. sm_params input_;
  161. sm_result output_;
  162. LDP prev_ldp_scan_;
  163.  
  164. // **** methods
  165.  
  166. void processScan(LDP& curr_ldp_scan, const ros::Time& time);
  167.  
  168. void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
  169. LDP& ldp);
  170. void PointCloudToLDP(const PointCloudT::ConstPtr& cloud,
  171. LDP& ldp);
  172.  
  173. void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
  174. void cloudCallback (const PointCloudT::ConstPtr& cloud);
  175.  
  176. void odomCallback (const nav_msgs::Odometry::ConstPtr& odom_msg);
  177. void imuCallback (const sensor_msgs::ImuPtr& imu_msg);
  178.  
  179. void createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
  180. bool getBaseToLaserTf (const std::string& frame_id);
  181. void initParams();
  182.  
  183. void getPrediction(double& pr_ch_x, double& pr_ch_y,
  184. double& pr_ch_a, double dt);
  185.  
  186. double getYawFromQuaternion(const geometry_msgs::Quaternion& quaternion);
  187. double getYawFromQuaternion(const tf::Quaternion& quaternion);
  188. void createTfFromXYTheta(double x, double y, double theta, tf::Transform& t);
  189.  
  190. public:
  191.  
  192. LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private);
  193. ~LaserScanMatcher();
  194. };
  195.  
  196. } //namespace
  197.  
  198. #endif // LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×