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);
}
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);
}
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);
}
//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);
};
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);
}
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();
}
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));
}
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);
}
void AmclNode::runFromBag(const std::string &in_bag_fn)
{
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 map...");
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0));
}
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_->getBuffer().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;
tf::Matrix3x3(tf::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();
}
