void SetUp()
{
success = false;
failure = false;
msg_i = -1;
ASSERT_TRUE(g_argc == 3);
msg_count = atoi(g_argv[1]);
dt.fromSec(atof(g_argv[2]));
}
int
main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "sub_pub");
if(argc != 3)
{
puts(USAGE);
return -1;
}
g_msg_count = atoi(argv[1]);
g_dt.fromSec(atof(argv[2]));
ros::NodeHandle nh;
return RUN_ALL_TESTS();
}
void AmclNode::reconfigureCB(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_ = LASER_MODEL_BEAM;
else if(config.laser_model_type == "likelihood_field")
laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;
if(config.odom_model_type == "diff")
odom_model_type_ = ODOM_MODEL_DIFF;
else if(config.odom_model_type == "omni")
odom_model_type_ = ODOM_MODEL_OMNI;
else if(config.odom_model_type == "diff-corrected")
odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED;
else if(config.odom_model_type == "omni-corrected")
odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED;
if(config.min_particles > config.max_particles)
{
ROS_WARN("You've set min_particles to be less 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;
pf_ = pf_alloc(min_particles_, max_particles_,
alpha_slow_, alpha_fast_,
(pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
(void *)map_);
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] = tf::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 AMCLOdom();
ROS_ASSERT(odom_);
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
// Laser
delete laser_;
laser_ = new AMCLLaser(max_beams_, map_);
ROS_ASSERT(laser_);
if(laser_model_type_ == LASER_MODEL_BEAM)
laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
sigma_hit_, lambda_short_, 0.0);
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.");
}
odom_frame_id_ = config.odom_frame_id;
base_frame_id_ = config.base_frame_id;
global_frame_id_ = config.global_frame_id;
delete laser_scan_filter_;
laser_scan_filter_ =
new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
*tf_,
odom_frame_id_,
100);
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
this, _1));
initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
}