1410 lines
48 KiB
C++
1410 lines
48 KiB
C++
#include "amcl/amcl.h"
|
|
|
|
amcl::Amcl::Amcl() : sent_first_transform_(false),
|
|
latest_tf_valid_(false),
|
|
map_(NULL),
|
|
pf_(NULL),
|
|
resample_count_(0),
|
|
odom_(NULL),
|
|
laser_(NULL),
|
|
private_nh_("~/Amcl"),
|
|
initial_pose_hyp_(NULL),
|
|
first_map_received_(false),
|
|
first_reconfigure_call_(true),
|
|
dsrv_(NULL),
|
|
laser_scan_filter_(NULL),
|
|
laser_scan_sub_(NULL),
|
|
tfb_(nullptr),
|
|
tf_(nullptr),
|
|
tfl_(nullptr)
|
|
{
|
|
init_pose_[0] = 0.0;
|
|
init_pose_[1] = 0.0;
|
|
init_pose_[2] = 0.0;
|
|
init_cov_[0] = 0.5 * 0.5;
|
|
init_cov_[1] = 0.5 * 0.5;
|
|
init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0);
|
|
this->initialize(private_nh_);
|
|
}
|
|
|
|
amcl::Amcl::Amcl(ros::NodeHandle nh) : sent_first_transform_(false),
|
|
latest_tf_valid_(false),
|
|
map_(NULL),
|
|
pf_(NULL),
|
|
resample_count_(0),
|
|
odom_(NULL),
|
|
laser_(NULL),
|
|
private_nh_("~/Amcl"),
|
|
initial_pose_hyp_(NULL),
|
|
first_map_received_(false),
|
|
first_reconfigure_call_(true),
|
|
initialized_(false),
|
|
dsrv_(NULL),
|
|
laser_scan_filter_(NULL),
|
|
laser_scan_sub_(NULL),
|
|
tfb_(nullptr),
|
|
tf_(nullptr),
|
|
tfl_(nullptr)
|
|
{
|
|
init_pose_[0] = 0.0;
|
|
init_pose_[1] = 0.0;
|
|
init_pose_[2] = 0.0;
|
|
init_cov_[0] = 0.5 * 0.5;
|
|
init_cov_[1] = 0.5 * 0.5;
|
|
init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0);
|
|
this->initialize(nh);
|
|
}
|
|
|
|
amcl::Amcl::~Amcl()
|
|
{
|
|
if (dsrv_)
|
|
{
|
|
delete dsrv_;
|
|
dsrv_ = NULL;
|
|
}
|
|
|
|
freeMapDependentMemory();
|
|
if (laser_scan_filter_)
|
|
{
|
|
delete laser_scan_filter_;
|
|
laser_scan_filter_ = NULL;
|
|
}
|
|
|
|
if (laser_scan_sub_)
|
|
{
|
|
delete laser_scan_sub_;
|
|
laser_scan_sub_ = NULL;
|
|
}
|
|
|
|
if (tfl_)
|
|
tfl_.reset();
|
|
if (tf_)
|
|
tf_.reset();
|
|
if (tfb_)
|
|
tfb_.reset();
|
|
// TODO: delete everything allocated in constructor
|
|
}
|
|
|
|
void amcl::Amcl::initialize(ros::NodeHandle nh)
|
|
{
|
|
if (!initialized_)
|
|
{
|
|
|
|
// boost::recursive_mutex::scoped_lock l(configuration_mutex_);
|
|
// Grab params off the param server
|
|
nh.param("use_map_topic", use_map_topic_, false);
|
|
nh.param("first_map_only", first_map_only_, false);
|
|
|
|
nh.param("scan_topic", scan_topic_, std::string("scan"));
|
|
nh.param("map_topic", map_topic_, std::string("map"));
|
|
nh.param("map_service", map_service_, std::string("static_map"));
|
|
|
|
double tmp;
|
|
nh.param("gui_publish_rate", tmp, -1.0);
|
|
gui_publish_period = ros::Duration(1.0 / tmp);
|
|
nh.param("save_pose_rate", tmp, 0.5);
|
|
save_pose_period = ros::Duration(1.0 / tmp);
|
|
|
|
nh.param("laser_min_range", laser_min_range_, -1.0);
|
|
nh.param("laser_max_range", laser_max_range_, -1.0);
|
|
nh.param("laser_max_beams", max_beams_, 30);
|
|
nh.param("min_particles", min_particles_, 100);
|
|
nh.param("max_particles", max_particles_, 5000);
|
|
nh.param("kld_err", pf_err_, 0.01);
|
|
nh.param("kld_z", pf_z_, 0.99);
|
|
nh.param("odom_alpha1", alpha1_, 0.2);
|
|
nh.param("odom_alpha2", alpha2_, 0.2);
|
|
nh.param("odom_alpha3", alpha3_, 0.2);
|
|
nh.param("odom_alpha4", alpha4_, 0.2);
|
|
nh.param("odom_alpha5", alpha5_, 0.2);
|
|
|
|
nh.param("do_beamskip", do_beamskip_, false);
|
|
nh.param("beam_skip_distance", beam_skip_distance_, 0.5);
|
|
nh.param("beam_skip_threshold", beam_skip_threshold_, 0.3);
|
|
if (nh.hasParam("beam_skip_error_threshold_"))
|
|
{
|
|
nh.param("beam_skip_error_threshold_", beam_skip_error_threshold_);
|
|
}
|
|
else
|
|
{
|
|
nh.param("beam_skip_error_threshold", beam_skip_error_threshold_, 0.9);
|
|
}
|
|
|
|
nh.param("laser_z_hit", z_hit_, 0.95);
|
|
nh.param("laser_z_short", z_short_, 0.1);
|
|
nh.param("laser_z_max", z_max_, 0.05);
|
|
nh.param("laser_z_rand", z_rand_, 0.05);
|
|
nh.param("laser_sigma_hit", sigma_hit_, 0.2);
|
|
nh.param("laser_lambda_short", lambda_short_, 0.1);
|
|
nh.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0);
|
|
std::string tmp_model_type;
|
|
nh.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
|
|
if (tmp_model_type == "beam")
|
|
laser_model_type_ = amcl::LASER_MODEL_BEAM;
|
|
else if (tmp_model_type == "likelihood_field")
|
|
laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD;
|
|
else if (tmp_model_type == "likelihood_field_prob")
|
|
{
|
|
laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB;
|
|
}
|
|
else
|
|
{
|
|
ROS_WARN("Unknown laser model type \"%s\"; defaulting to likelihood_field model",
|
|
tmp_model_type.c_str());
|
|
laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD;
|
|
}
|
|
|
|
nh.param("odom_model_type", tmp_model_type, std::string("diff"));
|
|
if (tmp_model_type == "diff")
|
|
odom_model_type_ = amcl::ODOM_MODEL_DIFF;
|
|
else if (tmp_model_type == "omni")
|
|
odom_model_type_ = amcl::ODOM_MODEL_OMNI;
|
|
else if (tmp_model_type == "diff-corrected")
|
|
odom_model_type_ = amcl::ODOM_MODEL_DIFF_CORRECTED;
|
|
else if (tmp_model_type == "omni-corrected")
|
|
odom_model_type_ = amcl::ODOM_MODEL_OMNI_CORRECTED;
|
|
else
|
|
{
|
|
ROS_WARN("Unknown odom model type \"%s\"; defaulting to diff model",
|
|
tmp_model_type.c_str());
|
|
odom_model_type_ = amcl::ODOM_MODEL_DIFF;
|
|
}
|
|
|
|
nh.param("update_min_d", d_thresh_, 0.2);
|
|
nh.param("update_min_a", a_thresh_, M_PI / 6.0);
|
|
nh.param("odom_frame_id", odom_frame_id_, std::string("odom"));
|
|
nh.param("base_frame_id", base_frame_id_, std::string("base_link"));
|
|
nh.param("global_frame_id", global_frame_id_, std::string("map"));
|
|
nh.param("resample_interval", resample_interval_, 2);
|
|
nh.param("selective_resampling", selective_resampling_, false);
|
|
double tmp_tol;
|
|
nh.param("transform_tolerance", tmp_tol, 0.1);
|
|
nh.param("recovery_alpha_slow", alpha_slow_, 0.001);
|
|
nh.param("recovery_alpha_fast", alpha_fast_, 0.1);
|
|
nh.param("tf_broadcast", tf_broadcast_, true);
|
|
nh.param("force_update_after_initialpose", force_update_after_initialpose_, false);
|
|
nh.param("force_update_after_set_map", force_update_after_set_map_, false);
|
|
|
|
// For diagnostics
|
|
nh.param("std_warn_level_x", std_warn_level_x_, 0.2);
|
|
nh.param("std_warn_level_y", std_warn_level_y_, 0.2);
|
|
nh.param("std_warn_level_yaw", std_warn_level_yaw_, 0.1);
|
|
|
|
transform_tolerance_.fromSec(tmp_tol);
|
|
{
|
|
double bag_scan_period;
|
|
nh.param("bag_scan_period", bag_scan_period, -1.0);
|
|
bag_scan_period_.fromSec(bag_scan_period);
|
|
}
|
|
|
|
odom_frame_id_ = stripSlash(odom_frame_id_);
|
|
base_frame_id_ = stripSlash(base_frame_id_);
|
|
global_frame_id_ = stripSlash(global_frame_id_);
|
|
|
|
updatePoseFromServer();
|
|
|
|
cloud_pub_interval.fromSec(1.0);
|
|
tfb_ = std::make_shared<tf2_ros::TransformBroadcaster>();
|
|
tf_ = std::make_shared<tf2_ros::Buffer>();
|
|
tfl_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
|
|
|
|
pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
|
|
particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);
|
|
global_loc_srv_ = nh_.advertiseService("global_localization",
|
|
&amcl::Amcl::globalLocalizationCallback,
|
|
this);
|
|
nomotion_update_srv_ = nh_.advertiseService("request_nomotion_update", &amcl::Amcl::nomotionUpdateCallback, this);
|
|
set_map_srv_ = nh_.advertiseService("set_map", &amcl::Amcl::setMapCallback, this);
|
|
|
|
laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
|
|
laser_scan_filter_ =
|
|
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
|
|
*tf_,
|
|
odom_frame_id_,
|
|
100,
|
|
nh_);
|
|
laser_scan_filter_->registerCallback(boost::bind(&amcl::Amcl::laserReceived,
|
|
this, _1));
|
|
initial_pose_sub_ = nh_.subscribe("initialpose", 2, &amcl::Amcl::initialPoseReceived, this);
|
|
|
|
if (use_map_topic_)
|
|
{
|
|
map_sub_ = nh_.subscribe(map_topic_, 1, &amcl::Amcl::mapReceived, this);
|
|
ROS_INFO("Subscribed to map topic.");
|
|
}
|
|
else
|
|
{
|
|
requestMap();
|
|
}
|
|
m_force_update = false;
|
|
|
|
dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(private_nh_);
|
|
dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&amcl::Amcl::reconfigureCB, this, _1, _2);
|
|
dsrv_->setCallback(cb);
|
|
|
|
// 15s timer to warn on lack of receipt of laser scans, #5209
|
|
laser_check_interval_ = ros::Duration(15.0);
|
|
check_laser_timer_ = nh_.createTimer(laser_check_interval_,
|
|
boost::bind(&amcl::Amcl::checkLaserReceived, this, _1));
|
|
|
|
diagnosic_updater_.setHardwareID("None");
|
|
diagnosic_updater_.add("Standard deviation", this, &amcl::Amcl::standardDeviationDiagnostics);
|
|
ros::Duration(0.5).sleep();
|
|
initialized_ = true;
|
|
}
|
|
}
|
|
|
|
void amcl::Amcl::reconfigureCB(amcl::AMCLConfig &config, uint32_t level)
|
|
{
|
|
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
|
|
|
|
// we don't want to do anything on the first call
|
|
// which corresponds to startup
|
|
if (first_reconfigure_call_)
|
|
{
|
|
first_reconfigure_call_ = false;
|
|
default_config_ = config;
|
|
return;
|
|
}
|
|
|
|
if (config.restore_defaults)
|
|
{
|
|
config = default_config_;
|
|
// avoid looping
|
|
config.restore_defaults = false;
|
|
}
|
|
|
|
d_thresh_ = config.update_min_d;
|
|
a_thresh_ = config.update_min_a;
|
|
|
|
resample_interval_ = config.resample_interval;
|
|
|
|
laser_min_range_ = config.laser_min_range;
|
|
laser_max_range_ = config.laser_max_range;
|
|
|
|
gui_publish_period = ros::Duration(1.0 / config.gui_publish_rate);
|
|
save_pose_period = ros::Duration(1.0 / config.save_pose_rate);
|
|
|
|
transform_tolerance_.fromSec(config.transform_tolerance);
|
|
|
|
max_beams_ = config.laser_max_beams;
|
|
alpha1_ = config.odom_alpha1;
|
|
alpha2_ = config.odom_alpha2;
|
|
alpha3_ = config.odom_alpha3;
|
|
alpha4_ = config.odom_alpha4;
|
|
alpha5_ = config.odom_alpha5;
|
|
|
|
z_hit_ = config.laser_z_hit;
|
|
z_short_ = config.laser_z_short;
|
|
z_max_ = config.laser_z_max;
|
|
z_rand_ = config.laser_z_rand;
|
|
sigma_hit_ = config.laser_sigma_hit;
|
|
lambda_short_ = config.laser_lambda_short;
|
|
laser_likelihood_max_dist_ = config.laser_likelihood_max_dist;
|
|
|
|
if (config.laser_model_type == "beam")
|
|
laser_model_type_ = amcl::LASER_MODEL_BEAM;
|
|
else if (config.laser_model_type == "likelihood_field")
|
|
laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD;
|
|
else if (config.laser_model_type == "likelihood_field_prob")
|
|
laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB;
|
|
|
|
if (config.odom_model_type == "diff")
|
|
odom_model_type_ = amcl::ODOM_MODEL_DIFF;
|
|
else if (config.odom_model_type == "omni")
|
|
odom_model_type_ = amcl::ODOM_MODEL_OMNI;
|
|
else if (config.odom_model_type == "diff-corrected")
|
|
odom_model_type_ = amcl::ODOM_MODEL_DIFF_CORRECTED;
|
|
else if (config.odom_model_type == "omni-corrected")
|
|
odom_model_type_ = amcl::ODOM_MODEL_OMNI_CORRECTED;
|
|
|
|
if (config.min_particles > config.max_particles)
|
|
{
|
|
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.");
|
|
config.max_particles = config.min_particles;
|
|
}
|
|
|
|
min_particles_ = config.min_particles;
|
|
max_particles_ = config.max_particles;
|
|
alpha_slow_ = config.recovery_alpha_slow;
|
|
alpha_fast_ = config.recovery_alpha_fast;
|
|
tf_broadcast_ = config.tf_broadcast;
|
|
force_update_after_initialpose_ = config.force_update_after_initialpose;
|
|
force_update_after_set_map_ = config.force_update_after_set_map;
|
|
|
|
do_beamskip_ = config.do_beamskip;
|
|
beam_skip_distance_ = config.beam_skip_distance;
|
|
beam_skip_threshold_ = config.beam_skip_threshold;
|
|
|
|
// Clear queued laser objects so that their parameters get updated
|
|
lasers_.clear();
|
|
lasers_update_.clear();
|
|
frame_to_laser_.clear();
|
|
|
|
if (pf_ != NULL)
|
|
{
|
|
pf_free(pf_);
|
|
pf_ = NULL;
|
|
}
|
|
pf_ = pf_alloc(min_particles_, max_particles_,
|
|
alpha_slow_, alpha_fast_,
|
|
(pf_init_model_fn_t)amcl::Amcl::uniformPoseGenerator,
|
|
(void *)map_);
|
|
pf_set_selective_resampling(pf_, selective_resampling_);
|
|
pf_err_ = config.kld_err;
|
|
pf_z_ = config.kld_z;
|
|
pf_->pop_err = pf_err_;
|
|
pf_->pop_z = pf_z_;
|
|
|
|
// Initialize the filter
|
|
pf_vector_t pf_init_pose_mean = pf_vector_zero();
|
|
pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
|
|
pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
|
|
pf_init_pose_mean.v[2] = tf2::getYaw(last_published_pose.pose.pose.orientation);
|
|
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
|
|
pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6 * 0 + 0];
|
|
pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6 * 1 + 1];
|
|
pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6 * 5 + 5];
|
|
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
|
|
pf_init_ = false;
|
|
|
|
// Instantiate the sensor objects
|
|
// Odometry
|
|
delete odom_;
|
|
odom_ = new amcl::AMCLOdom();
|
|
ROS_ASSERT(odom_);
|
|
odom_->SetModel(odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
|
|
// Laser
|
|
delete laser_;
|
|
laser_ = new amcl::AMCLLaser(max_beams_, map_);
|
|
ROS_ASSERT(laser_);
|
|
if (laser_model_type_ == amcl::LASER_MODEL_BEAM)
|
|
laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
|
|
sigma_hit_, lambda_short_, 0.0);
|
|
else if (laser_model_type_ == amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB)
|
|
{
|
|
ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
|
|
laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,
|
|
laser_likelihood_max_dist_,
|
|
do_beamskip_, beam_skip_distance_,
|
|
beam_skip_threshold_, beam_skip_error_threshold_);
|
|
ROS_INFO("Done initializing likelihood field model with probabilities.");
|
|
}
|
|
else if (laser_model_type_ == amcl::LASER_MODEL_LIKELIHOOD_FIELD)
|
|
{
|
|
ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
|
|
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
|
|
laser_likelihood_max_dist_);
|
|
ROS_INFO("Done initializing likelihood field model.");
|
|
}
|
|
|
|
odom_frame_id_ = stripSlash(config.odom_frame_id);
|
|
base_frame_id_ = stripSlash(config.base_frame_id);
|
|
global_frame_id_ = stripSlash(config.global_frame_id);
|
|
|
|
delete laser_scan_filter_;
|
|
laser_scan_filter_ =
|
|
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
|
|
*tf_,
|
|
odom_frame_id_,
|
|
100,
|
|
nh_);
|
|
laser_scan_filter_->registerCallback(boost::bind(&amcl::Amcl::laserReceived,
|
|
this, _1));
|
|
|
|
initial_pose_sub_ = nh_.subscribe("initialpose", 2, &amcl::Amcl::initialPoseReceived, this);
|
|
}
|
|
|
|
void amcl::Amcl::runFromBag(const std::string &in_bag_fn, bool trigger_global_localization)
|
|
{
|
|
rosbag::Bag bag;
|
|
bag.open(in_bag_fn, rosbag::bagmode::Read);
|
|
std::vector<std::string> topics;
|
|
topics.push_back(std::string("tf"));
|
|
std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS
|
|
topics.push_back(scan_topic_name);
|
|
rosbag::View view(bag, rosbag::TopicQuery(topics));
|
|
|
|
ros::Publisher laser_pub = nh_.advertise<sensor_msgs::LaserScan>(scan_topic_name, 100);
|
|
ros::Publisher tf_pub = nh_.advertise<tf2_msgs::TFMessage>("/tf", 100);
|
|
|
|
// Sleep for a second to let all subscribers connect
|
|
ros::WallDuration(1.0).sleep();
|
|
|
|
ros::WallTime start(ros::WallTime::now());
|
|
|
|
// Wait for map
|
|
while (ros::ok())
|
|
{
|
|
{
|
|
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
|
|
if (map_)
|
|
{
|
|
ROS_INFO("Map is ready");
|
|
break;
|
|
}
|
|
}
|
|
ROS_INFO("Waiting for the map...");
|
|
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0));
|
|
}
|
|
|
|
if (trigger_global_localization)
|
|
{
|
|
std_srvs::Empty empty_srv;
|
|
globalLocalizationCallback(empty_srv.request, empty_srv.response);
|
|
}
|
|
|
|
BOOST_FOREACH (rosbag::MessageInstance const msg, view)
|
|
{
|
|
if (!ros::ok())
|
|
{
|
|
break;
|
|
}
|
|
|
|
// Process any ros messages or callbacks at this point
|
|
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration());
|
|
|
|
tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
|
|
if (tf_msg != NULL)
|
|
{
|
|
tf_pub.publish(msg);
|
|
for (size_t ii = 0; ii < tf_msg->transforms.size(); ++ii)
|
|
{
|
|
tf_->setTransform(tf_msg->transforms[ii], "rosbag_authority");
|
|
}
|
|
continue;
|
|
}
|
|
|
|
sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate<sensor_msgs::LaserScan>();
|
|
if (base_scan != NULL)
|
|
{
|
|
laser_pub.publish(msg);
|
|
laser_scan_filter_->add(base_scan);
|
|
if (bag_scan_period_ > ros::WallDuration(0))
|
|
{
|
|
bag_scan_period_.sleep();
|
|
}
|
|
continue;
|
|
}
|
|
|
|
ROS_WARN_STREAM("Unsupported message type" << msg.getTopic());
|
|
}
|
|
|
|
bag.close();
|
|
|
|
double runtime = (ros::WallTime::now() - start).toSec();
|
|
ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime);
|
|
|
|
const geometry_msgs::Quaternion &q(last_published_pose.pose.pose.orientation);
|
|
double yaw, pitch, roll;
|
|
tf2::Matrix3x3(tf2::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw, pitch, roll);
|
|
ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f",
|
|
last_published_pose.pose.pose.position.x,
|
|
last_published_pose.pose.pose.position.y,
|
|
yaw, last_published_pose.header.stamp.toSec());
|
|
|
|
ros::shutdown();
|
|
}
|
|
|
|
void amcl::Amcl::savePoseToServer(geometry_msgs::PoseWithCovarianceStamped &amcl_pose)
|
|
{
|
|
// We need to apply the last transform to the latest odom pose to get
|
|
// the latest map pose to store. We'll take the covariance from
|
|
// last_published_pose.
|
|
tf2::Transform odom_pose_tf2;
|
|
tf2::convert(latest_odom_pose_.pose, odom_pose_tf2);
|
|
tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;
|
|
|
|
double yaw = tf2::getYaw(map_pose.getRotation());
|
|
|
|
ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f, yaw: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y(), yaw);
|
|
// ROS_INFO("Saving pose to server %s. x: %.3f, y: %.3f, yaw: %.3f", private_nh_.getNamespace().c_str(), map_pose.getOrigin().x(), map_pose.getOrigin().y(), yaw);
|
|
|
|
private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
|
|
private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
|
|
private_nh_.setParam("initial_pose_a", yaw);
|
|
private_nh_.setParam("initial_cov_xx",
|
|
last_published_pose.pose.covariance[6 * 0 + 0]);
|
|
private_nh_.setParam("initial_cov_yy",
|
|
last_published_pose.pose.covariance[6 * 1 + 1]);
|
|
private_nh_.setParam("initial_cov_aa",
|
|
last_published_pose.pose.covariance[6 * 5 + 5]);
|
|
|
|
// Set the pose in the input parameter object (amcl_pose)
|
|
amcl_pose.header.stamp = ros::Time::now(); // Set timestamp
|
|
amcl_pose.header.frame_id = "map"; // Set frame id to "map"
|
|
amcl_pose.pose.pose.position.x = map_pose.getOrigin().x();
|
|
amcl_pose.pose.pose.position.y = map_pose.getOrigin().y();
|
|
amcl_pose.pose.pose.orientation = tf2::toMsg(map_pose.getRotation());
|
|
|
|
// Set the covariance from last_published_pose
|
|
for (size_t i = 0; i < 36; ++i)
|
|
{
|
|
amcl_pose.pose.covariance[i] = last_published_pose.pose.covariance[i];
|
|
}
|
|
}
|
|
|
|
void amcl::Amcl::updatePoseFromServer()
|
|
{
|
|
init_pose_[0] = 0.0;
|
|
init_pose_[1] = 0.0;
|
|
init_pose_[2] = 0.0;
|
|
init_cov_[0] = 0.5 * 0.5;
|
|
init_cov_[1] = 0.5 * 0.5;
|
|
init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0);
|
|
// Check for NAN on input from param server, #5239
|
|
double tmp_pos;
|
|
private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]);
|
|
if (!std::isnan(tmp_pos))
|
|
init_pose_[0] = tmp_pos;
|
|
else
|
|
ROS_WARN("ignoring NAN in initial pose X position");
|
|
private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]);
|
|
if (!std::isnan(tmp_pos))
|
|
init_pose_[1] = tmp_pos;
|
|
else
|
|
ROS_WARN("ignoring NAN in initial pose Y position");
|
|
private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]);
|
|
if (!std::isnan(tmp_pos))
|
|
init_pose_[2] = tmp_pos;
|
|
else
|
|
ROS_WARN("ignoring NAN in initial pose Yaw");
|
|
private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]);
|
|
if (!std::isnan(tmp_pos))
|
|
init_cov_[0] = tmp_pos;
|
|
else
|
|
ROS_WARN("ignoring NAN in initial covariance XX");
|
|
private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]);
|
|
if (!std::isnan(tmp_pos))
|
|
init_cov_[1] = tmp_pos;
|
|
else
|
|
ROS_WARN("ignoring NAN in initial covariance YY");
|
|
private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]);
|
|
if (!std::isnan(tmp_pos))
|
|
init_cov_[2] = tmp_pos;
|
|
else
|
|
ROS_WARN("ignoring NAN in initial covariance AA");
|
|
|
|
// ROS_INFO("Updates pose %s [%f %f %f]", private_nh_.getNamespace().c_str(), init_pose_[0], init_pose_[1], init_pose_[2]);
|
|
}
|
|
|
|
void amcl::Amcl::checkLaserReceived(const ros::TimerEvent &event)
|
|
{
|
|
ros::Duration d = ros::Time::now() - last_laser_received_ts_;
|
|
if (d > laser_check_interval_)
|
|
{
|
|
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.",
|
|
d.toSec(),
|
|
ros::names::resolve(scan_topic_).c_str());
|
|
}
|
|
}
|
|
|
|
void amcl::Amcl::requestMap()
|
|
{
|
|
boost::recursive_mutex::scoped_lock ml(configuration_mutex_);
|
|
|
|
// get map via RPC
|
|
nav_msgs::GetMap::Request req;
|
|
nav_msgs::GetMap::Response resp;
|
|
ROS_INFO("Requesting the map...");
|
|
while (!ros::service::call(map_service_, req, resp))
|
|
{
|
|
ROS_WARN("Request for map failed; trying again...");
|
|
ros::Duration d(0.5);
|
|
d.sleep();
|
|
}
|
|
handleMapMessage(resp.map);
|
|
}
|
|
|
|
void amcl::Amcl::mapReceived(const nav_msgs::OccupancyGridConstPtr &msg)
|
|
{
|
|
if (first_map_only_ && first_map_received_)
|
|
{
|
|
return;
|
|
}
|
|
|
|
handleMapMessage(*msg);
|
|
|
|
first_map_received_ = true;
|
|
}
|
|
|
|
void amcl::Amcl::handleMapMessage(const nav_msgs::OccupancyGrid &msg)
|
|
{
|
|
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
|
|
|
|
ROS_INFO("[Amcl] Received a %d X %d map @ %.3f m/pix\n",
|
|
msg.info.width,
|
|
msg.info.height,
|
|
msg.info.resolution);
|
|
|
|
if (msg.header.frame_id != global_frame_id_)
|
|
ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics",
|
|
msg.header.frame_id.c_str(),
|
|
global_frame_id_.c_str());
|
|
|
|
freeMapDependentMemory();
|
|
|
|
// Clear queued laser objects because they hold pointers to the existing
|
|
// map, #5202.
|
|
lasers_.clear();
|
|
lasers_update_.clear();
|
|
frame_to_laser_.clear();
|
|
|
|
map_ = convertMap(msg);
|
|
|
|
#if NEW_UNIFORM_SAMPLING
|
|
// Index of free space
|
|
free_space_indices.resize(0);
|
|
for (int i = 0; i < map_->size_x; i++)
|
|
for (int j = 0; j < map_->size_y; j++)
|
|
if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1)
|
|
free_space_indices.push_back(std::make_pair(i, j));
|
|
#endif
|
|
// Create the particle filter
|
|
pf_ = pf_alloc(min_particles_, max_particles_,
|
|
alpha_slow_, alpha_fast_,
|
|
(pf_init_model_fn_t)amcl::Amcl::uniformPoseGenerator,
|
|
(void *)map_);
|
|
pf_set_selective_resampling(pf_, selective_resampling_);
|
|
pf_->pop_err = pf_err_;
|
|
pf_->pop_z = pf_z_;
|
|
|
|
// Initialize the filter
|
|
updatePoseFromServer();
|
|
pf_vector_t pf_init_pose_mean = pf_vector_zero();
|
|
pf_init_pose_mean.v[0] = init_pose_[0];
|
|
pf_init_pose_mean.v[1] = init_pose_[1];
|
|
pf_init_pose_mean.v[2] = init_pose_[2];
|
|
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
|
|
pf_init_pose_cov.m[0][0] = init_cov_[0];
|
|
pf_init_pose_cov.m[1][1] = init_cov_[1];
|
|
pf_init_pose_cov.m[2][2] = init_cov_[2];
|
|
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
|
|
pf_init_ = false;
|
|
|
|
// Instantiate the sensor objects
|
|
// Odometry
|
|
delete odom_;
|
|
odom_ = new amcl::AMCLOdom();
|
|
ROS_ASSERT(odom_);
|
|
odom_->SetModel(odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
|
|
// Laser
|
|
delete laser_;
|
|
laser_ = new amcl::AMCLLaser(max_beams_, map_);
|
|
ROS_ASSERT(laser_);
|
|
if (laser_model_type_ == amcl::LASER_MODEL_BEAM)
|
|
laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
|
|
sigma_hit_, lambda_short_, 0.0);
|
|
else if (laser_model_type_ == amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB)
|
|
{
|
|
ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
|
|
laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,
|
|
laser_likelihood_max_dist_,
|
|
do_beamskip_, beam_skip_distance_,
|
|
beam_skip_threshold_, beam_skip_error_threshold_);
|
|
ROS_INFO("Done initializing likelihood field model.");
|
|
}
|
|
else
|
|
{
|
|
ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
|
|
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
|
|
laser_likelihood_max_dist_);
|
|
ROS_INFO("Done initializing likelihood field model.");
|
|
}
|
|
|
|
// In case the initial pose message arrived before the first map,
|
|
// try to apply the initial pose now that the map has arrived.
|
|
applyInitialPose();
|
|
}
|
|
|
|
void amcl::Amcl::freeMapDependentMemory()
|
|
{
|
|
if (map_ != NULL)
|
|
{
|
|
map_free(map_);
|
|
map_ = NULL;
|
|
}
|
|
if (pf_ != NULL)
|
|
{
|
|
pf_free(pf_);
|
|
pf_ = NULL;
|
|
}
|
|
odom_ = NULL;
|
|
delete odom_;
|
|
laser_ = NULL;
|
|
delete laser_;
|
|
}
|
|
|
|
/**
|
|
* Convert an OccupancyGrid map message into the internal
|
|
* representation. This allocates a map_t and returns it.
|
|
*/
|
|
map_t *
|
|
amcl::Amcl::convertMap(const nav_msgs::OccupancyGrid &map_msg)
|
|
{
|
|
map_t *map = map_alloc();
|
|
ROS_ASSERT(map);
|
|
|
|
map->size_x = map_msg.info.width;
|
|
map->size_y = map_msg.info.height;
|
|
map->scale = map_msg.info.resolution;
|
|
map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
|
|
map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
|
|
// Convert to player format
|
|
map->cells = (map_cell_t *)malloc(sizeof(map_cell_t) * map->size_x * map->size_y);
|
|
ROS_ASSERT(map->cells);
|
|
for (int i = 0; i < map->size_x * map->size_y; i++)
|
|
{
|
|
if (map_msg.data[i] == 0)
|
|
map->cells[i].occ_state = -1;
|
|
else if (map_msg.data[i] == 100)
|
|
map->cells[i].occ_state = +1;
|
|
else
|
|
map->cells[i].occ_state = 0;
|
|
}
|
|
|
|
return map;
|
|
}
|
|
|
|
bool amcl::Amcl::getOdomPose(geometry_msgs::PoseStamped &odom_pose,
|
|
double &x, double &y, double &yaw,
|
|
const ros::Time &t, const std::string &f)
|
|
{
|
|
// Get the robot's pose
|
|
geometry_msgs::PoseStamped ident;
|
|
ident.header.frame_id = stripSlash(f);
|
|
ident.header.stamp = t;
|
|
tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);
|
|
try
|
|
{
|
|
this->tf_->transform(ident, odom_pose, odom_frame_id_);
|
|
}
|
|
catch (const tf2::TransformException &e)
|
|
{
|
|
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
|
|
return false;
|
|
}
|
|
x = odom_pose.pose.position.x;
|
|
y = odom_pose.pose.position.y;
|
|
yaw = tf2::getYaw(odom_pose.pose.orientation);
|
|
|
|
return true;
|
|
}
|
|
|
|
pf_vector_t
|
|
amcl::Amcl::uniformPoseGenerator(void *arg)
|
|
{
|
|
map_t *map = (map_t *)arg;
|
|
#if NEW_UNIFORM_SAMPLING
|
|
unsigned int rand_index = drand48() * free_space_indices.size();
|
|
std::pair<int, int> free_point = free_space_indices[rand_index];
|
|
pf_vector_t p;
|
|
p.v[0] = MAP_WXGX(map, free_point.first);
|
|
p.v[1] = MAP_WYGY(map, free_point.second);
|
|
p.v[2] = drand48() * 2 * M_PI - M_PI;
|
|
#else
|
|
double min_x, max_x, min_y, max_y;
|
|
|
|
min_x = (map->size_x * map->scale) / 2.0 - map->origin_x;
|
|
max_x = (map->size_x * map->scale) / 2.0 + map->origin_x;
|
|
min_y = (map->size_y * map->scale) / 2.0 - map->origin_y;
|
|
max_y = (map->size_y * map->scale) / 2.0 + map->origin_y;
|
|
|
|
pf_vector_t p;
|
|
|
|
ROS_DEBUG("Generating new uniform sample");
|
|
for (;;)
|
|
{
|
|
p.v[0] = min_x + drand48() * (max_x - min_x);
|
|
p.v[1] = min_y + drand48() * (max_y - min_y);
|
|
p.v[2] = drand48() * 2 * M_PI - M_PI;
|
|
// Check that it's a free cell
|
|
int i, j;
|
|
i = MAP_GXWX(map, p.v[0]);
|
|
j = MAP_GYWY(map, p.v[1]);
|
|
if (MAP_VALID(map, i, j) && (map->cells[MAP_INDEX(map, i, j)].occ_state == -1))
|
|
break;
|
|
}
|
|
#endif
|
|
return p;
|
|
}
|
|
|
|
bool amcl::Amcl::globalLocalizationCallback(std_srvs::Empty::Request &req,
|
|
std_srvs::Empty::Response &res)
|
|
{
|
|
if (map_ == NULL)
|
|
{
|
|
return true;
|
|
}
|
|
boost::recursive_mutex::scoped_lock gl(configuration_mutex_);
|
|
ROS_INFO("Initializing with uniform distribution");
|
|
pf_init_model(pf_, (pf_init_model_fn_t)amcl::Amcl::uniformPoseGenerator,
|
|
(void *)map_);
|
|
ROS_INFO("Global initialisation done!");
|
|
pf_init_ = false;
|
|
return true;
|
|
}
|
|
|
|
// force nomotion updates (amcl updating without requiring motion)
|
|
bool amcl::Amcl::nomotionUpdateCallback(std_srvs::Empty::Request &req,
|
|
std_srvs::Empty::Response &res)
|
|
{
|
|
m_force_update = true;
|
|
// ROS_INFO("Requesting no-motion update");
|
|
return true;
|
|
}
|
|
|
|
bool amcl::Amcl::setMapCallback(nav_msgs::SetMap::Request &req,
|
|
nav_msgs::SetMap::Response &res)
|
|
{
|
|
handleMapMessage(req.map);
|
|
handleInitialPoseMessage(req.initial_pose);
|
|
if (force_update_after_set_map_)
|
|
{
|
|
m_force_update = true;
|
|
}
|
|
res.success = true;
|
|
return true;
|
|
}
|
|
|
|
void amcl::Amcl::laserReceived(const sensor_msgs::LaserScanConstPtr &laser_scan)
|
|
{
|
|
std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
|
|
last_laser_received_ts_ = ros::Time::now();
|
|
if (map_ == NULL)
|
|
{
|
|
return;
|
|
}
|
|
boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
|
|
int laser_index = -1;
|
|
|
|
// Do we have the base->base_laser Tx yet?
|
|
if (frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())
|
|
{
|
|
ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str());
|
|
lasers_.push_back(new amcl::AMCLLaser(*laser_));
|
|
lasers_update_.push_back(true);
|
|
laser_index = frame_to_laser_.size();
|
|
|
|
geometry_msgs::PoseStamped ident;
|
|
ident.header.frame_id = laser_scan_frame_id;
|
|
ident.header.stamp = ros::Time();
|
|
tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);
|
|
|
|
geometry_msgs::PoseStamped laser_pose;
|
|
try
|
|
{
|
|
this->tf_->transform(ident, laser_pose, base_frame_id_);
|
|
}
|
|
catch (const tf2::TransformException &e)
|
|
{
|
|
ROS_ERROR("Couldn't transform from %s to %s, "
|
|
"even though the message notifier is in use",
|
|
laser_scan_frame_id.c_str(),
|
|
base_frame_id_.c_str());
|
|
return;
|
|
}
|
|
|
|
pf_vector_t laser_pose_v;
|
|
laser_pose_v.v[0] = laser_pose.pose.position.x;
|
|
laser_pose_v.v[1] = laser_pose.pose.position.y;
|
|
// laser mounting angle gets computed later -> set to 0 here!
|
|
laser_pose_v.v[2] = 0;
|
|
lasers_[laser_index]->SetLaserPose(laser_pose_v);
|
|
ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
|
|
laser_pose_v.v[0],
|
|
laser_pose_v.v[1],
|
|
laser_pose_v.v[2]);
|
|
|
|
frame_to_laser_[laser_scan_frame_id] = laser_index;
|
|
}
|
|
else
|
|
{
|
|
// we have the laser pose, retrieve laser index
|
|
laser_index = frame_to_laser_[laser_scan_frame_id];
|
|
}
|
|
|
|
// Where was the robot when this scan was taken?
|
|
pf_vector_t pose;
|
|
if (!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
|
|
laser_scan->header.stamp, base_frame_id_))
|
|
{
|
|
ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
|
|
return;
|
|
}
|
|
|
|
pf_vector_t delta = pf_vector_zero();
|
|
|
|
if (pf_init_)
|
|
{
|
|
// Compute change in pose
|
|
// delta = pf_vector_coord_sub(pose, pf_odom_pose_);
|
|
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
|
|
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
|
|
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
|
|
|
|
// See if we should update the filter
|
|
bool update = fabs(delta.v[0]) > d_thresh_ ||
|
|
fabs(delta.v[1]) > d_thresh_ ||
|
|
fabs(delta.v[2]) > a_thresh_;
|
|
update = update || m_force_update;
|
|
m_force_update = false;
|
|
|
|
// Set the laser update flags
|
|
if (update)
|
|
for (unsigned int i = 0; i < lasers_update_.size(); i++)
|
|
lasers_update_[i] = true;
|
|
}
|
|
|
|
bool force_publication = false;
|
|
if (!pf_init_)
|
|
{
|
|
// Pose at last filter update
|
|
pf_odom_pose_ = pose;
|
|
|
|
// Filter is now initialized
|
|
pf_init_ = true;
|
|
|
|
// Should update sensor data
|
|
for (unsigned int i = 0; i < lasers_update_.size(); i++)
|
|
lasers_update_[i] = true;
|
|
|
|
force_publication = true;
|
|
|
|
resample_count_ = 0;
|
|
}
|
|
// If the robot has moved, update the filter
|
|
else if (pf_init_ && lasers_update_[laser_index])
|
|
{
|
|
// printf("pose\n");
|
|
// pf_vector_fprintf(pose, stdout, "%.3f");
|
|
|
|
amcl::AMCLOdomData odata;
|
|
odata.pose = pose;
|
|
// HACK
|
|
// Modify the delta in the action data so the filter gets
|
|
// updated correctly
|
|
odata.delta = delta;
|
|
|
|
// Use the action data to update the filter
|
|
odom_->UpdateAction(pf_, (amcl::AMCLSensorData *)&odata);
|
|
|
|
// Pose at last filter update
|
|
// this->pf_odom_pose = pose;
|
|
}
|
|
|
|
bool resampled = false;
|
|
// If the robot has moved, update the filter
|
|
if (lasers_update_[laser_index])
|
|
{
|
|
amcl::AMCLLaserData ldata;
|
|
ldata.sensor = lasers_[laser_index];
|
|
ldata.range_count = laser_scan->ranges.size();
|
|
|
|
// To account for lasers that are mounted upside-down, we determine the
|
|
// min, max, and increment angles of the laser in the base frame.
|
|
//
|
|
// Construct min and max angles of laser, in the base_link frame.
|
|
tf2::Quaternion q;
|
|
q.setRPY(0.0, 0.0, laser_scan->angle_min);
|
|
geometry_msgs::QuaternionStamped min_q, inc_q;
|
|
min_q.header.stamp = laser_scan->header.stamp;
|
|
min_q.header.frame_id = stripSlash(laser_scan->header.frame_id);
|
|
tf2::convert(q, min_q.quaternion);
|
|
|
|
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
|
|
inc_q.header = min_q.header;
|
|
tf2::convert(q, inc_q.quaternion);
|
|
try
|
|
{
|
|
tf_->transform(min_q, min_q, base_frame_id_);
|
|
tf_->transform(inc_q, inc_q, base_frame_id_);
|
|
}
|
|
catch (const tf2::TransformException &e)
|
|
{
|
|
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
|
|
e.what());
|
|
return;
|
|
}
|
|
|
|
double angle_min = tf2::getYaw(min_q.quaternion);
|
|
double angle_increment = tf2::getYaw(inc_q.quaternion) - angle_min;
|
|
|
|
// wrapping angle to [-pi .. pi]
|
|
angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI;
|
|
|
|
ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
|
|
|
|
// Apply range min/max thresholds, if the user supplied them
|
|
if (laser_max_range_ > 0.0)
|
|
ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
|
|
else
|
|
ldata.range_max = laser_scan->range_max;
|
|
double range_min;
|
|
if (laser_min_range_ > 0.0)
|
|
range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
|
|
else
|
|
range_min = laser_scan->range_min;
|
|
|
|
if (ldata.range_max <= 0.0 || range_min < 0.0)
|
|
{
|
|
ROS_ERROR("range_max or range_min from laser is negative! ignore this message.");
|
|
return; // ignore this.
|
|
}
|
|
|
|
// The AMCLLaserData destructor will free this memory
|
|
ldata.ranges = new double[ldata.range_count][2];
|
|
ROS_ASSERT(ldata.ranges);
|
|
for (int i = 0; i < ldata.range_count; i++)
|
|
{
|
|
// amcl doesn't (yet) have a concept of min range. So we'll map short
|
|
// readings to max range.
|
|
if (laser_scan->ranges[i] <= range_min)
|
|
ldata.ranges[i][0] = ldata.range_max;
|
|
else if (laser_scan->ranges[i] > ldata.range_max)
|
|
ldata.ranges[i][0] = std::numeric_limits<decltype(ldata.range_max)>::max();
|
|
else
|
|
ldata.ranges[i][0] = laser_scan->ranges[i];
|
|
// Compute bearing
|
|
ldata.ranges[i][1] = angle_min +
|
|
(i * angle_increment);
|
|
}
|
|
|
|
lasers_[laser_index]->UpdateSensor(pf_, (amcl::AMCLSensorData *)&ldata);
|
|
|
|
lasers_update_[laser_index] = false;
|
|
|
|
pf_odom_pose_ = pose;
|
|
|
|
// Resample the particles
|
|
if (!(++resample_count_ % resample_interval_))
|
|
{
|
|
pf_update_resample(pf_);
|
|
resampled = true;
|
|
}
|
|
|
|
pf_sample_set_t *set = pf_->sets + pf_->current_set;
|
|
ROS_DEBUG("Num samples: %d\n", set->sample_count);
|
|
|
|
// Publish the resulting cloud
|
|
// TODO: set maximum rate for publishing
|
|
if (!m_force_update)
|
|
{
|
|
geometry_msgs::PoseArray cloud_msg;
|
|
cloud_msg.header.stamp = ros::Time::now();
|
|
cloud_msg.header.frame_id = global_frame_id_;
|
|
cloud_msg.poses.resize(set->sample_count);
|
|
for (int i = 0; i < set->sample_count; i++)
|
|
{
|
|
cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
|
|
cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
|
|
cloud_msg.poses[i].position.z = 0;
|
|
tf2::Quaternion q;
|
|
q.setRPY(0, 0, set->samples[i].pose.v[2]);
|
|
tf2::convert(q, cloud_msg.poses[i].orientation);
|
|
}
|
|
particlecloud_pub_.publish(cloud_msg);
|
|
}
|
|
}
|
|
|
|
if (resampled || force_publication)
|
|
{
|
|
// Read out the current hypotheses
|
|
double max_weight = 0.0;
|
|
int max_weight_hyp = -1;
|
|
std::vector<amcl_hyp_t> hyps;
|
|
hyps.resize(pf_->sets[pf_->current_set].cluster_count);
|
|
for (int hyp_count = 0;
|
|
hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
|
|
{
|
|
double weight;
|
|
pf_vector_t pose_mean;
|
|
pf_matrix_t pose_cov;
|
|
if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
|
|
{
|
|
ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
|
|
break;
|
|
}
|
|
|
|
hyps[hyp_count].weight = weight;
|
|
hyps[hyp_count].pf_pose_mean = pose_mean;
|
|
hyps[hyp_count].pf_pose_cov = pose_cov;
|
|
|
|
if (hyps[hyp_count].weight > max_weight)
|
|
{
|
|
max_weight = hyps[hyp_count].weight;
|
|
max_weight_hyp = hyp_count;
|
|
}
|
|
}
|
|
|
|
if (max_weight > 0.0)
|
|
{
|
|
ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
|
|
hyps[max_weight_hyp].pf_pose_mean.v[0],
|
|
hyps[max_weight_hyp].pf_pose_mean.v[1],
|
|
hyps[max_weight_hyp].pf_pose_mean.v[2]);
|
|
|
|
/*
|
|
puts("");
|
|
pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
|
|
puts("");
|
|
*/
|
|
|
|
geometry_msgs::PoseWithCovarianceStamped p;
|
|
// Fill in the header
|
|
p.header.frame_id = global_frame_id_;
|
|
p.header.stamp = laser_scan->header.stamp;
|
|
// Copy in the pose
|
|
p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
|
|
p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
|
|
|
|
tf2::Quaternion q;
|
|
q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
|
|
tf2::convert(q, p.pose.pose.orientation);
|
|
// Copy in the covariance, converting from 3-D to 6-D
|
|
pf_sample_set_t *set = pf_->sets + pf_->current_set;
|
|
for (int i = 0; i < 2; i++)
|
|
{
|
|
for (int j = 0; j < 2; j++)
|
|
{
|
|
// Report the overall filter covariance, rather than the
|
|
// covariance for the highest-weight cluster
|
|
// p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
|
|
p.pose.covariance[6 * i + j] = set->cov.m[i][j];
|
|
}
|
|
}
|
|
// Report the overall filter covariance, rather than the
|
|
// covariance for the highest-weight cluster
|
|
// p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
|
|
p.pose.covariance[6 * 5 + 5] = set->cov.m[2][2];
|
|
|
|
/*
|
|
printf("cov:\n");
|
|
for(int i=0; i<6; i++)
|
|
{
|
|
for(int j=0; j<6; j++)
|
|
printf("%6.3f ", p.covariance[6*i+j]);
|
|
puts("");
|
|
}
|
|
*/
|
|
|
|
pose_pub_.publish(p);
|
|
last_published_pose = p;
|
|
|
|
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
|
|
hyps[max_weight_hyp].pf_pose_mean.v[0],
|
|
hyps[max_weight_hyp].pf_pose_mean.v[1],
|
|
hyps[max_weight_hyp].pf_pose_mean.v[2]);
|
|
|
|
// subtracting base to odom from map to base and send map to odom instead
|
|
geometry_msgs::PoseStamped odom_to_map;
|
|
try
|
|
{
|
|
tf2::Quaternion q;
|
|
q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
|
|
tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
|
|
hyps[max_weight_hyp].pf_pose_mean.v[1],
|
|
0.0));
|
|
|
|
geometry_msgs::PoseStamped tmp_tf_stamped;
|
|
tmp_tf_stamped.header.frame_id = base_frame_id_;
|
|
tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
|
|
tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);
|
|
|
|
this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
|
|
}
|
|
catch (const tf2::TransformException &)
|
|
{
|
|
ROS_DEBUG("Failed to subtract base to odom transform");
|
|
return;
|
|
}
|
|
|
|
tf2::convert(odom_to_map.pose, latest_tf_);
|
|
latest_tf_valid_ = true;
|
|
|
|
if (tf_broadcast_ == true)
|
|
{
|
|
// We want to send a transform that is good up until a
|
|
// tolerance time so that odom can be used
|
|
ros::Time transform_expiration = (laser_scan->header.stamp +
|
|
transform_tolerance_);
|
|
geometry_msgs::TransformStamped tmp_tf_stamped;
|
|
tmp_tf_stamped.header.frame_id = global_frame_id_;
|
|
tmp_tf_stamped.header.stamp = transform_expiration;
|
|
tmp_tf_stamped.child_frame_id = odom_frame_id_;
|
|
tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
|
|
if (transform_expiration - last_tfb_time_ > ros::Duration(0.02))
|
|
{
|
|
// ROS_INFO("%s -> %s %.6f %.6f", global_frame_id_.c_str(), odom_frame_id_.c_str(),
|
|
// tmp_tf_stamped.header.stamp.toSec(), last_tfb_time_.toSec());
|
|
this->tfb_->sendTransform(tmp_tf_stamped);
|
|
sent_first_transform_ = true;
|
|
last_tfb_time_ = transform_expiration;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
ROS_ERROR("No pose!");
|
|
}
|
|
}
|
|
else if (latest_tf_valid_)
|
|
{
|
|
if (tf_broadcast_ == true)
|
|
{
|
|
// Nothing changed, so we'll just republish the last transform, to keep
|
|
// everybody happy.
|
|
ros::Time transform_expiration = (laser_scan->header.stamp +
|
|
transform_tolerance_);
|
|
geometry_msgs::TransformStamped tmp_tf_stamped;
|
|
tmp_tf_stamped.header.frame_id = global_frame_id_;
|
|
tmp_tf_stamped.header.stamp = transform_expiration;
|
|
tmp_tf_stamped.child_frame_id = odom_frame_id_;
|
|
tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
|
|
if (transform_expiration - last_tfb_time_ > ros::Duration(0.02))
|
|
{
|
|
// ROS_INFO("%s -> %s %.6f %.6f", global_frame_id_.c_str(), odom_frame_id_.c_str(),
|
|
// tmp_tf_stamped.header.stamp.toSec(), last_tfb_time_.toSec());
|
|
this->tfb_->sendTransform(tmp_tf_stamped);
|
|
last_tfb_time_ = transform_expiration;
|
|
}
|
|
}
|
|
|
|
// Is it time to save our last pose to the param server
|
|
ros::Time now = ros::Time::now();
|
|
if ((save_pose_period.toSec() > 0.0) &&
|
|
(now - save_pose_last_time) >= save_pose_period)
|
|
{
|
|
geometry_msgs::PoseWithCovarianceStamped amcl_pose;
|
|
this->savePoseToServer(amcl_pose);
|
|
save_pose_last_time = now;
|
|
}
|
|
}
|
|
|
|
diagnosic_updater_.update();
|
|
}
|
|
|
|
void amcl::Amcl::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
|
|
{
|
|
handleInitialPoseMessage(*msg);
|
|
if (force_update_after_initialpose_)
|
|
{
|
|
m_force_update = true;
|
|
}
|
|
}
|
|
|
|
void amcl::Amcl::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped &msg)
|
|
{
|
|
boost::recursive_mutex::scoped_lock prl(configuration_mutex_);
|
|
if (msg.header.frame_id == "")
|
|
{
|
|
// This should be removed at some point
|
|
ROS_WARN("Received initial pose with empty frame_id. You should always supply a frame_id.");
|
|
}
|
|
// We only accept initial pose estimates in the global frame, #5148.
|
|
else if (stripSlash(msg.header.frame_id) != global_frame_id_)
|
|
{
|
|
ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
|
|
stripSlash(msg.header.frame_id).c_str(),
|
|
global_frame_id_.c_str());
|
|
return;
|
|
}
|
|
|
|
// In case the client sent us a pose estimate in the past, integrate the
|
|
// intervening odometric change.
|
|
geometry_msgs::TransformStamped tx_odom;
|
|
try
|
|
{
|
|
// wait a little for the latest tf to become available
|
|
tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,
|
|
base_frame_id_, ros::Time::now(),
|
|
odom_frame_id_, ros::Duration(0.5));
|
|
}
|
|
catch (const tf2::TransformException &e)
|
|
{
|
|
// If we've never sent a transform, then this is normal, because the
|
|
// global_frame_id_ frame doesn't exist. We only care about in-time
|
|
// transformation for on-the-move pose-setting, so ignoring this
|
|
// startup condition doesn't really cost us anything.
|
|
if (sent_first_transform_)
|
|
ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
|
|
tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform);
|
|
}
|
|
|
|
tf2::Transform tx_odom_tf2;
|
|
tf2::convert(tx_odom.transform, tx_odom_tf2);
|
|
tf2::Transform pose_old, pose_new;
|
|
tf2::convert(msg.pose.pose, pose_old);
|
|
pose_new = pose_old * tx_odom_tf2;
|
|
|
|
// Transform into the global frame
|
|
|
|
ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
|
|
ros::Time::now().toSec(),
|
|
pose_new.getOrigin().x(),
|
|
pose_new.getOrigin().y(),
|
|
tf2::getYaw(pose_new.getRotation()));
|
|
// Re-initialize the filter
|
|
pf_vector_t pf_init_pose_mean = pf_vector_zero();
|
|
pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
|
|
pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
|
|
pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation());
|
|
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
|
|
// Copy in the covariance, converting from 6-D to 3-D
|
|
for (int i = 0; i < 2; i++)
|
|
{
|
|
for (int j = 0; j < 2; j++)
|
|
{
|
|
pf_init_pose_cov.m[i][j] = msg.pose.covariance[6 * i + j];
|
|
}
|
|
}
|
|
pf_init_pose_cov.m[2][2] = msg.pose.covariance[6 * 5 + 5];
|
|
|
|
delete initial_pose_hyp_;
|
|
initial_pose_hyp_ = new amcl_hyp_t();
|
|
initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
|
|
initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
|
|
applyInitialPose();
|
|
}
|
|
|
|
/**
|
|
* If initial_pose_hyp_ and map_ are both non-null, apply the initial
|
|
* pose to the particle filter state. initial_pose_hyp_ is deleted
|
|
* and set to NULL after it is used.
|
|
*/
|
|
void amcl::Amcl::applyInitialPose()
|
|
{
|
|
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
|
|
if (initial_pose_hyp_ != NULL && map_ != NULL)
|
|
{
|
|
pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
|
|
pf_init_ = false;
|
|
|
|
delete initial_pose_hyp_;
|
|
initial_pose_hyp_ = NULL;
|
|
}
|
|
}
|
|
|
|
void amcl::Amcl::standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status)
|
|
{
|
|
double std_x = sqrt(last_published_pose.pose.covariance[6 * 0 + 0]);
|
|
double std_y = sqrt(last_published_pose.pose.covariance[6 * 1 + 1]);
|
|
double std_yaw = sqrt(last_published_pose.pose.covariance[6 * 5 + 5]);
|
|
|
|
diagnostic_status.add("std_x", std_x);
|
|
diagnostic_status.add("std_y", std_y);
|
|
diagnostic_status.add("std_yaw", std_yaw);
|
|
diagnostic_status.add("std_warn_level_x", std_warn_level_x_);
|
|
diagnostic_status.add("std_warn_level_y", std_warn_level_y_);
|
|
diagnostic_status.add("std_warn_level_yaw", std_warn_level_yaw_);
|
|
|
|
if (std_x > std_warn_level_x_ || std_y > std_warn_level_y_ || std_yaw > std_warn_level_yaw_)
|
|
{
|
|
diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Too large");
|
|
}
|
|
else
|
|
{
|
|
diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
|
|
}
|
|
}
|