TrajGenerator(ros::NodeHandle nh) :
nh_(nh),
people_sub_(nh_,"people_tracker_measurements",10),
people_notifier_(people_sub_,tfl_,fixed_frame,10)
{
people_notifier_.registerCallback(boost::bind(&TrajGenerator::peopleCallback, this, _1));
people_notifier_.setTolerance(ros::Duration(0.01));
markers_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 20);
}
LaserScanToPointCloud(ros::NodeHandle n) :
n_(n),
laser_sub_(n_, "/robot_1/base_scan", 10),
laser_notifier_(laser_sub_,listener_, "/robot_1/base_link", 10)
{
laser_notifier_.registerCallback(
boost::bind(&LaserScanToPointCloud::scanCallback, this, _1));
laser_notifier_.setTolerance(ros::Duration(0.01));
scan_pub_ = n_.advertise<sensor_msgs::PointCloud>("/my_cloud",1);
}
PCFilter() :
nh_(),
point_cloud_processed_(true),
tolerance_(0.04)
{
m_pointCloudSub = new message_filters::Subscriber<sensor_msgs::PointCloud2> (nh_, "cloud_in", 5);
m_tfPointCloudSub = new tf::MessageFilter<sensor_msgs::PointCloud2> (*m_pointCloudSub, tf_listener_, "world", 5);
m_tfPointCloudSub->registerCallback(boost::bind(&PCFilter::insertCloudCallback, this, _1));
pub_pc_ = nh_.advertise<sensor_msgs::PointCloud2 >("cloud_out", 10);
}
LaserScanReceiver(ros::NodeHandle node, std::string tf, std::string topic) :
n(node),
base_tf(tf),
laser_topic(topic),
laser_sub(n, topic, 10),
laser_notifier(laser_sub,listener, tf, 10)
{
laser_notifier.registerCallback(&LaserScanReceiver::scanCallback, this);
laser_notifier.setTolerance(ros::Duration(0.2));
laser_count = 0;
pcl_pub = node.advertise<sensor_msgs::PointCloud>("laser_pcl",1);
}
CollisionMapTest(): private_handle_("~")
{
done_ = false;
num_msgs_ = 0;
result_ = true;
// ROS_INFO("Private handle: %s, %s",private_handle_.getName(), private_handle_.getNamespace());
private_handle_.param<double>("min_z_threshold",min_z_threshold_,MIN_Z_THRESHOLD);
private_handle_.param<double>("max_z_threshold",max_z_threshold_,MAX_Z_THRESHOLD);
private_handle_.param<int>("test_num_collision_msgs",test_num_msgs_,TEST_NUM_MSGS);
private_handle_.param<std::string>("collision_map_frame",collision_map_frame_,COLLISION_MAP_FRAME);
cloud_subscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionMap>(node_,COLLISION_MAP_TOPIC,50);
cloud_notifier_ = new tf::MessageFilter<arm_navigation_msgs::CollisionMap>(*cloud_subscriber_,tf_,collision_map_frame_,50);
cloud_notifier_->registerCallback(boost::bind(&CollisionMapTest::collisionCallback,this,_1));
}
//Nodelet initialization
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
private_nh.getParam("min_height", min_height_);
private_nh.getParam("max_height", max_height_);
private_nh.getParam("base_frame", baseFrame);
private_nh.getParam("laser_frame", laserFrame);
NODELET_INFO("CloudToScanHoriz min_height: %f, max_height: %f", min_height_, max_height_);
NODELET_INFO("CloudToScanHoriz baseFrame: %s, laserFrame: %s", baseFrame.c_str(), laserFrame.c_str());
//Set up to process new pointCloud and tf data together
cloudSubscriber.subscribe(nh, "cloud_in", 5);
tfFilter = new tf::MessageFilter<PointCloud>(cloudSubscriber, tfListener, laserFrame, 1);
tfFilter->registerCallback(boost::bind(&CloudToScanHoriz::callback, this, _1));
tfFilter->setTolerance(ros::Duration(0.05));
laserPublisher = nh.advertise<sensor_msgs::LaserScan>("laserScanHoriz", 10);
};
ScanToPointCloud2For2Lasers(ros::NodeHandle n) :
_nodeHandle(n),
target_frame("/rotary_base"),
_laser_sub(_nodeHandle, "/scan",300),
_laser_tfMessagefilter(_laser_sub, _tfListener, "/laser", 300),
producedPcdCount(0),
maxToProducePcdCount(5),
bRunning(true),
isSkipThisCloud(true), // set it to true to skip the first cloud
_the_last_1st_yaw(0),
_the_last_2nd_yaw(0)
{
_laser_tfMessagefilter.setTolerance(ros::Duration(0.1));
_laser_tfMessagefilter.registerCallback(boost::bind(&ScanToPointCloud2For2Lasers::scanCallback, this, _1));
_scan_pub = _nodeHandle.advertise<sensor_msgs::PointCloud2> ("/cloud", 100);
std::stringstream pid;
pid << getpid();
pcd_prefix = pid.str();
}
HeadTransformer() : tf_(), target_frame_("head") {
point_sub_.subscribe(n_, "/head/pose", 10);
tf_filter_ = new tf::MessageFilter<geometry_msgs::PoseStamped>(point_sub_, tf_, target_frame_, 10);
tf_filter_->registerCallback( boost::bind( &HeadTransformer::msgCallback, this, _1) );
} ;
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);
}
