amcl_node.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * This library is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public
7  * License as published by the Free Software Foundation; either
8  * version 2.1 of the License, or (at your option) any later version.
9  *
10  * This library is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13  * Lesser General Public License for more details.
14  *
15  * You should have received a copy of the GNU Lesser General Public
16  * License along with this library; if not, write to the Free Software
17  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18  *
19  */
20 
21 /* Author: Brian Gerkey */
22 
23 #include <algorithm>
24 #include <vector>
25 #include <map>
26 #include <cmath>
27 
28 #include <boost/bind.hpp>
29 #include <boost/thread/mutex.hpp>
30 
31 // Signal handling
32 #include <signal.h>
33 
34 #include "amcl/map/map.h"
35 #include "amcl/pf/pf.h"
36 #include "amcl/sensors/amcl_odom.h"
38 
39 #include "ros/assert.h"
40 
41 // roscpp
42 #include "ros/ros.h"
43 
44 // Messages that I need
45 #include "sensor_msgs/LaserScan.h"
46 #include "geometry_msgs/PoseWithCovarianceStamped.h"
47 #include "geometry_msgs/PoseArray.h"
48 #include "geometry_msgs/Pose.h"
49 #include "nav_msgs/GetMap.h"
50 #include "nav_msgs/SetMap.h"
51 #include "std_srvs/Empty.h"
52 
53 // For transform support
55 #include "tf/transform_listener.h"
56 #include "tf/message_filter.h"
57 #include "tf/tf.h"
59 
60 // Dynamic_reconfigure
61 #include "dynamic_reconfigure/server.h"
62 #include "amcl/AMCLConfig.h"
63 
64 // Allows AMCL to run from bag file
65 #include <rosbag/bag.h>
66 #include <rosbag/view.h>
67 #include <boost/foreach.hpp>
68 
69 #define NEW_UNIFORM_SAMPLING 1
70 
71 using namespace amcl;
72 
73 // Pose hypothesis
74 typedef struct
75 {
76  // Total weight (weights sum to 1)
77  double weight;
78 
79  // Mean of pose esimate
81 
82  // Covariance of pose estimate
84 
85 } amcl_hyp_t;
86 
87 static double
88 normalize(double z)
89 {
90  return atan2(sin(z),cos(z));
91 }
92 static double
93 angle_diff(double a, double b)
94 {
95  double d1, d2;
96  a = normalize(a);
97  b = normalize(b);
98  d1 = a-b;
99  d2 = 2*M_PI - fabs(d1);
100  if(d1 > 0)
101  d2 *= -1.0;
102  if(fabs(d1) < fabs(d2))
103  return(d1);
104  else
105  return(d2);
106 }
107 
108 static const std::string scan_topic_ = "scan";
109 
110 class AmclNode
111 {
112  public:
113  AmclNode();
114  ~AmclNode();
115 
122  void runFromBag(const std::string &in_bag_fn, bool trigger_global_localization = false);
123 
124  int process();
125  void savePoseToServer();
126 
127  private:
129 
130  // Use a child class to get access to tf2::Buffer class inside of tf_
132  {
133  inline tf2_ros::Buffer &getBuffer() {return tf2_buffer_;}
134  };
135 
137 
139 
142 
143  // Pose-generating function used to uniformly distribute particles over
144  // the map
145  static pf_vector_t uniformPoseGenerator(void* arg);
146 #if NEW_UNIFORM_SAMPLING
147  static std::vector<std::pair<int,int> > free_space_indices;
148 #endif
149  // Callbacks
150  bool globalLocalizationCallback(std_srvs::Empty::Request& req,
151  std_srvs::Empty::Response& res);
152  bool nomotionUpdateCallback(std_srvs::Empty::Request& req,
153  std_srvs::Empty::Response& res);
154  bool setMapCallback(nav_msgs::SetMap::Request& req,
155  nav_msgs::SetMap::Response& res);
156 
157  void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan);
158  void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
159  void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg);
160  void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg);
161 
162  void handleMapMessage(const nav_msgs::OccupancyGrid& msg);
163  void freeMapDependentMemory();
164  map_t* convertMap( const nav_msgs::OccupancyGrid& map_msg );
165  void updatePoseFromServer();
166  void applyInitialPose();
167 
168  double getYaw(tf::Pose& t);
169 
170  //parameter for what odom to use
171  std::string odom_frame_id_;
172 
173  //paramater to store latest odom pose
175 
176  //parameter for what base to use
177  std::string base_frame_id_;
178  std::string global_frame_id_;
179 
182 
186 
187  geometry_msgs::PoseWithCovarianceStamped last_published_pose;
188 
190  char* mapdata;
191  int sx, sy;
192  double resolution;
193 
197  std::vector< AMCLLaser* > lasers_;
198  std::vector< bool > lasers_update_;
199  std::map< std::string, int > frame_to_laser_;
200 
201  // Particle filter
203  double pf_err_, pf_z_;
204  bool pf_init_;
206  double d_thresh_, a_thresh_;
211 
212  //Nomotion update control
213  bool m_force_update; // used to temporarily let amcl update samples even when no motion occurs...
214 
217 
220 
221  // For slowing play-back when reading directly from a bag file
223 
224  void requestMap();
225 
226  // Helper to get odometric pose from transform system
227  bool getOdomPose(tf::Stamped<tf::Pose>& pose,
228  double& x, double& y, double& yaw,
229  const ros::Time& t, const std::string& f);
230 
231  //time for tolerance on the published transform,
232  //basically defines how long a map->odom transform is good for
234 
240  ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion
244 
248 
249  boost::recursive_mutex configuration_mutex_;
250  dynamic_reconfigure::Server<amcl::AMCLConfig> *dsrv_;
251  amcl::AMCLConfig default_config_;
253 
254  int max_beams_, min_particles_, max_particles_;
255  double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
256  double alpha_slow_, alpha_fast_;
257  double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
258  //beam skip related params
260  double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_;
263  double init_pose_[3];
264  double init_cov_[3];
268 
269  void reconfigureCB(amcl::AMCLConfig &config, uint32_t level);
270 
273  void checkLaserReceived(const ros::TimerEvent& event);
274 };
275 
276 #if NEW_UNIFORM_SAMPLING
277 std::vector<std::pair<int,int> > AmclNode::free_space_indices;
278 #endif
279 
280 #define USAGE "USAGE: amcl"
281 
283 
284 void sigintHandler(int sig)
285 {
286  // Save latest pose as we're shutting down.
287  amcl_node_ptr->savePoseToServer();
288  ros::shutdown();
289 }
290 
291 int
292 main(int argc, char** argv)
293 {
294  ros::init(argc, argv, "amcl");
295  ros::NodeHandle nh;
296 
297  // Override default sigint handler
298  signal(SIGINT, sigintHandler);
299 
300  // Make our node available to sigintHandler
301  amcl_node_ptr.reset(new AmclNode());
302 
303  if (argc == 1)
304  {
305  // run using ROS input
306  ros::spin();
307  }
308  else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag"))
309  {
310  if (argc == 3)
311  {
312  amcl_node_ptr->runFromBag(argv[2]);
313  }
314  else if ((argc == 4) && (std::string(argv[3]) == "--global-localization"))
315  {
316  amcl_node_ptr->runFromBag(argv[2], true);
317  }
318  }
319 
320  // Without this, our boost locks are not shut down nicely
321  amcl_node_ptr.reset();
322 
323  // To quote Morgan, Hooray!
324  return(0);
325 }
326 
328  sent_first_transform_(false),
329  latest_tf_valid_(false),
330  map_(NULL),
331  pf_(NULL),
332  resample_count_(0),
333  odom_(NULL),
334  laser_(NULL),
335  private_nh_("~"),
336  initial_pose_hyp_(NULL),
337  first_map_received_(false),
338  first_reconfigure_call_(true)
339 {
340  boost::recursive_mutex::scoped_lock l(configuration_mutex_);
341 
342  // Grab params off the param server
343  private_nh_.param("use_map_topic", use_map_topic_, false);
344  private_nh_.param("first_map_only", first_map_only_, false);
345 
346  double tmp;
347  private_nh_.param("gui_publish_rate", tmp, -1.0);
349  private_nh_.param("save_pose_rate", tmp, 0.5);
350  save_pose_period = ros::Duration(1.0/tmp);
351 
352  private_nh_.param("laser_min_range", laser_min_range_, -1.0);
353  private_nh_.param("laser_max_range", laser_max_range_, -1.0);
354  private_nh_.param("laser_max_beams", max_beams_, 30);
355  private_nh_.param("min_particles", min_particles_, 100);
356  private_nh_.param("max_particles", max_particles_, 5000);
357  private_nh_.param("kld_err", pf_err_, 0.01);
358  private_nh_.param("kld_z", pf_z_, 0.99);
359  private_nh_.param("odom_alpha1", alpha1_, 0.2);
360  private_nh_.param("odom_alpha2", alpha2_, 0.2);
361  private_nh_.param("odom_alpha3", alpha3_, 0.2);
362  private_nh_.param("odom_alpha4", alpha4_, 0.2);
363  private_nh_.param("odom_alpha5", alpha5_, 0.2);
364 
365  private_nh_.param("do_beamskip", do_beamskip_, false);
366  private_nh_.param("beam_skip_distance", beam_skip_distance_, 0.5);
367  private_nh_.param("beam_skip_threshold", beam_skip_threshold_, 0.3);
368  if (private_nh_.hasParam("beam_skip_error_threshold_"))
369  {
370  private_nh_.param("beam_skip_error_threshold_", beam_skip_error_threshold_);
371  }
372  else
373  {
374  private_nh_.param("beam_skip_error_threshold", beam_skip_error_threshold_, 0.9);
375  }
376 
377  private_nh_.param("laser_z_hit", z_hit_, 0.95);
378  private_nh_.param("laser_z_short", z_short_, 0.1);
379  private_nh_.param("laser_z_max", z_max_, 0.05);
380  private_nh_.param("laser_z_rand", z_rand_, 0.05);
381  private_nh_.param("laser_sigma_hit", sigma_hit_, 0.2);
382  private_nh_.param("laser_lambda_short", lambda_short_, 0.1);
383  private_nh_.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0);
384  std::string tmp_model_type;
385  private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
386  if(tmp_model_type == "beam")
388  else if(tmp_model_type == "likelihood_field")
390  else if(tmp_model_type == "likelihood_field_prob"){
392  }
393  else
394  {
395  ROS_WARN("Unknown laser model type \"%s\"; defaulting to likelihood_field model",
396  tmp_model_type.c_str());
398  }
399 
400  private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));
401  if(tmp_model_type == "diff")
403  else if(tmp_model_type == "omni")
405  else if(tmp_model_type == "diff-corrected")
407  else if(tmp_model_type == "omni-corrected")
409  else
410  {
411  ROS_WARN("Unknown odom model type \"%s\"; defaulting to diff model",
412  tmp_model_type.c_str());
414  }
415 
416  private_nh_.param("update_min_d", d_thresh_, 0.2);
417  private_nh_.param("update_min_a", a_thresh_, M_PI/6.0);
418  private_nh_.param("odom_frame_id", odom_frame_id_, std::string("odom"));
419  private_nh_.param("base_frame_id", base_frame_id_, std::string("base_link"));
420  private_nh_.param("global_frame_id", global_frame_id_, std::string("map"));
421  private_nh_.param("resample_interval", resample_interval_, 2);
422  private_nh_.param("selective_resampling", selective_resampling_, false);
423  double tmp_tol;
424  private_nh_.param("transform_tolerance", tmp_tol, 0.1);
425  private_nh_.param("recovery_alpha_slow", alpha_slow_, 0.001);
426  private_nh_.param("recovery_alpha_fast", alpha_fast_, 0.1);
427  private_nh_.param("tf_broadcast", tf_broadcast_, true);
428 
429  transform_tolerance_.fromSec(tmp_tol);
430 
431  {
432  double bag_scan_period;
433  private_nh_.param("bag_scan_period", bag_scan_period, -1.0);
434  bag_scan_period_.fromSec(bag_scan_period);
435  }
436 
438 
442 
443  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
444  particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);
445  global_loc_srv_ = nh_.advertiseService("global_localization",
447  this);
450 
454  *tf_,
456  100);
457  laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
458  this, _1));
459  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
460 
461  if(use_map_topic_) {
462  map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
463  ROS_INFO("Subscribed to map topic.");
464  } else {
465  requestMap();
466  }
467  m_force_update = false;
468 
469  dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));
470  dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
471  dsrv_->setCallback(cb);
472 
473  // 15s timer to warn on lack of receipt of laser scans, #5209
475  check_laser_timer_ = nh_.createTimer(laser_check_interval_,
476  boost::bind(&AmclNode::checkLaserReceived, this, _1));
477 }
478 
479 void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)
480 {
481  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
482 
483  //we don't want to do anything on the first call
484  //which corresponds to startup
486  {
487  first_reconfigure_call_ = false;
488  default_config_ = config;
489  return;
490  }
491 
492  if(config.restore_defaults) {
493  config = default_config_;
494  //avoid looping
495  config.restore_defaults = false;
496  }
497 
498  d_thresh_ = config.update_min_d;
499  a_thresh_ = config.update_min_a;
500 
501  resample_interval_ = config.resample_interval;
502 
503  laser_min_range_ = config.laser_min_range;
504  laser_max_range_ = config.laser_max_range;
505 
506  gui_publish_period = ros::Duration(1.0/config.gui_publish_rate);
507  save_pose_period = ros::Duration(1.0/config.save_pose_rate);
508 
509  transform_tolerance_.fromSec(config.transform_tolerance);
510 
511  max_beams_ = config.laser_max_beams;
512  alpha1_ = config.odom_alpha1;
513  alpha2_ = config.odom_alpha2;
514  alpha3_ = config.odom_alpha3;
515  alpha4_ = config.odom_alpha4;
516  alpha5_ = config.odom_alpha5;
517 
518  z_hit_ = config.laser_z_hit;
519  z_short_ = config.laser_z_short;
520  z_max_ = config.laser_z_max;
521  z_rand_ = config.laser_z_rand;
522  sigma_hit_ = config.laser_sigma_hit;
523  lambda_short_ = config.laser_lambda_short;
524  laser_likelihood_max_dist_ = config.laser_likelihood_max_dist;
525 
526  if(config.laser_model_type == "beam")
528  else if(config.laser_model_type == "likelihood_field")
530  else if(config.laser_model_type == "likelihood_field_prob")
532 
533  if(config.odom_model_type == "diff")
535  else if(config.odom_model_type == "omni")
537  else if(config.odom_model_type == "diff-corrected")
539  else if(config.odom_model_type == "omni-corrected")
541 
542  if(config.min_particles > config.max_particles)
543  {
544  ROS_WARN("You've set min_particles to be greater than max particles, this isn't allowed so they'll be set to be equal.");
545  config.max_particles = config.min_particles;
546  }
547 
548  min_particles_ = config.min_particles;
549  max_particles_ = config.max_particles;
550  alpha_slow_ = config.recovery_alpha_slow;
551  alpha_fast_ = config.recovery_alpha_fast;
552  tf_broadcast_ = config.tf_broadcast;
553 
554  do_beamskip_= config.do_beamskip;
555  beam_skip_distance_ = config.beam_skip_distance;
556  beam_skip_threshold_ = config.beam_skip_threshold;
557 
561  (void *)map_);
563  pf_err_ = config.kld_err;
564  pf_z_ = config.kld_z;
565  pf_->pop_err = pf_err_;
566  pf_->pop_z = pf_z_;
567 
568  // Initialize the filter
569  pf_vector_t pf_init_pose_mean = pf_vector_zero();
570  pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
571  pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
572  pf_init_pose_mean.v[2] = tf::getYaw(last_published_pose.pose.pose.orientation);
573  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
574  pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0];
575  pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1];
576  pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5];
577  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
578  pf_init_ = false;
579 
580  // Instantiate the sensor objects
581  // Odometry
582  delete odom_;
583  odom_ = new AMCLOdom();
584  ROS_ASSERT(odom_);
586  // Laser
587  delete laser_;
592  sigma_hit_, lambda_short_, 0.0);
594  ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
599  ROS_INFO("Done initializing likelihood field model with probabilities.");
600  }
602  ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
605  ROS_INFO("Done initializing likelihood field model.");
606  }
607 
608  odom_frame_id_ = config.odom_frame_id;
609  base_frame_id_ = config.base_frame_id;
610  global_frame_id_ = config.global_frame_id;
611 
612  delete laser_scan_filter_;
615  *tf_,
617  100);
619  this, _1));
620 
621  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
622 }
623 
624 
625 void AmclNode::runFromBag(const std::string &in_bag_fn, bool trigger_global_localization)
626 {
627  rosbag::Bag bag;
628  bag.open(in_bag_fn, rosbag::bagmode::Read);
629  std::vector<std::string> topics;
630  topics.push_back(std::string("tf"));
631  std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS
632  topics.push_back(scan_topic_name);
633  rosbag::View view(bag, rosbag::TopicQuery(topics));
634 
635  ros::Publisher laser_pub = nh_.advertise<sensor_msgs::LaserScan>(scan_topic_name, 100);
636  ros::Publisher tf_pub = nh_.advertise<tf2_msgs::TFMessage>("/tf", 100);
637 
638  // Sleep for a second to let all subscribers connect
639  ros::WallDuration(1.0).sleep();
640 
642 
643  // Wait for map
644  while (ros::ok())
645  {
646  {
647  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
648  if (map_)
649  {
650  ROS_INFO("Map is ready");
651  break;
652  }
653  }
654  ROS_INFO("Waiting for map...");
656  }
657 
658  if (trigger_global_localization)
659  {
660  std_srvs::Empty empty_srv;
661  globalLocalizationCallback(empty_srv.request, empty_srv.response);
662  }
663 
664  BOOST_FOREACH(rosbag::MessageInstance const msg, view)
665  {
666  if (!ros::ok())
667  {
668  break;
669  }
670 
671  // Process any ros messages or callbacks at this point
673 
674  tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
675  if (tf_msg != NULL)
676  {
677  tf_pub.publish(msg);
678  for (size_t ii=0; ii<tf_msg->transforms.size(); ++ii)
679  {
680  tf_->getBuffer().setTransform(tf_msg->transforms[ii], "rosbag_authority");
681  }
682  continue;
683  }
684 
685  sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate<sensor_msgs::LaserScan>();
686  if (base_scan != NULL)
687  {
688  laser_pub.publish(msg);
689  laser_scan_filter_->add(base_scan);
691  {
693  }
694  continue;
695  }
696 
697  ROS_WARN_STREAM("Unsupported message type" << msg.getTopic());
698  }
699 
700  bag.close();
701 
702  double runtime = (ros::WallTime::now() - start).toSec();
703  ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime);
704 
705  const geometry_msgs::Quaternion & q(last_published_pose.pose.pose.orientation);
706  double yaw, pitch, roll;
707  tf::Matrix3x3(tf::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw,pitch,roll);
708  ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f",
709  last_published_pose.pose.pose.position.x,
710  last_published_pose.pose.pose.position.y,
711  yaw, last_published_pose.header.stamp.toSec()
712  );
713 
714  ros::shutdown();
715 }
716 
717 
719 {
720  // We need to apply the last transform to the latest odom pose to get
721  // the latest map pose to store. We'll take the covariance from
722  // last_published_pose.
724  double yaw,pitch,roll;
725  map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
726 
727  ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() );
728 
729  private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
730  private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
731  private_nh_.setParam("initial_pose_a", yaw);
732  private_nh_.setParam("initial_cov_xx",
733  last_published_pose.pose.covariance[6*0+0]);
734  private_nh_.setParam("initial_cov_yy",
735  last_published_pose.pose.covariance[6*1+1]);
736  private_nh_.setParam("initial_cov_aa",
737  last_published_pose.pose.covariance[6*5+5]);
738 }
739 
741 {
742  init_pose_[0] = 0.0;
743  init_pose_[1] = 0.0;
744  init_pose_[2] = 0.0;
745  init_cov_[0] = 0.5 * 0.5;
746  init_cov_[1] = 0.5 * 0.5;
747  init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);
748  // Check for NAN on input from param server, #5239
749  double tmp_pos;
750  private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]);
751  if(!std::isnan(tmp_pos))
752  init_pose_[0] = tmp_pos;
753  else
754  ROS_WARN("ignoring NAN in initial pose X position");
755  private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]);
756  if(!std::isnan(tmp_pos))
757  init_pose_[1] = tmp_pos;
758  else
759  ROS_WARN("ignoring NAN in initial pose Y position");
760  private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]);
761  if(!std::isnan(tmp_pos))
762  init_pose_[2] = tmp_pos;
763  else
764  ROS_WARN("ignoring NAN in initial pose Yaw");
765  private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]);
766  if(!std::isnan(tmp_pos))
767  init_cov_[0] =tmp_pos;
768  else
769  ROS_WARN("ignoring NAN in initial covariance XX");
770  private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]);
771  if(!std::isnan(tmp_pos))
772  init_cov_[1] = tmp_pos;
773  else
774  ROS_WARN("ignoring NAN in initial covariance YY");
775  private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]);
776  if(!std::isnan(tmp_pos))
777  init_cov_[2] = tmp_pos;
778  else
779  ROS_WARN("ignoring NAN in initial covariance AA");
780 }
781 
782 void
784 {
786  if(d > laser_check_interval_)
787  {
788  ROS_WARN("No laser scan received (and thus no pose updates have been published) for %f seconds. Verify that data is being published on the %s topic.",
789  d.toSec(),
791  }
792 }
793 
794 void
796 {
797  boost::recursive_mutex::scoped_lock ml(configuration_mutex_);
798 
799  // get map via RPC
800  nav_msgs::GetMap::Request req;
801  nav_msgs::GetMap::Response resp;
802  ROS_INFO("Requesting the map...");
803  while(!ros::service::call("static_map", req, resp))
804  {
805  ROS_WARN("Request for map failed; trying again...");
806  ros::Duration d(0.5);
807  d.sleep();
808  }
809  handleMapMessage( resp.map );
810 }
811 
812 void
813 AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg)
814 {
816  return;
817  }
818 
819  handleMapMessage( *msg );
820 
821  first_map_received_ = true;
822 }
823 
824 void
825 AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
826 {
827  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
828 
829  ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
830  msg.info.width,
831  msg.info.height,
832  msg.info.resolution);
833 
834  if(msg.header.frame_id != global_frame_id_)
835  ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics",
836  msg.header.frame_id.c_str(),
837  global_frame_id_.c_str());
838 
840  // Clear queued laser objects because they hold pointers to the existing
841  // map, #5202.
842  lasers_.clear();
843  lasers_update_.clear();
844  frame_to_laser_.clear();
845 
846  map_ = convertMap(msg);
847 
848 #if NEW_UNIFORM_SAMPLING
849  // Index of free space
850  free_space_indices.resize(0);
851  for(int i = 0; i < map_->size_x; i++)
852  for(int j = 0; j < map_->size_y; j++)
853  if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
854  free_space_indices.push_back(std::make_pair(i,j));
855 #endif
856  // Create the particle filter
860  (void *)map_);
862  pf_->pop_err = pf_err_;
863  pf_->pop_z = pf_z_;
864 
865  // Initialize the filter
867  pf_vector_t pf_init_pose_mean = pf_vector_zero();
868  pf_init_pose_mean.v[0] = init_pose_[0];
869  pf_init_pose_mean.v[1] = init_pose_[1];
870  pf_init_pose_mean.v[2] = init_pose_[2];
871  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
872  pf_init_pose_cov.m[0][0] = init_cov_[0];
873  pf_init_pose_cov.m[1][1] = init_cov_[1];
874  pf_init_pose_cov.m[2][2] = init_cov_[2];
875  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
876  pf_init_ = false;
877 
878  // Instantiate the sensor objects
879  // Odometry
880  delete odom_;
881  odom_ = new AMCLOdom();
882  ROS_ASSERT(odom_);
884  // Laser
885  delete laser_;
890  sigma_hit_, lambda_short_, 0.0);
892  ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
897  ROS_INFO("Done initializing likelihood field model.");
898  }
899  else
900  {
901  ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
904  ROS_INFO("Done initializing likelihood field model.");
905  }
906 
907  // In case the initial pose message arrived before the first map,
908  // try to apply the initial pose now that the map has arrived.
910 
911 }
912 
913 void
915 {
916  if( map_ != NULL ) {
917  map_free( map_ );
918  map_ = NULL;
919  }
920  if( pf_ != NULL ) {
921  pf_free( pf_ );
922  pf_ = NULL;
923  }
924  delete odom_;
925  odom_ = NULL;
926  delete laser_;
927  laser_ = NULL;
928 }
929 
934 map_t*
935 AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg )
936 {
937  map_t* map = map_alloc();
938  ROS_ASSERT(map);
939 
940  map->size_x = map_msg.info.width;
941  map->size_y = map_msg.info.height;
942  map->scale = map_msg.info.resolution;
943  map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
944  map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
945  // Convert to player format
946  map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
947  ROS_ASSERT(map->cells);
948  for(int i=0;i<map->size_x * map->size_y;i++)
949  {
950  if(map_msg.data[i] == 0)
951  map->cells[i].occ_state = -1;
952  else if(map_msg.data[i] == 100)
953  map->cells[i].occ_state = +1;
954  else
955  map->cells[i].occ_state = 0;
956  }
957 
958  return map;
959 }
960 
962 {
963  delete dsrv_;
965  delete laser_scan_filter_;
966  delete laser_scan_sub_;
967  delete tfb_;
968  delete tf_;
969  // TODO: delete everything allocated in constructor
970 }
971 
972 bool
974  double& x, double& y, double& yaw,
975  const ros::Time& t, const std::string& f)
976 {
977  // Get the robot's pose
979  tf::Vector3(0,0,0)), t, f);
980  try
981  {
982  this->tf_->transformPose(odom_frame_id_, ident, odom_pose);
983  }
984  catch(tf::TransformException e)
985  {
986  ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
987  return false;
988  }
989  x = odom_pose.getOrigin().x();
990  y = odom_pose.getOrigin().y();
991  double pitch,roll;
992  odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);
993 
994  return true;
995 }
996 
997 
1000 {
1001  map_t* map = (map_t*)arg;
1002 #if NEW_UNIFORM_SAMPLING
1003  unsigned int rand_index = drand48() * free_space_indices.size();
1004  std::pair<int,int> free_point = free_space_indices[rand_index];
1005  pf_vector_t p;
1006  p.v[0] = MAP_WXGX(map, free_point.first);
1007  p.v[1] = MAP_WYGY(map, free_point.second);
1008  p.v[2] = drand48() * 2 * M_PI - M_PI;
1009 #else
1010  double min_x, max_x, min_y, max_y;
1011 
1012  min_x = (map->size_x * map->scale)/2.0 - map->origin_x;
1013  max_x = (map->size_x * map->scale)/2.0 + map->origin_x;
1014  min_y = (map->size_y * map->scale)/2.0 - map->origin_y;
1015  max_y = (map->size_y * map->scale)/2.0 + map->origin_y;
1016 
1017  pf_vector_t p;
1018 
1019  ROS_DEBUG("Generating new uniform sample");
1020  for(;;)
1021  {
1022  p.v[0] = min_x + drand48() * (max_x - min_x);
1023  p.v[1] = min_y + drand48() * (max_y - min_y);
1024  p.v[2] = drand48() * 2 * M_PI - M_PI;
1025  // Check that it's a free cell
1026  int i,j;
1027  i = MAP_GXWX(map, p.v[0]);
1028  j = MAP_GYWY(map, p.v[1]);
1029  if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
1030  break;
1031  }
1032 #endif
1033  return p;
1034 }
1035 
1036 bool
1037 AmclNode::globalLocalizationCallback(std_srvs::Empty::Request& req,
1038  std_srvs::Empty::Response& res)
1039 {
1040  if( map_ == NULL ) {
1041  return true;
1042  }
1043  boost::recursive_mutex::scoped_lock gl(configuration_mutex_);
1044  ROS_INFO("Initializing with uniform distribution");
1046  (void *)map_);
1047  ROS_INFO("Global initialisation done!");
1048  pf_init_ = false;
1049  return true;
1050 }
1051 
1052 // force nomotion updates (amcl updating without requiring motion)
1053 bool
1054 AmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req,
1055  std_srvs::Empty::Response& res)
1056 {
1057  m_force_update = true;
1058  //ROS_INFO("Requesting no-motion update");
1059  return true;
1060 }
1061 
1062 bool
1063 AmclNode::setMapCallback(nav_msgs::SetMap::Request& req,
1064  nav_msgs::SetMap::Response& res)
1065 {
1066  handleMapMessage(req.map);
1067  handleInitialPoseMessage(req.initial_pose);
1068  res.success = true;
1069  return true;
1070 }
1071 
1072 void
1073 AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
1074 {
1076  if( map_ == NULL ) {
1077  return;
1078  }
1079  boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
1080  int laser_index = -1;
1081 
1082  // Do we have the base->base_laser Tx yet?
1083  if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
1084  {
1085  ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
1086  lasers_.push_back(new AMCLLaser(*laser_));
1087  lasers_update_.push_back(true);
1088  laser_index = frame_to_laser_.size();
1089 
1091  tf::Vector3(0,0,0)),
1092  ros::Time(), laser_scan->header.frame_id);
1093  tf::Stamped<tf::Pose> laser_pose;
1094  try
1095  {
1096  this->tf_->transformPose(base_frame_id_, ident, laser_pose);
1097  }
1098  catch(tf::TransformException& e)
1099  {
1100  ROS_ERROR("Couldn't transform from %s to %s, "
1101  "even though the message notifier is in use",
1102  laser_scan->header.frame_id.c_str(),
1103  base_frame_id_.c_str());
1104  return;
1105  }
1106 
1107  pf_vector_t laser_pose_v;
1108  laser_pose_v.v[0] = laser_pose.getOrigin().x();
1109  laser_pose_v.v[1] = laser_pose.getOrigin().y();
1110  // laser mounting angle gets computed later -> set to 0 here!
1111  laser_pose_v.v[2] = 0;
1112  lasers_[laser_index]->SetLaserPose(laser_pose_v);
1113  ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
1114  laser_pose_v.v[0],
1115  laser_pose_v.v[1],
1116  laser_pose_v.v[2]);
1117 
1118  frame_to_laser_[laser_scan->header.frame_id] = laser_index;
1119  } else {
1120  // we have the laser pose, retrieve laser index
1121  laser_index = frame_to_laser_[laser_scan->header.frame_id];
1122  }
1123 
1124  // Where was the robot when this scan was taken?
1125  pf_vector_t pose;
1126  if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
1127  laser_scan->header.stamp, base_frame_id_))
1128  {
1129  ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
1130  return;
1131  }
1132 
1133 
1134  pf_vector_t delta = pf_vector_zero();
1135 
1136  if(pf_init_)
1137  {
1138  // Compute change in pose
1139  //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
1140  delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
1141  delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
1142  delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
1143 
1144  // See if we should update the filter
1145  bool update = fabs(delta.v[0]) > d_thresh_ ||
1146  fabs(delta.v[1]) > d_thresh_ ||
1147  fabs(delta.v[2]) > a_thresh_;
1148  update = update || m_force_update;
1149  m_force_update=false;
1150 
1151  // Set the laser update flags
1152  if(update)
1153  for(unsigned int i=0; i < lasers_update_.size(); i++)
1154  lasers_update_[i] = true;
1155  }
1156 
1157  bool force_publication = false;
1158  if(!pf_init_)
1159  {
1160  // Pose at last filter update
1161  pf_odom_pose_ = pose;
1162 
1163  // Filter is now initialized
1164  pf_init_ = true;
1165 
1166  // Should update sensor data
1167  for(unsigned int i=0; i < lasers_update_.size(); i++)
1168  lasers_update_[i] = true;
1169 
1170  force_publication = true;
1171 
1172  resample_count_ = 0;
1173  }
1174  // If the robot has moved, update the filter
1175  else if(pf_init_ && lasers_update_[laser_index])
1176  {
1177  //printf("pose\n");
1178  //pf_vector_fprintf(pose, stdout, "%.3f");
1179 
1180  AMCLOdomData odata;
1181  odata.pose = pose;
1182  // HACK
1183  // Modify the delta in the action data so the filter gets
1184  // updated correctly
1185  odata.delta = delta;
1186 
1187  // Use the action data to update the filter
1188  odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
1189 
1190  // Pose at last filter update
1191  //this->pf_odom_pose = pose;
1192  }
1193 
1194  bool resampled = false;
1195  // If the robot has moved, update the filter
1196  if(lasers_update_[laser_index])
1197  {
1198  AMCLLaserData ldata;
1199  ldata.sensor = lasers_[laser_index];
1200  ldata.range_count = laser_scan->ranges.size();
1201 
1202  // To account for lasers that are mounted upside-down, we determine the
1203  // min, max, and increment angles of the laser in the base frame.
1204  //
1205  // Construct min and max angles of laser, in the base_link frame.
1206  tf::Quaternion q;
1207  q.setRPY(0.0, 0.0, laser_scan->angle_min);
1208  tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
1209  laser_scan->header.frame_id);
1210  q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
1211  tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
1212  laser_scan->header.frame_id);
1213  try
1214  {
1215  tf_->transformQuaternion(base_frame_id_, min_q, min_q);
1216  tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
1217  }
1218  catch(tf::TransformException& e)
1219  {
1220  ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
1221  e.what());
1222  return;
1223  }
1224 
1225  double angle_min = tf::getYaw(min_q);
1226  double angle_increment = tf::getYaw(inc_q) - angle_min;
1227 
1228  // wrapping angle to [-pi .. pi]
1229  angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
1230 
1231  ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
1232 
1233  // Apply range min/max thresholds, if the user supplied them
1234  if(laser_max_range_ > 0.0)
1235  ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
1236  else
1237  ldata.range_max = laser_scan->range_max;
1238  double range_min;
1239  if(laser_min_range_ > 0.0)
1240  range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
1241  else
1242  range_min = laser_scan->range_min;
1243  // The AMCLLaserData destructor will free this memory
1244  ldata.ranges = new double[ldata.range_count][2];
1245  ROS_ASSERT(ldata.ranges);
1246  for(int i=0;i<ldata.range_count;i++)
1247  {
1248  // amcl doesn't (yet) have a concept of min range. So we'll map short
1249  // readings to max range.
1250  if(laser_scan->ranges[i] <= range_min)
1251  ldata.ranges[i][0] = ldata.range_max;
1252  else
1253  ldata.ranges[i][0] = laser_scan->ranges[i];
1254  // Compute bearing
1255  ldata.ranges[i][1] = angle_min +
1256  (i * angle_increment);
1257  }
1258 
1259  lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
1260 
1261  lasers_update_[laser_index] = false;
1262 
1263  pf_odom_pose_ = pose;
1264 
1265  // Resample the particles
1267  {
1269  resampled = true;
1270  }
1271 
1272  pf_sample_set_t* set = pf_->sets + pf_->current_set;
1273  ROS_DEBUG("Num samples: %d\n", set->sample_count);
1274 
1275  // Publish the resulting cloud
1276  // TODO: set maximum rate for publishing
1277  if (!m_force_update) {
1278  geometry_msgs::PoseArray cloud_msg;
1279  cloud_msg.header.stamp = ros::Time::now();
1280  cloud_msg.header.frame_id = global_frame_id_;
1281  cloud_msg.poses.resize(set->sample_count);
1282  for(int i=0;i<set->sample_count;i++)
1283  {
1285  tf::Vector3(set->samples[i].pose.v[0],
1286  set->samples[i].pose.v[1], 0)),
1287  cloud_msg.poses[i]);
1288  }
1289  particlecloud_pub_.publish(cloud_msg);
1290  }
1291  }
1292 
1293  if(resampled || force_publication)
1294  {
1295  // Read out the current hypotheses
1296  double max_weight = 0.0;
1297  int max_weight_hyp = -1;
1298  std::vector<amcl_hyp_t> hyps;
1299  hyps.resize(pf_->sets[pf_->current_set].cluster_count);
1300  for(int hyp_count = 0;
1301  hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
1302  {
1303  double weight;
1304  pf_vector_t pose_mean;
1305  pf_matrix_t pose_cov;
1306  if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
1307  {
1308  ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
1309  break;
1310  }
1311 
1312  hyps[hyp_count].weight = weight;
1313  hyps[hyp_count].pf_pose_mean = pose_mean;
1314  hyps[hyp_count].pf_pose_cov = pose_cov;
1315 
1316  if(hyps[hyp_count].weight > max_weight)
1317  {
1318  max_weight = hyps[hyp_count].weight;
1319  max_weight_hyp = hyp_count;
1320  }
1321  }
1322 
1323  if(max_weight > 0.0)
1324  {
1325  ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
1326  hyps[max_weight_hyp].pf_pose_mean.v[0],
1327  hyps[max_weight_hyp].pf_pose_mean.v[1],
1328  hyps[max_weight_hyp].pf_pose_mean.v[2]);
1329 
1330  /*
1331  puts("");
1332  pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
1333  puts("");
1334  */
1335 
1336  geometry_msgs::PoseWithCovarianceStamped p;
1337  // Fill in the header
1338  p.header.frame_id = global_frame_id_;
1339  p.header.stamp = laser_scan->header.stamp;
1340  // Copy in the pose
1341  p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
1342  p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
1343  tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
1344  p.pose.pose.orientation);
1345  // Copy in the covariance, converting from 3-D to 6-D
1346  pf_sample_set_t* set = pf_->sets + pf_->current_set;
1347  for(int i=0; i<2; i++)
1348  {
1349  for(int j=0; j<2; j++)
1350  {
1351  // Report the overall filter covariance, rather than the
1352  // covariance for the highest-weight cluster
1353  //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
1354  p.pose.covariance[6*i+j] = set->cov.m[i][j];
1355  }
1356  }
1357  // Report the overall filter covariance, rather than the
1358  // covariance for the highest-weight cluster
1359  //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
1360  p.pose.covariance[6*5+5] = set->cov.m[2][2];
1361 
1362  /*
1363  printf("cov:\n");
1364  for(int i=0; i<6; i++)
1365  {
1366  for(int j=0; j<6; j++)
1367  printf("%6.3f ", p.covariance[6*i+j]);
1368  puts("");
1369  }
1370  */
1371 
1372  pose_pub_.publish(p);
1373  last_published_pose = p;
1374 
1375  ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
1376  hyps[max_weight_hyp].pf_pose_mean.v[0],
1377  hyps[max_weight_hyp].pf_pose_mean.v[1],
1378  hyps[max_weight_hyp].pf_pose_mean.v[2]);
1379 
1380  // subtracting base to odom from map to base and send map to odom instead
1381  tf::Stamped<tf::Pose> odom_to_map;
1382  try
1383  {
1384  tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
1385  tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
1386  hyps[max_weight_hyp].pf_pose_mean.v[1],
1387  0.0));
1388  tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
1389  laser_scan->header.stamp,
1390  base_frame_id_);
1392  tmp_tf_stamped,
1393  odom_to_map);
1394  }
1395  catch(tf::TransformException)
1396  {
1397  ROS_DEBUG("Failed to subtract base to odom transform");
1398  return;
1399  }
1400 
1401  latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
1402  tf::Point(odom_to_map.getOrigin()));
1403  latest_tf_valid_ = true;
1404 
1405  if (tf_broadcast_ == true)
1406  {
1407  // We want to send a transform that is good up until a
1408  // tolerance time so that odom can be used
1409  ros::Time transform_expiration = (laser_scan->header.stamp +
1411  tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
1412  transform_expiration,
1414  this->tfb_->sendTransform(tmp_tf_stamped);
1415  sent_first_transform_ = true;
1416  }
1417  }
1418  else
1419  {
1420  ROS_ERROR("No pose!");
1421  }
1422  }
1423  else if(latest_tf_valid_)
1424  {
1425  if (tf_broadcast_ == true)
1426  {
1427  // Nothing changed, so we'll just republish the last transform, to keep
1428  // everybody happy.
1429  ros::Time transform_expiration = (laser_scan->header.stamp +
1431  tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
1432  transform_expiration,
1434  this->tfb_->sendTransform(tmp_tf_stamped);
1435  }
1436 
1437  // Is it time to save our last pose to the param server
1438  ros::Time now = ros::Time::now();
1439  if((save_pose_period.toSec() > 0.0) &&
1441  {
1442  this->savePoseToServer();
1443  save_pose_last_time = now;
1444  }
1445  }
1446 
1447 }
1448 
1449 double
1451 {
1452  double yaw, pitch, roll;
1453  t.getBasis().getEulerYPR(yaw,pitch,roll);
1454  return yaw;
1455 }
1456 
1457 void
1458 AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
1459 {
1461 }
1462 
1463 void
1464 AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg)
1465 {
1466  boost::recursive_mutex::scoped_lock prl(configuration_mutex_);
1467  if(msg.header.frame_id == "")
1468  {
1469  // This should be removed at some point
1470  ROS_WARN("Received initial pose with empty frame_id. You should always supply a frame_id.");
1471  }
1472  // We only accept initial pose estimates in the global frame, #5148.
1473  else if(tf_->resolve(msg.header.frame_id) != tf_->resolve(global_frame_id_))
1474  {
1475  ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
1476  msg.header.frame_id.c_str(),
1477  global_frame_id_.c_str());
1478  return;
1479  }
1480 
1481  // In case the client sent us a pose estimate in the past, integrate the
1482  // intervening odometric change.
1483  tf::StampedTransform tx_odom;
1484  try
1485  {
1486  ros::Time now = ros::Time::now();
1487  // wait a little for the latest tf to become available
1488  tf_->waitForTransform(base_frame_id_, msg.header.stamp,
1489  base_frame_id_, now,
1491  tf_->lookupTransform(base_frame_id_, msg.header.stamp,
1492  base_frame_id_, now,
1493  odom_frame_id_, tx_odom);
1494  }
1495  catch(tf::TransformException e)
1496  {
1497  // If we've never sent a transform, then this is normal, because the
1498  // global_frame_id_ frame doesn't exist. We only care about in-time
1499  // transformation for on-the-move pose-setting, so ignoring this
1500  // startup condition doesn't really cost us anything.
1502  ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
1503  tx_odom.setIdentity();
1504  }
1505 
1506  tf::Pose pose_old, pose_new;
1507  tf::poseMsgToTF(msg.pose.pose, pose_old);
1508  pose_new = pose_old * tx_odom;
1509 
1510  // Transform into the global frame
1511 
1512  ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
1513  ros::Time::now().toSec(),
1514  pose_new.getOrigin().x(),
1515  pose_new.getOrigin().y(),
1516  getYaw(pose_new));
1517  // Re-initialize the filter
1518  pf_vector_t pf_init_pose_mean = pf_vector_zero();
1519  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
1520  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
1521  pf_init_pose_mean.v[2] = getYaw(pose_new);
1522  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
1523  // Copy in the covariance, converting from 6-D to 3-D
1524  for(int i=0; i<2; i++)
1525  {
1526  for(int j=0; j<2; j++)
1527  {
1528  pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j];
1529  }
1530  }
1531  pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5];
1532 
1533  delete initial_pose_hyp_;
1534  initial_pose_hyp_ = new amcl_hyp_t();
1535  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
1536  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
1537  applyInitialPose();
1538 }
1539 
1545 void
1547 {
1548  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
1549  if( initial_pose_hyp_ != NULL && map_ != NULL ) {
1551  pf_init_ = false;
1552 
1553  delete initial_pose_hyp_;
1554  initial_pose_hyp_ = NULL;
1555  }
1556 }
d
ros::ServiceServer set_map_srv_
Definition: amcl_node.cpp:241
bool globalLocalizationCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: amcl_node.cpp:1037
bool tf_broadcast_
Definition: amcl_node.cpp:266
#define MAP_GXWX(map, x)
Definition: map.h:137
double beam_skip_distance_
Definition: amcl_node.cpp:260
static pf_vector_t uniformPoseGenerator(void *arg)
Definition: amcl_node.cpp:999
ros::Time last_laser_received_ts_
Definition: amcl_node.cpp:271
int min_particles_
Definition: amcl_node.cpp:254
ros::Publisher pose_pub_
Definition: amcl_node.cpp:237
double beam_skip_error_threshold_
Definition: amcl_node.cpp:260
boost::shared_ptr< AmclNode > amcl_node_ptr
Definition: amcl_node.cpp:282
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
double pop_err
Definition: pf.h:118
bool first_map_only_
Definition: amcl_node.cpp:181
double v[3]
Definition: pf_vector.h:40
void publish(const boost::shared_ptr< M > &message) const
std::string global_frame_id_
Definition: amcl_node.cpp:178
dynamic_reconfigure::Server< amcl::AMCLConfig > * dsrv_
Definition: amcl_node.cpp:250
void applyInitialPose()
Definition: amcl_node.cpp:1546
void open(std::string const &filename, uint32_t mode=bagmode::Read)
ros::Subscriber initial_pose_sub_
Definition: amcl_node.cpp:196
std::map< std::string, int > frame_to_laser_
Definition: amcl_node.cpp:199
Definition: pf.h:112
#define MAP_VALID(map, i, j)
Definition: map.h:141
map_cell_t * cells
Definition: map.h:73
double pf_err_
Definition: amcl_node.cpp:203
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
ros::Publisher particlecloud_pub_
Definition: amcl_node.cpp:238
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
double laser_max_range_
Definition: amcl_node.cpp:210
pf_vector_t(* pf_init_model_fn_t)(void *init_data)
Definition: pf.h:45
int resample_count_
Definition: amcl_node.cpp:208
void getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) const
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
void add(const MEvent &evt)
static double getYaw(const Quaternion &bt_q)
Definition: map.h:46
void checkLaserReceived(const ros::TimerEvent &event)
Definition: amcl_node.cpp:783
bool call(const std::string &service_name, MReq &req, MRes &res)
std::string base_frame_id_
Definition: amcl_node.cpp:177
bool sleep() const
static double normalize(double z)
Definition: amcl_node.cpp:88
void mapReceived(const nav_msgs::OccupancyGridConstPtr &msg)
Definition: amcl_node.cpp:813
pf_vector_t pf_vector_zero()
Definition: pf_vector.c:38
double beam_skip_threshold_
Definition: amcl_node.cpp:260
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
pf_t * pf_alloc(int min_samples, int max_samples, double alpha_slow, double alpha_fast, pf_init_model_fn_t random_pose_fn, void *random_pose_data)
Definition: pf.c:45
tf::TransformBroadcaster * tfb_
Definition: amcl_node.cpp:128
int size_y
Definition: map.h:70
int max_particles_
Definition: amcl_node.cpp:254
laser_model_t
Definition: amcl_laser.h:38
#define MAP_INDEX(map, i, j)
Definition: map.h:144
double origin_x
Definition: map.h:64
double z_max_
Definition: amcl_node.cpp:257
double alpha4_
Definition: amcl_node.cpp:255
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
double z_short_
Definition: amcl_node.cpp:257
ros::Time last_cloud_pub_time
Definition: amcl_node.cpp:219
#define MAP_WXGX(map, i)
Definition: map.h:133
int resample_interval_
Definition: amcl_node.cpp:207
void updatePoseFromServer()
Definition: amcl_node.cpp:740
int main(int argc, char **argv)
Definition: amcl_node.cpp:292
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
double scale
Definition: map.h:67
std::string resolve(const std::string &frame_name)
#define MAP_GYWY(map, y)
Definition: map.h:138
void sigintHandler(int sig)
Definition: amcl_node.cpp:284
void SetModelBeam(double z_hit, double z_short, double z_max, double z_rand, double sigma_hit, double labda_short, double chi_outlier)
Definition: amcl_laser.cpp:65
virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data)
Definition: amcl_odom.cpp:113
odom_model_t odom_model_type_
Definition: amcl_node.cpp:262
#define ROS_WARN(...)
void savePoseToServer()
Definition: amcl_node.cpp:718
static tf::Quaternion createIdentityQuaternion()
void close()
double alpha3_
Definition: amcl_node.cpp:255
#define MAP_WYGY(map, j)
Definition: map.h:134
TransformListenerWrapper * tf_
Definition: amcl_node.cpp:136
double alpha1_
Definition: amcl_node.cpp:255
int size_x
Definition: map.h:70
double init_pose_[3]
Definition: amcl_node.cpp:263
tf::MessageFilter< sensor_msgs::LaserScan > * laser_scan_filter_
Definition: amcl_node.cpp:195
ROSCPP_DECL void spin(Spinner &spinner)
void SetModel(odom_model_t type, double alpha1, double alpha2, double alpha3, double alpha4, double alpha5=0)
Definition: amcl_odom.cpp:96
void setIdentity()
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
Definition: pf.c:135
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
std::vector< AMCLLaser * > lasers_
Definition: amcl_node.cpp:197
int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight, pf_vector_t *mean, pf_matrix_t *cov)
Definition: pf.c:730
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
ROSCPP_DECL CallbackQueue * getGlobalCallbackQueue()
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool selective_resampling_
Definition: amcl_node.cpp:267
pf_vector_t pf_pose_mean
Definition: amcl_node.cpp:80
AMCLLaser * laser_
Definition: amcl_node.cpp:216
ros::Subscriber initial_pose_sub_old_
Definition: amcl_node.cpp:242
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
double origin_y
Definition: map.h:64
ros::ServiceServer nomotion_update_srv_
Definition: amcl_node.cpp:240
double getYaw(tf::Pose &t)
Definition: amcl_node.cpp:1450
int occ_state
Definition: map.h:49
std::string odom_frame_id_
Definition: amcl_node.cpp:171
void transformQuaternion(const std::string &target_frame, const geometry_msgs::QuaternionStamped &stamped_in, geometry_msgs::QuaternionStamped &stamped_out) const
bool sleep() const
double z_hit_
Definition: amcl_node.cpp:257
double pop_z
Definition: pf.h:118
bool first_reconfigure_call_
Definition: amcl_node.cpp:247
static Quaternion createQuaternionFromYaw(double yaw)
Definition: map.h:61
bool getOdomPose(tf::Stamped< tf::Pose > &pose, double &x, double &y, double &yaw, const ros::Time &t, const std::string &f)
Definition: amcl_node.cpp:973
void requestMap()
Definition: amcl_node.cpp:795
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Duration & fromSec(double t)
void handleMapMessage(const nav_msgs::OccupancyGrid &msg)
Definition: amcl_node.cpp:825
int current_set
Definition: pf.h:125
std::vector< bool > lasers_update_
Definition: amcl_node.cpp:198
double alpha2_
Definition: amcl_node.cpp:255
AMCLSensor * sensor
Definition: amcl_sensor.h:88
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
double alpha5_
Definition: amcl_node.cpp:255
static double angle_diff(double a, double b)
Definition: amcl_node.cpp:93
static std::vector< std::pair< int, int > > free_space_indices
Definition: amcl_node.cpp:147
double m[3][3]
Definition: pf_vector.h:47
ros::Duration cloud_pub_interval
Definition: amcl_node.cpp:218
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
boost::shared_ptr< T > instantiate() const
double(* ranges)[2]
Definition: amcl_laser.h:54
double sigma_hit_
Definition: amcl_node.cpp:257
pf_matrix_t pf_matrix_zero()
Definition: pf_vector.c:134
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & y() const
pf_matrix_t pf_pose_cov
Definition: amcl_node.cpp:83
void sendTransform(const StampedTransform &transform)
void laserReceived(const sensor_msgs::LaserScanConstPtr &laser_scan)
Definition: amcl_node.cpp:1073
ros::Subscriber map_sub_
Definition: amcl_node.cpp:243
bool m_force_update
Definition: amcl_node.cpp:213
AMCLOdom * odom_
Definition: amcl_node.cpp:215
amcl_hyp_t * initial_pose_hyp_
Definition: amcl_node.cpp:245
pf_vector_t pose
Definition: amcl_odom.h:50
ros::Duration transform_tolerance_
Definition: amcl_node.cpp:233
ros::Time save_pose_last_time
Definition: amcl_node.cpp:184
void pf_set_selective_resampling(pf_t *pf, int selective_resampling)
Definition: pf.c:688
double getYaw(const tf2::Quaternion &q)
Transform inverse() const
map_t * convertMap(const nav_msgs::OccupancyGrid &map_msg)
Definition: amcl_node.cpp:935
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void SetModelLikelihoodField(double z_hit, double z_rand, double sigma_hit, double max_occ_dist)
Definition: amcl_laser.cpp:84
bool sent_first_transform_
Definition: amcl_node.cpp:138
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
Definition: pf.c:177
ros::WallDuration bag_scan_period_
Definition: amcl_node.cpp:222
static const std::string scan_topic_
Definition: amcl_node.cpp:108
#define ROS_WARN_STREAM(args)
tf::Transform latest_tf_
Definition: amcl_node.cpp:140
bool first_map_received_
Definition: amcl_node.cpp:246
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
double alpha_slow_
Definition: amcl_node.cpp:256
map_t * map_
Definition: amcl_node.cpp:189
ros::NodeHandle nh_
Definition: amcl_node.cpp:235
ros::ServiceServer global_loc_srv_
Definition: amcl_node.cpp:239
char * mapdata
Definition: amcl_node.cpp:190
ros::Timer check_laser_timer_
Definition: amcl_node.cpp:252
geometry_msgs::PoseWithCovarianceStamped last_published_pose
Definition: amcl_node.cpp:187
bool use_map_topic_
Definition: amcl_node.cpp:180
double lambda_short_
Definition: amcl_node.cpp:257
boost::recursive_mutex configuration_mutex_
Definition: amcl_node.cpp:249
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static WallTime now()
void pf_update_resample(pf_t *pf)
Definition: pf.c:362
void map_free(map_t *map)
Definition: map.c:61
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
pf_t * pf_
Definition: amcl_node.cpp:202
void freeMapDependentMemory()
Definition: amcl_node.cpp:914
ros::Duration gui_publish_period
Definition: amcl_node.cpp:183
std::string const & getTopic() const
laser_model_t laser_model_type_
Definition: amcl_node.cpp:265
amcl::AMCLConfig default_config_
Definition: amcl_node.cpp:251
ros::Duration save_pose_period
Definition: amcl_node.cpp:185
double weight
Definition: amcl_node.cpp:77
bool nomotionUpdateCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: amcl_node.cpp:1054
odom_model_t
Definition: amcl_odom.h:38
static Time now()
bool do_beamskip_
Definition: amcl_node.cpp:259
double d_thresh_
Definition: amcl_node.cpp:206
ROSCPP_DECL void shutdown()
void reconfigureCB(amcl::AMCLConfig &config, uint32_t level)
Definition: amcl_node.cpp:479
double pf_z_
Definition: amcl_node.cpp:203
void runFromBag(const std::string &in_bag_fn, bool trigger_global_localization=false)
Uses TF and LaserScan messages from bag file to drive AMCL instead.
Definition: amcl_node.cpp:625
ros::NodeHandle private_nh_
Definition: amcl_node.cpp:236
map_t * map_alloc(void)
Definition: map.c:38
bool pf_init_
Definition: amcl_node.cpp:204
message_filters::Subscriber< sensor_msgs::LaserScan > * laser_scan_sub_
Definition: amcl_node.cpp:194
bool hasParam(const std::string &key) const
void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped &msg)
Definition: amcl_node.cpp:1464
#define ROS_ASSERT(cond)
pf_sample_set_t sets[2]
Definition: pf.h:126
void pf_free(pf_t *pf)
Definition: pf.c:117
bool latest_tf_valid_
Definition: amcl_node.cpp:141
ros::Duration laser_check_interval_
Definition: amcl_node.cpp:272
int cluster_count
Definition: pf.h:100
tf::Stamped< tf::Pose > latest_odom_pose_
Definition: amcl_node.cpp:174
double resolution
Definition: amcl_node.cpp:192
#define ROS_ERROR(...)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
void SetModelLikelihoodFieldProb(double z_hit, double z_rand, double sigma_hit, double max_occ_dist, bool do_beamskip, double beam_skip_distance, double beam_skip_threshold, double beam_skip_error_threshold)
Definition: amcl_laser.cpp:98
double laser_min_range_
Definition: amcl_node.cpp:209
double laser_likelihood_max_dist_
Definition: amcl_node.cpp:261
pf_vector_t delta
Definition: amcl_odom.h:53
pf_vector_t pf_odom_pose_
Definition: amcl_node.cpp:205
double alpha_fast_
Definition: amcl_node.cpp:256
int max_beams_
Definition: amcl_node.cpp:254
bool setMapCallback(nav_msgs::SetMap::Request &req, nav_msgs::SetMap::Response &res)
Definition: amcl_node.cpp:1063
double a_thresh_
Definition: amcl_node.cpp:206
void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
Definition: amcl_node.cpp:1458
double init_cov_[3]
Definition: amcl_node.cpp:264
Connection registerCallback(const C &callback)
#define ROS_DEBUG(...)
double z_rand_
Definition: amcl_node.cpp:257


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:36