API tools faq
paste
Advertisement
mpai227

laser_scan_matcher_odom.cpp

mpai227
Jul 22nd, 2015
1,787
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 31.48 KB | None | 0 0
raw download clone embed print report
  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. #include <laser_scan_matcher/laser_scan_matcher_odom.h>
  45. #include <pcl_conversions/pcl_conversions.h>
  46.  
  47. namespace scan_tools1
  48. {
  49.  
  50. LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private):
  51. nh_(nh),
  52. nh_private_(nh_private)
  53. {
  54. ROS_INFO("Starting LaserScanMatcher");
  55.  
  56. // **** init parameters
  57.  
  58. initParams();
  59.  
  60. // **** state variables
  61.  
  62. initialized_ = false;
  63. received_imu_ = false;
  64. received_odom_ = false;
  65.  
  66. w2b_.setIdentity();
  67.  
  68. v_x_ = 0;
  69. v_y_ = 0;
  70. v_a_ = 0;
  71.  
  72. vel_x_ = 0;
  73. vel_y_ = 0;
  74. vel_a_ = 0;
  75. last_vel_x_ = 0;
  76. last_vel_y_ = 0;
  77. last_vel_a_ = 0;
  78.  
  79. input_.laser[0] = 0.0;
  80. input_.laser[1] = 0.0;
  81. input_.laser[2] = 0.0;
  82.  
  83. // *** subscribers
  84.  
  85. if (use_cloud_input_)
  86. {
  87. cloud_subscriber_ = nh_.subscribe(
  88. cloud_topic_, 1, &LaserScanMatcher::cloudCallback, this);
  89.  
  90. }
  91. else
  92. {
  93. scan_subscriber_ = nh_.subscribe(
  94. scan_topic_, 1, &LaserScanMatcher::scanCallback, this);
  95. }
  96.  
  97. if (use_imu_)
  98. {
  99. imu_subscriber_ = nh_.subscribe(
  100. imu_topic_, 1, &LaserScanMatcher::imuCallback, this);
  101. }
  102.  
  103. if (use_odom_)
  104. {
  105. odom_subscriber_ = nh_.subscribe(
  106. odom_topic_, 1, &LaserScanMatcher::odomCallback, this);
  107. }
  108.  
  109. // **** pose publisher
  110. if (publish_pose_)
  111. {
  112. pose_publisher_ = nh_.advertise<geometry_msgs::Pose2D>(
  113. pose_topic_, 5);
  114. }
  115.  
  116. if (publish_vel_)
  117. {
  118. vel_publisher_ = nh_.advertise<nav_msgs::Odometry>(odom_topic_, 100);
  119. }
  120. }
  121.  
  122.  
  123. LaserScanMatcher::~LaserScanMatcher()
  124. {
  125.  
  126. }
  127.  
  128. void LaserScanMatcher::initParams()
  129. {
  130. if (!nh_private_.getParam ("base_frame", base_frame_))
  131. base_frame_ = "base_link";
  132. if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
  133. fixed_frame_ = "world";
  134.  
  135. // **** input type - laser scan, or point clouds?
  136. // if false, will subscrive to LaserScan msgs on /scan.
  137. // if true, will subscrive to PointCloud2 msgs on /cloud
  138.  
  139. if (!nh_private_.getParam ("use_cloud_input", use_cloud_input_))
  140. use_cloud_input_= false;
  141.  
  142. if (use_cloud_input_)
  143. {
  144. if (!nh_private_.getParam ("cloud_range_min", cloud_range_min_))
  145. cloud_range_min_ = 0.1;
  146. if (!nh_private_.getParam ("cloud_range_max", cloud_range_max_))
  147. cloud_range_max_ = 50.0;
  148.  
  149. input_.min_reading = cloud_range_min_;
  150. input_.max_reading = cloud_range_max_;
  151. }
  152.  
  153. // **** What predictions are available to speed up the ICP?
  154. // 1) imu - [theta] from imu yaw angle - /odom topic
  155. // 2) odom - [x, y, theta] from wheel odometry - /imu topic
  156. // 3) alpha_beta - [x, y, theta] from simple tracking filter - no topic req.
  157. // If more than one is enabled, priority is imu > odom > alpha_beta
  158.  
  159. if (!nh_private_.getParam ("use_imu", use_imu_))
  160. use_imu_ = false;
  161. if (!nh_private_.getParam ("use_odom", use_odom_))
  162. use_odom_ = false;
  163. if (!nh_private_.getParam ("use_alpha_beta", use_alpha_beta_))
  164. use_alpha_beta_ = true;
  165. if (!nh_private_.getParam ("publish_vel", publish_vel_))
  166. publish_vel_ = true;
  167.  
  168. // **** How to publish the output?
  169. // tf (fixed_frame->base_frame),
  170. // pose message (pose of base frame in the fixed frame)
  171.  
  172. if (!nh_private_.getParam ("publish_tf", publish_tf_))
  173. publish_tf_ = true;
  174. if (!nh_private_.getParam ("publish_pose", publish_pose_))
  175. publish_pose_ = true;
  176.  
  177. if (!nh_private_.getParam ("alpha", alpha_))
  178. alpha_ = 1.0;
  179. if (!nh_private_.getParam ("beta", beta_))
  180. beta_ = 0.8;
  181.  
  182. // **** CSM parameters - comments copied from algos.h (by Andrea Censi)
  183.  
  184. // Maximum angular displacement between scans
  185. if (!nh_private_.getParam ("max_angular_correction_deg", input_.max_angular_correction_deg))
  186. input_.max_angular_correction_deg = 45.0;
  187.  
  188. // Maximum translation between scans (m)
  189. if (!nh_private_.getParam ("max_linear_correction", input_.max_linear_correction))
  190. input_.max_linear_correction = 0.50;
  191.  
  192. // Maximum ICP cycle iterations
  193. if (!nh_private_.getParam ("max_iterations", input_.max_iterations))
  194. input_.max_iterations = 10;
  195.  
  196. // A threshold for stopping (m)
  197. if (!nh_private_.getParam ("epsilon_xy", input_.epsilon_xy))
  198. input_.epsilon_xy = 0.000001;
  199.  
  200. // A threshold for stopping (rad)
  201. if (!nh_private_.getParam ("epsilon_theta", input_.epsilon_theta))
  202. input_.epsilon_theta = 0.000001;
  203.  
  204. // Maximum distance for a correspondence to be valid
  205. if (!nh_private_.getParam ("max_correspondence_dist", input_.max_correspondence_dist))
  206. input_.max_correspondence_dist = 0.3;
  207.  
  208. // Noise in the scan (m)
  209. if (!nh_private_.getParam ("sigma", input_.sigma))
  210. input_.sigma = 0.010;
  211.  
  212. // Use smart tricks for finding correspondences.
  213. if (!nh_private_.getParam ("use_corr_tricks", input_.use_corr_tricks))
  214. input_.use_corr_tricks = 1;
  215.  
  216. // Restart: Restart if error is over threshold
  217. if (!nh_private_.getParam ("restart", input_.restart))
  218. input_.restart = 0;
  219.  
  220. // Restart: Threshold for restarting
  221. if (!nh_private_.getParam ("restart_threshold_mean_error", input_.restart_threshold_mean_error))
  222. input_.restart_threshold_mean_error = 0.01;
  223.  
  224. // Restart: displacement for restarting. (m)
  225. if (!nh_private_.getParam ("restart_dt", input_.restart_dt))
  226. input_.restart_dt = 1.0;
  227.  
  228. // Restart: displacement for restarting. (rad)
  229. if (!nh_private_.getParam ("restart_dtheta", input_.restart_dtheta))
  230. input_.restart_dtheta = 0.1;
  231.  
  232. // Max distance for staying in the same clustering
  233. if (!nh_private_.getParam ("clustering_threshold", input_.clustering_threshold))
  234. input_.clustering_threshold = 0.25;
  235.  
  236. // Number of neighbour rays used to estimate the orientation
  237. if (!nh_private_.getParam ("orientation_neighbourhood", input_.orientation_neighbourhood))
  238. input_.orientation_neighbourhood = 20;
  239.  
  240. // If 0, it's vanilla ICP
  241. if (!nh_private_.getParam ("use_point_to_line_distance", input_.use_point_to_line_distance))
  242. input_.use_point_to_line_distance = 1;
  243.  
  244. // Discard correspondences based on the angles
  245. if (!nh_private_.getParam ("do_alpha_test", input_.do_alpha_test))
  246. input_.do_alpha_test = 0;
  247.  
  248. // Discard correspondences based on the angles - threshold angle, in degrees
  249. if (!nh_private_.getParam ("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg))
  250. input_.do_alpha_test_thresholdDeg = 20.0;
  251.  
  252. // Percentage of correspondences to consider: if 0.9,
  253. // always discard the top 10% of correspondences with more error
  254. if (!nh_private_.getParam ("outliers_maxPerc", input_.outliers_maxPerc))
  255. input_.outliers_maxPerc = 0.90;
  256.  
  257. // Parameters describing a simple adaptive algorithm for discarding.
  258. // 1) Order the errors.
  259. // 2) Choose the percentile according to outliers_adaptive_order.
  260. // (if it is 0.7, get the 70% percentile)
  261. // 3) Define an adaptive threshold multiplying outliers_adaptive_mult
  262. // with the value of the error at the chosen percentile.
  263. // 4) Discard correspondences over the threshold.
  264. // This is useful to be conservative; yet remove the biggest errors.
  265. if (!nh_private_.getParam ("outliers_adaptive_order", input_.outliers_adaptive_order))
  266. input_.outliers_adaptive_order = 0.7;
  267.  
  268. if (!nh_private_.getParam ("outliers_adaptive_mult", input_.outliers_adaptive_mult))
  269. input_.outliers_adaptive_mult = 2.0;
  270.  
  271. //If you already have a guess of the solution, you can compute the polar angle
  272. // of the points of one scan in the new position. If the polar angle is not a monotone
  273. // function of the readings index, it means that the surface is not visible in the
  274. // next position. If it is not visible, then we don't use it for matching.
  275. if (!nh_private_.getParam ("do_visibility_test", input_.do_visibility_test))
  276. input_.do_visibility_test = 0;
  277.  
  278. // no two points in laser_sens can have the same corr.
  279. if (!nh_private_.getParam ("outliers_remove_doubles", input_.outliers_remove_doubles))
  280. input_.outliers_remove_doubles = 1;
  281.  
  282. // If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov
  283. if (!nh_private_.getParam ("do_compute_covariance", input_.do_compute_covariance))
  284. input_.do_compute_covariance = 0;
  285.  
  286. // Checks that find_correspondences_tricks gives the right answer
  287. if (!nh_private_.getParam ("debug_verify_tricks", input_.debug_verify_tricks))
  288. input_.debug_verify_tricks = 0;
  289.  
  290. // If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the
  291. // incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence.");
  292. if (!nh_private_.getParam ("use_ml_weights", input_.use_ml_weights))
  293. input_.use_ml_weights = 0;
  294.  
  295. // If 1, the field 'readings_sigma' in the second scan is used to weight the
  296. // correspondence by 1/sigma^2
  297. if (!nh_private_.getParam ("use_sigma_weights", input_.use_sigma_weights))
  298. input_.use_sigma_weights = 0;
  299. }
  300.  
  301. void LaserScanMatcher::imuCallback (const sensor_msgs::ImuPtr& imu_msg)
  302. {
  303. boost::mutex::scoped_lock(mutex_);
  304. latest_imu_yaw_ = getYawFromQuaternion(imu_msg->orientation);
  305. if (!received_imu_)
  306. {
  307. last_imu_yaw_ = getYawFromQuaternion(imu_msg->orientation);
  308. received_imu_ = true;
  309. }
  310. }
  311.  
  312. void LaserScanMatcher::odomCallback (const nav_msgs::Odometry::ConstPtr& odom_msg)
  313. {
  314. boost::mutex::scoped_lock(mutex_);
  315. latest_odom_ = *odom_msg;
  316. if (!received_odom_)
  317. {
  318. last_odom_ = *odom_msg;
  319. received_odom_ = true;
  320. }
  321. }
  322.  
  323. void LaserScanMatcher::cloudCallback (const PointCloudT::ConstPtr& cloud)
  324. {
  325. // **** if first scan, cache the tf from base to the scanner
  326. std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud->header);
  327.  
  328. if (!initialized_)
  329. {
  330. // cache the static tf from base to laser
  331. if (!getBaseToLaserTf(cloud->header.frame_id))
  332. {
  333. ROS_WARN("ScanMatcher: Skipping scan");
  334. return;
  335. }
  336.  
  337. PointCloudToLDP(cloud, prev_ldp_scan_);
  338. last_icp_time_ = cloud_header.stamp;
  339. last_imu_yaw_ = latest_imu_yaw_;
  340. last_odom_ = latest_odom_;
  341. initialized_ = true;
  342. }
  343.  
  344. LDP curr_ldp_scan;
  345. PointCloudToLDP(cloud, curr_ldp_scan);
  346. processScan(curr_ldp_scan, cloud_header.stamp);
  347. }
  348.  
  349. void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
  350. {
  351. // **** if first scan, cache the tf from base to the scanner
  352.  
  353. if (!initialized_)
  354. {
  355. createCache(scan_msg); // caches the sin and cos of all angles
  356.  
  357. // cache the static tf from base to laser
  358. if (!getBaseToLaserTf(scan_msg->header.frame_id))
  359. {
  360. ROS_WARN("ScanMatcher: Skipping scan");
  361. return;
  362. }
  363.  
  364. laserScanToLDP(scan_msg, prev_ldp_scan_);
  365. last_icp_time_ = scan_msg->header.stamp;
  366. last_imu_yaw_ = latest_imu_yaw_;
  367. last_odom_ = latest_odom_;
  368. initialized_ = true;
  369. }
  370.  
  371. LDP curr_ldp_scan;
  372. laserScanToLDP(scan_msg, curr_ldp_scan);
  373. processScan(curr_ldp_scan, scan_msg->header.stamp);
  374. }
  375.  
  376. void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
  377. {
  378. struct timeval start, end; // used for timing
  379. gettimeofday(&start, NULL);
  380.  
  381. // CSM is used in the following way:
  382. // The scans are always in the laser frame
  383. // The reference scan (prevLDPcan_) has a pose of 0
  384. // The new scan (currLDPScan) has a pose equal to the movement
  385. // of the laser in the laser frame since the last scan
  386. // The computed correction is then propagated using the tf machinery
  387.  
  388. prev_ldp_scan_->odometry[0] = 0;
  389. prev_ldp_scan_->odometry[1] = 0;
  390. prev_ldp_scan_->odometry[2] = 0;
  391.  
  392. prev_ldp_scan_->estimate[0] = 0;
  393. prev_ldp_scan_->estimate[1] = 0;
  394. prev_ldp_scan_->estimate[2] = 0;
  395.  
  396. prev_ldp_scan_->true_pose[0] = 0;
  397. prev_ldp_scan_->true_pose[1] = 0;
  398. prev_ldp_scan_->true_pose[2] = 0;
  399.  
  400. input_.laser_ref = prev_ldp_scan_;
  401. input_.laser_sens = curr_ldp_scan;
  402.  
  403. // **** estimated change since last scan
  404.  
  405. ros::Time new_icp_time = ros::Time::now();
  406. ros::Duration dur = new_icp_time - last_icp_time_;
  407. double dt = dur.toSec();
  408.  
  409. double pr_ch_x, pr_ch_y, pr_ch_a;
  410. getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);
  411.  
  412. // the predicted change of the laser's position, in the base frame
  413.  
  414. tf::Transform pr_ch;
  415. createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch);
  416.  
  417. // the predicted change of the laser's position, in the laser frame
  418.  
  419. tf::Transform pr_ch_l;
  420. pr_ch_l = laser_to_base_ * pr_ch * base_to_laser_;
  421.  
  422. input_.first_guess[0] = pr_ch_l.getOrigin().getX();
  423. input_.first_guess[1] = pr_ch_l.getOrigin().getY();
  424. input_.first_guess[2] = getYawFromQuaternion(pr_ch_l.getRotation());
  425.  
  426. /*
  427. printf("%f, %f, %f\n", input_.first_guess[0],
  428. input_.first_guess[1],
  429. input_.first_guess[2]);
  430. */
  431. // *** scan match - using point to line icp from CSM
  432.  
  433. sm_icp(&input_, &output_);
  434.  
  435. if (output_.valid)
  436. {
  437. // the correction of the laser's position, in the laser frame
  438.  
  439. //printf("%f, %f, %f\n", output_.x[0], output_.x[1], output_.x[2]);
  440.  
  441. tf::Transform corr_ch_l;
  442. createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
  443.  
  444. // the correction of the base's position, in the world frame
  445.  
  446. tf::Transform corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
  447.  
  448. if(use_alpha_beta_ || publish_vel_)
  449. {
  450. tf::Transform w2b_new = w2b_ * corr_ch;
  451.  
  452. double dx = w2b_new.getOrigin().getX() - w2b_.getOrigin().getX();
  453. double dy = w2b_new.getOrigin().getY() - w2b_.getOrigin().getY();
  454. double da = getYawFromQuaternion(w2b_new.getRotation()) -
  455. getYawFromQuaternion(w2b_.getRotation());
  456.  
  457. double r_x = dx - pr_ch_x;
  458. double r_y = dy - pr_ch_y;
  459. double r_a = da - pr_ch_a;
  460.  
  461. double x = w2b_.getOrigin().getX();
  462. double y = w2b_.getOrigin().getY();
  463. double a = getYawFromQuaternion(w2b_.getRotation());
  464.  
  465. double x_new = (x + pr_ch_x) + alpha_ * r_x;
  466. double y_new = (y + pr_ch_y) + alpha_ * r_y;
  467. double a_new = (a + pr_ch_a) + alpha_ * r_a;
  468.  
  469. createTfFromXYTheta(x_new, y_new, a_new, w2b_);
  470.  
  471. if (dt != 0.0)
  472. {
  473. v_x_ = v_x_ + (beta_ / dt) * r_x;
  474. v_y_ = v_y_ + (beta_ / dt) * r_y;
  475. v_a_ = v_a_ + (beta_ / dt) * r_a;
  476.  
  477. //TODO
  478. //Low-pass filter, 4.9Hz cutoff first order Butterworth
  479. //Warning: non-linear phase, be careful in control applications
  480. //http://www-users.cs.york.ac.uk/~fisher/mkfilter/trad.html
  481. double vx_raw = dx / dt / 1.031426266;
  482. double vy_raw = dy / dt / 1.031426266;
  483. double va_raw = da / dt / 1.031426266;
  484.  
  485. vel_x_ = vx_raw + last_vel_x_ - 0.9390625058 * vel_x_;
  486. vel_y_ = vy_raw + last_vel_y_ - 0.9390625058 * vel_y_;
  487. vel_a_ = va_raw + last_vel_a_ - 0.9390625058 * vel_a_;
  488.  
  489. last_vel_x_ = vx_raw;
  490. last_vel_y_ = vy_raw;
  491. last_vel_a_ = va_raw;
  492. }
  493. }
  494. else
  495. {
  496. w2b_ = w2b_ * corr_ch;
  497. }
  498.  
  499. // **** publish
  500.  
  501. if (publish_pose_)
  502. {
  503. geometry_msgs::Pose2D::Ptr pose_msg;
  504. pose_msg = boost::make_shared<geometry_msgs::Pose2D>();
  505. pose_msg->x = w2b_.getOrigin().getX();
  506. pose_msg->y = w2b_.getOrigin().getY();
  507. pose_msg->theta = getYawFromQuaternion(w2b_.getRotation());
  508. pose_publisher_.publish(pose_msg);
  509. }
  510. if (publish_tf_)
  511. {
  512. tf::StampedTransform transform_msg (w2b_, time, fixed_frame_, base_frame_);
  513. tf_broadcaster_.sendTransform (transform_msg);
  514. }
  515. if (publish_vel_)
  516. {
  517. odom.header.stamp = time;
  518. odom.header.frame_id = fixed_frame_;
  519.  
  520. odom.pose.pose.orientation.x = 0;
  521. odom.pose.pose.orientation.y = 0;
  522. odom.pose.pose.orientation.z = 0;
  523. odom.pose.pose.orientation.w = 1;
  524.  
  525. odom.child_frame_id = base_frame_;
  526. odom.twist.twist.linear.x = vel_x_;
  527. odom.twist.twist.linear.y = vel_y_;
  528. odom.twist.twist.angular.z = vel_a_;
  529.  
  530. vel_publisher_.publish(odom);
  531. }
  532. }
  533. else
  534. {
  535. ROS_WARN("Error in scan matching");
  536.  
  537. v_x_ = 0.0;
  538. v_y_ = 0.0;
  539. v_a_ = 0.0;
  540. }
  541.  
  542. // **** swap old and new
  543.  
  544. ld_free(prev_ldp_scan_);
  545. prev_ldp_scan_ = curr_ldp_scan;
  546. last_icp_time_ = new_icp_time;
  547.  
  548. // **** statistics
  549.  
  550. gettimeofday(&end, NULL);
  551. double dur_total = ((end.tv_sec * 1000000 + end.tv_usec ) -
  552. (start.tv_sec * 1000000 + start.tv_usec)) / 1000.0;
  553. ROS_DEBUG("scan matcher total duration: %.1f ms", dur_total);
  554. }
  555.  
  556. void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud,
  557. LDP& ldp)
  558. {
  559. unsigned int n = cloud->width * cloud->height ;
  560. ldp = ld_alloc_new(n);
  561.  
  562. for (unsigned int i = 0; i < n; i++)
  563. {
  564. // calculate position in laser frame
  565.  
  566. if (is_nan(cloud->points[i].x) || is_nan(cloud->points[i].y))
  567. {
  568. ROS_WARN("Laser Scan Matcher: Cloud input contains NaN values. \
  569. Please use a filtered cloud input.");
  570. }
  571. else
  572. {
  573. double r = sqrt(cloud->points[i].x * cloud->points[i].x +
  574. cloud->points[i].y * cloud->points[i].y);
  575.  
  576. if (r > cloud_range_min_ && r < cloud_range_max_)
  577. {
  578. ldp->valid[i] = 1;
  579. ldp->readings[i] = r;
  580. }
  581. else
  582. {
  583. ldp->valid[i] = 0;
  584. ldp->readings[i] = -1; // for invalid range
  585. }
  586. }
  587.  
  588. ldp->theta[i] = atan2(cloud->points[i].y, cloud->points[i].x);
  589. ldp->cluster[i] = -1;
  590. }
  591.  
  592. ldp->min_theta = ldp->theta[0];
  593. ldp->max_theta = ldp->theta[n-1];
  594.  
  595. ldp->odometry[0] = 0.0;
  596. ldp->odometry[1] = 0.0;
  597. ldp->odometry[2] = 0.0;
  598.  
  599. ldp->true_pose[0] = 0.0;
  600. ldp->true_pose[1] = 0.0;
  601. ldp->true_pose[2] = 0.0;
  602. }
  603.  
  604. void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
  605. LDP& ldp)
  606. {
  607. unsigned int n = scan_msg->ranges.size();
  608. ldp = ld_alloc_new(n);
  609.  
  610. for (unsigned int i = 0; i < n; i++)
  611. {
  612. // calculate position in laser frame
  613.  
  614. double r = scan_msg->ranges[i];
  615.  
  616. if (r > scan_msg->range_min && r < scan_msg->range_max)
  617. {
  618. // fill in laser scan data
  619.  
  620. ldp->valid[i] = 1;
  621. ldp->readings[i] = r;
  622. }
  623. else
  624. {
  625. ldp->valid[i] = 0;
  626. ldp->readings[i] = -1; // for invalid range
  627. }
  628.  
  629. ldp->theta[i] = scan_msg->angle_min + i * scan_msg->angle_increment;
  630.  
  631. ldp->cluster[i] = -1;
  632. }
  633.  
  634. ldp->min_theta = ldp->theta[0];
  635. ldp->max_theta = ldp->theta[n-1];
  636.  
  637. ldp->odometry[0] = 0.0;
  638. ldp->odometry[1] = 0.0;
  639. ldp->odometry[2] = 0.0;
  640.  
  641. ldp->true_pose[0] = 0.0;
  642. ldp->true_pose[1] = 0.0;
  643. ldp->true_pose[2] = 0.0;
  644. }
  645.  
  646. void LaserScanMatcher::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
  647. {
  648. a_cos_.clear();
  649. a_sin_.clear();
  650.  
  651. for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
  652. {
  653. double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
  654. a_cos_.push_back(cos(angle));
  655. a_sin_.push_back(sin(angle));
  656. }
  657.  
  658. input_.min_reading = scan_msg->range_min;
  659. input_.max_reading = scan_msg->range_max;
  660. }
  661.  
  662. bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id)
  663. {
  664. ros::Time t = ros::Time::now();
  665.  
  666. tf::StampedTransform base_to_laser_tf;
  667. try
  668. {
  669. tf_listener_.waitForTransform(
  670. base_frame_, frame_id, t, ros::Duration(1.0));
  671. tf_listener_.lookupTransform (
  672. base_frame_, frame_id, t, base_to_laser_tf);
  673. }
  674. catch (tf::TransformException ex)
  675. {
  676. ROS_WARN("ScanMatcher: Could not get initial laser transform(%s)", ex.what());
  677. return false;
  678. }
  679. base_to_laser_ = base_to_laser_tf;
  680. laser_to_base_ = base_to_laser_.inverse();
  681.  
  682. return true;
  683. }
  684.  
  685. // returns the predicted change in pose (in fixed frame)
  686. // since the last time we did icp
  687. void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y,
  688. double& pr_ch_a, double dt)
  689. {
  690. boost::mutex::scoped_lock(mutex_);
  691.  
  692. // **** base case - no input available, use zero-motion model
  693. pr_ch_x = 0.0;
  694. pr_ch_y = 0.0;
  695. pr_ch_a = 0.0;
  696.  
  697. // **** use alpha-beta tracking (const. vel. model)
  698. if (use_alpha_beta_)
  699. {
  700. // estmate change in fixed frame, using fixed velocity
  701. pr_ch_x = v_x_ * dt; // in fixed frame
  702. pr_ch_y = v_y_ * dt;
  703. pr_ch_a = v_a_ * dt;
  704. }
  705.  
  706. // **** use wheel odometry
  707. if (use_odom_ && received_odom_)
  708. {
  709. pr_ch_x = latest_odom_.pose.pose.position.x -
  710. last_odom_.pose.pose.position.x;
  711.  
  712. pr_ch_y = latest_odom_.pose.pose.position.y -
  713. last_odom_.pose.pose.position.y;
  714.  
  715. pr_ch_a = getYawFromQuaternion(latest_odom_.pose.pose.orientation) -
  716. getYawFromQuaternion(last_odom_.pose.pose.orientation);
  717.  
  718. last_odom_ = latest_odom_;
  719. }
  720.  
  721. // **** use imu
  722. if (use_imu_ && received_imu_)
  723. {
  724. pr_ch_a = latest_imu_yaw_ - last_imu_yaw_;
  725. last_imu_yaw_ = latest_imu_yaw_;
  726. }
  727. }
  728.  
  729. double LaserScanMatcher::getYawFromQuaternion(
  730. const tf::Quaternion& quaternion)
  731. {
  732. double temp, yaw;
  733. tf::Matrix3x3 m(quaternion);
  734. m.getRPY(temp, temp, yaw);
  735. return yaw;
  736. }
  737.  
  738. double LaserScanMatcher::getYawFromQuaternion(
  739. const geometry_msgs::Quaternion& quaternion)
  740. {
  741. tf::Quaternion q;
  742. tf::quaternionMsgToTF(quaternion, q);
  743. return getYawFromQuaternion(q);
  744. }
  745.  
  746. void LaserScanMatcher::createTfFromXYTheta(
  747. double x, double y, double theta, tf::Transform& t)
  748. {
  749. t.setOrigin(tf::Vector3(x, y, 0.0));
  750. tf::Quaternion q;
  751. q.setRPY(0.0, 0.0, theta);
  752. t.setRotation(q);
  753. }
  754.  
  755. } //namespace
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!