Ejemplo n.º 1
0
    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]));
    }
Ejemplo n.º 2
0
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();
}
Ejemplo n.º 3
0
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);
}