29 #include <boost/bind.hpp>
30 #include <boost/thread/mutex.hpp>
47 #include "sensor_msgs/LaserScan.h"
48 #include "geometry_msgs/PoseWithCovarianceStamped.h"
49 #include "geometry_msgs/PoseArray.h"
50 #include "geometry_msgs/Pose.h"
51 #include "geometry_msgs/PoseStamped.h"
52 #include "nav_msgs/GetMap.h"
53 #include "nav_msgs/SetMap.h"
54 #include "std_srvs/Empty.h"
68 #include "dynamic_reconfigure/server.h"
69 #include "amcl/AMCLConfig.h"
74 #include <boost/foreach.hpp>
79 #define NEW_UNIFORM_SAMPLING 1
100 return atan2(sin(z),cos(z));
109 d2 = 2*M_PI - fabs(d1);
112 if(fabs(d1) < fabs(d2))
126 std::string out = in;
127 if ( ( !in.empty() ) && (in[0] ==
'/') )
144 void runFromBag(
const std::string &in_bag_fn,
bool trigger_global_localization =
false);
147 void savePoseToServer();
150 std::shared_ptr<tf2_ros::TransformBroadcaster>
tfb_;
151 std::shared_ptr<tf2_ros::TransformListener>
tfl_;
152 std::shared_ptr<tf2_ros::Buffer>
tf_;
161 static pf_vector_t uniformPoseGenerator(
void* arg);
162 #if NEW_UNIFORM_SAMPLING
166 bool globalLocalizationCallback(std_srvs::Empty::Request& req,
167 std_srvs::Empty::Response& res);
168 bool nomotionUpdateCallback(std_srvs::Empty::Request& req,
169 std_srvs::Empty::Response& res);
170 bool setMapCallback(nav_msgs::SetMap::Request& req,
171 nav_msgs::SetMap::Response& res);
173 void laserReceived(
const sensor_msgs::LaserScanConstPtr& laser_scan);
174 void initialPoseReceived(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
175 void handleInitialPoseMessage(
const geometry_msgs::PoseWithCovarianceStamped& msg);
176 void mapReceived(
const nav_msgs::OccupancyGridConstPtr& msg);
178 void handleMapMessage(
const nav_msgs::OccupancyGrid& msg);
179 void freeMapDependentMemory();
180 map_t* convertMap(
const nav_msgs::OccupancyGrid& map_msg );
181 void updatePoseFromServer();
182 void applyInitialPose();
241 bool getOdomPose(geometry_msgs::PoseStamped&
pose,
242 double& x,
double& y,
double& yaw,
243 const ros::Time& t,
const std::string& f);
270 dynamic_reconfigure::Server<amcl::AMCLConfig> *
dsrv_;
275 double alpha1_, alpha2_, alpha3_, alpha4_,
alpha5_;
277 double z_hit_,
z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
283 double init_pose_[3];
291 void reconfigureCB(amcl::AMCLConfig &config, uint32_t level);
298 #if NEW_UNIFORM_SAMPLING
302 #define USAGE "USAGE: amcl"
330 else if ((argc >= 3) && (std::string(argv[1]) ==
"--run-from-bag"))
336 else if ((argc == 4) && (std::string(argv[3]) ==
"--global-localization"))
350 sent_first_transform_(false),
351 latest_tf_valid_(false),
358 initial_pose_hyp_(NULL),
359 first_map_received_(false),
360 first_reconfigure_call_(true)
406 std::string tmp_model_type;
407 private_nh_.
param(
"laser_model_type", tmp_model_type, std::string(
"likelihood_field"));
408 if(tmp_model_type ==
"beam")
410 else if(tmp_model_type ==
"likelihood_field")
412 else if(tmp_model_type ==
"likelihood_field_prob"){
417 ROS_WARN(
"Unknown laser model type \"%s\"; defaulting to likelihood_field model",
418 tmp_model_type.c_str());
423 if(tmp_model_type ==
"diff")
425 else if(tmp_model_type ==
"omni")
427 else if(tmp_model_type ==
"diff-corrected")
429 else if(tmp_model_type ==
"omni-corrected")
433 ROS_WARN(
"Unknown odom model type \"%s\"; defaulting to diff model",
434 tmp_model_type.c_str());
461 double bag_scan_period;
498 ROS_INFO(
"Subscribed to map topic.");
505 dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&
AmclNode::reconfigureCB,
this, _1, _2);
506 dsrv_->setCallback(cb);
530 if(config.restore_defaults) {
533 config.restore_defaults =
false;
556 z_hit_ = config.laser_z_hit;
558 z_max_ = config.laser_z_max;
564 if(config.laser_model_type ==
"beam")
566 else if(config.laser_model_type ==
"likelihood_field")
568 else if(config.laser_model_type ==
"likelihood_field_prob")
571 if(config.odom_model_type ==
"diff")
573 else if(config.odom_model_type ==
"omni")
575 else if(config.odom_model_type ==
"diff-corrected")
577 else if(config.odom_model_type ==
"omni-corrected")
580 if(config.min_particles > config.max_particles)
582 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.");
583 config.max_particles = config.min_particles;
614 pf_z_ = config.kld_z;
627 pf_init(
pf_, pf_init_pose_mean, pf_init_pose_cov);
644 ROS_INFO(
"Initializing likelihood field model; this can take some time on large maps...");
649 ROS_INFO(
"Done initializing likelihood field model with probabilities.");
652 ROS_INFO(
"Initializing likelihood field model; this can take some time on large maps...");
655 ROS_INFO(
"Done initializing likelihood field model.");
680 std::vector<std::string> topics;
681 topics.push_back(std::string(
"tf"));
682 std::string scan_topic_name =
"base_scan";
683 topics.push_back(scan_topic_name);
709 if (trigger_global_localization)
711 std_srvs::Empty empty_srv;
725 tf2_msgs::TFMessage::ConstPtr tf_msg =
msg.instantiate<tf2_msgs::TFMessage>();
729 for (
size_t ii=0; ii<tf_msg->transforms.size(); ++ii)
731 tf_->setTransform(tf_msg->transforms[ii],
"rosbag_authority");
736 sensor_msgs::LaserScan::ConstPtr base_scan =
msg.instantiate<sensor_msgs::LaserScan>();
737 if (base_scan != NULL)
754 ROS_INFO(
"Bag complete, took %.1f seconds to process, shutting down", runtime);
757 double yaw, pitch, roll;
759 ROS_INFO(
"Final location %.3f, %.3f, %.3f with stamp=%f",
800 init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);
804 if(!std::isnan(tmp_pos))
807 ROS_WARN(
"ignoring NAN in initial pose X position");
809 if(!std::isnan(tmp_pos))
812 ROS_WARN(
"ignoring NAN in initial pose Y position");
814 if(!std::isnan(tmp_pos))
817 ROS_WARN(
"ignoring NAN in initial pose Yaw");
819 if(!std::isnan(tmp_pos))
822 ROS_WARN(
"ignoring NAN in initial covariance XX");
824 if(!std::isnan(tmp_pos))
827 ROS_WARN(
"ignoring NAN in initial covariance YY");
829 if(!std::isnan(tmp_pos))
832 ROS_WARN(
"ignoring NAN in initial covariance AA");
841 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.",
853 nav_msgs::GetMap::Request req;
854 nav_msgs::GetMap::Response resp;
858 ROS_WARN(
"Request for map failed; trying again...");
882 ROS_INFO(
"Received a %d X %d map @ %.3f m/pix\n",
885 msg.info.resolution);
888 ROS_WARN(
"Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics",
889 msg.header.frame_id.c_str(),
901 #if NEW_UNIFORM_SAMPLING
928 pf_init(
pf_, pf_init_pose_mean, pf_init_pose_cov);
945 ROS_INFO(
"Initializing likelihood field model; this can take some time on large maps...");
950 ROS_INFO(
"Done initializing likelihood field model.");
954 ROS_INFO(
"Initializing likelihood field model; this can take some time on large maps...");
957 ROS_INFO(
"Done initializing likelihood field model.");
993 map->
size_x = map_msg.info.width;
994 map->
size_y = map_msg.info.height;
995 map->
scale = map_msg.info.resolution;
1003 if(map_msg.data[i] == 0)
1005 else if(map_msg.data[i] == 100)
1025 double& x,
double& y,
double& yaw,
1026 const ros::Time& t,
const std::string& f)
1029 geometry_msgs::PoseStamped ident;
1031 ident.header.stamp =
t;
1039 ROS_WARN(
"Failed to compute odom pose, skipping scan (%s)", e.what());
1042 x = odom_pose.pose.position.x;
1043 y = odom_pose.pose.position.y;
1054 #if NEW_UNIFORM_SAMPLING
1058 p.
v[0] =
MAP_WXGX(map, free_point.first);
1059 p.
v[1] =
MAP_WYGY(map, free_point.second);
1060 p.
v[2] =
drand48() * 2 * M_PI - M_PI;
1062 double min_x, max_x, min_y, max_y;
1071 ROS_DEBUG(
"Generating new uniform sample");
1074 p.
v[0] = min_x +
drand48() * (max_x - min_x);
1075 p.
v[1] = min_y +
drand48() * (max_y - min_y);
1076 p.
v[2] =
drand48() * 2 * M_PI - M_PI;
1090 std_srvs::Empty::Response& res)
1092 if(
map_ == NULL ) {
1096 ROS_INFO(
"Initializing with uniform distribution");
1099 ROS_INFO(
"Global initialisation done!");
1107 std_srvs::Empty::Response& res)
1116 nav_msgs::SetMap::Response& res)
1131 std::string laser_scan_frame_id =
stripSlash(laser_scan->header.frame_id);
1133 if(
map_ == NULL ) {
1137 int laser_index = -1;
1147 geometry_msgs::PoseStamped ident;
1148 ident.header.frame_id = laser_scan_frame_id;
1152 geometry_msgs::PoseStamped laser_pose;
1159 ROS_ERROR(
"Couldn't transform from %s to %s, "
1160 "even though the message notifier is in use",
1161 laser_scan_frame_id.c_str(),
1167 laser_pose_v.
v[0] = laser_pose.pose.position.x;
1168 laser_pose_v.
v[1] = laser_pose.pose.position.y;
1170 laser_pose_v.
v[2] = 0;
1171 lasers_[laser_index]->SetLaserPose(laser_pose_v);
1172 ROS_DEBUG(
"Received laser's pose wrt robot: %.3f %.3f %.3f",
1188 ROS_ERROR(
"Couldn't determine robot's pose associated with laser scan");
1216 bool force_publication =
false;
1229 force_publication =
true;
1244 odata.
delta = delta;
1253 bool resampled =
false;
1266 q.
setRPY(0.0, 0.0, laser_scan->angle_min);
1267 geometry_msgs::QuaternionStamped min_q, inc_q;
1268 min_q.header.stamp = laser_scan->header.stamp;
1269 min_q.header.frame_id =
stripSlash(laser_scan->header.frame_id);
1272 q.
setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
1273 inc_q.header = min_q.header;
1282 ROS_WARN(
"Unable to transform min/max laser angles into base frame: %s",
1288 double angle_increment =
tf2::getYaw(inc_q.quaternion) - angle_min;
1291 angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
1293 ROS_DEBUG(
"Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
1299 ldata.
range_max = laser_scan->range_max;
1304 range_min = laser_scan->range_min;
1306 if(ldata.
range_max <= 0.0 || range_min < 0.0) {
1307 ROS_ERROR(
"range_max or range_min from laser is negative! ignore this message.");
1318 if(laser_scan->ranges[i] <= range_min)
1320 else if(laser_scan->ranges[i] > ldata.
range_max)
1321 ldata.
ranges[i][0] = std::numeric_limits<decltype(ldata.
range_max)>::max();
1323 ldata.
ranges[i][0] = laser_scan->ranges[i];
1325 ldata.
ranges[i][1] = angle_min +
1326 (i * angle_increment);
1349 geometry_msgs::PoseArray cloud_msg;
1352 cloud_msg.poses.resize(
set->sample_count);
1353 for(
int i=0;i<
set->sample_count;i++)
1355 cloud_msg.poses[i].position.x =
set->samples[i].pose.v[0];
1356 cloud_msg.poses[i].position.y =
set->samples[i].pose.v[1];
1357 cloud_msg.poses[i].position.z = 0;
1359 q.
setRPY(0, 0,
set->samples[i].pose.v[2]);
1366 if(resampled || force_publication)
1369 double max_weight = 0.0;
1370 int max_weight_hyp = -1;
1371 std::vector<amcl_hyp_t> hyps;
1373 for(
int hyp_count = 0;
1381 ROS_ERROR(
"Couldn't get stats on cluster %d", hyp_count);
1385 hyps[hyp_count].weight = weight;
1386 hyps[hyp_count].pf_pose_mean = pose_mean;
1387 hyps[hyp_count].pf_pose_cov = pose_cov;
1389 if(hyps[hyp_count].weight > max_weight)
1391 max_weight = hyps[hyp_count].weight;
1392 max_weight_hyp = hyp_count;
1396 if(max_weight > 0.0)
1398 ROS_DEBUG(
"Max weight pose: %.3f %.3f %.3f",
1399 hyps[max_weight_hyp].pf_pose_mean.v[0],
1400 hyps[max_weight_hyp].pf_pose_mean.v[1],
1401 hyps[max_weight_hyp].pf_pose_mean.v[2]);
1409 geometry_msgs::PoseWithCovarianceStamped p;
1412 p.header.stamp = laser_scan->header.stamp;
1414 p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
1415 p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
1418 q.
setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
1422 for(
int i=0; i<2; i++)
1424 for(
int j=0; j<2; j++)
1429 p.pose.covariance[6*i+j] =
set->cov.m[i][j];
1435 p.pose.covariance[6*5+5] =
set->cov.m[2][2];
1450 ROS_DEBUG(
"New pose: %6.3f %6.3f %6.3f",
1451 hyps[max_weight_hyp].pf_pose_mean.v[0],
1452 hyps[max_weight_hyp].pf_pose_mean.v[1],
1453 hyps[max_weight_hyp].pf_pose_mean.v[2]);
1456 geometry_msgs::PoseStamped odom_to_map;
1460 q.
setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
1461 tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
1462 hyps[max_weight_hyp].pf_pose_mean.v[1],
1465 geometry_msgs::PoseStamped tmp_tf_stamped;
1467 tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
1474 ROS_DEBUG(
"Failed to subtract base to odom transform");
1485 ros::Time transform_expiration = (laser_scan->header.stamp +
1487 geometry_msgs::TransformStamped tmp_tf_stamped;
1489 tmp_tf_stamped.header.stamp = transform_expiration;
1493 this->
tfb_->sendTransform(tmp_tf_stamped);
1508 ros::Time transform_expiration = (laser_scan->header.stamp +
1510 geometry_msgs::TransformStamped tmp_tf_stamped;
1512 tmp_tf_stamped.header.stamp = transform_expiration;
1515 this->
tfb_->sendTransform(tmp_tf_stamped);
1545 if(
msg.header.frame_id ==
"")
1548 ROS_WARN(
"Received initial pose with empty frame_id. You should always supply a frame_id.");
1553 ROS_WARN(
"Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
1561 geometry_msgs::TransformStamped tx_odom;
1576 ROS_WARN(
"Failed to transform initial pose in time (%s)", e.what());
1584 pose_new = pose_old * tx_odom_tf2;
1588 ROS_INFO(
"Setting pose (%.6f): %.3f %.3f %.3f",
1595 pf_init_pose_mean.
v[0] = pose_new.
getOrigin().x();
1596 pf_init_pose_mean.
v[1] = pose_new.
getOrigin().y();
1600 for(
int i=0; i<2; i++)
1602 for(
int j=0; j<2; j++)
1604 pf_init_pose_cov.
m[i][j] =
msg.pose.covariance[6*i+j];
1607 pf_init_pose_cov.
m[2][2] =
msg.pose.covariance[6*5+5];
1641 diagnostic_status.
add(
"std_x", std_x);
1642 diagnostic_status.
add(
"std_y", std_y);
1643 diagnostic_status.
add(
"std_yaw", std_yaw);
1650 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"Too large");
1654 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"OK");