DogPositionMeasurer() :
pnh("~"),
meanPositionDeviation(0),
m2PositionDeviation(0),
meanTimeDuration(0),
m2TimeDuration(0),
meanUnknownTimeDuration(0),
m2UnknownTimeDuration(0),
n(0),
unknownN(0),
startMeasuringSub(nh, "start_measuring", 1),
stopMeasuringSub(nh, "stop_measuring", 1) {
pnh.param<string>("model_name", modelName, "dog");
// Setup the subscriber
dogSub.reset(
new message_filters::Subscriber<dogsim::DogPosition>(nh,
"/dog_position_detector/dog_position", 1));
dogSub->unsubscribe();
dogSub->registerCallback(boost::bind(&DogPositionMeasurer::callback, this, _1));
startMeasuringSub.registerCallback(
boost::bind(&DogPositionMeasurer::startMeasuring, this, _1));
stopMeasuringSub.registerCallback(
boost::bind(&DogPositionMeasurer::stopMeasuring, this, _1));
}
/**
* Constructor, subscribes to input topics using image transport and registers
* callbacks.
* \param transport The image transport to use
*/
StereoProcessor(const std::string& transport) :
left_received_(0), right_received_(0), left_info_received_(0), right_info_received_(0), all_received_(0)
{
// Read local parameters
ros::NodeHandle local_nh("~");
// Resolve topic names
ros::NodeHandle nh;
std::string stereo_ns = nh.resolveName("stereo");
std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
std::string left_info_topic = stereo_ns + "/left/camera_info";
std::string right_info_topic = stereo_ns + "/right/camera_info";
// Subscribe to four input topics.
ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s",
left_topic.c_str(), right_topic.c_str(),
left_info_topic.c_str(), right_info_topic.c_str());
image_transport::ImageTransport it(nh);
left_sub_.subscribe(it, left_topic, 1, transport);
right_sub_.subscribe(it, right_topic, 1, transport);
left_info_sub_.subscribe(nh, left_info_topic, 1);
right_info_sub_.subscribe(nh, right_info_topic, 1);
// Complain every 15s if the topics appear unsynchronized
left_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_received_));
right_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_received_));
left_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_info_received_));
right_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_info_received_));
check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
boost::bind(&StereoProcessor::checkInputsSynchronized, this));
// Synchronize input topics. Optionally do approximate synchronization.
local_nh.param("queue_size", queue_size_, 5);
bool approx;
local_nh.param("approximate_sync", approx, false);
if (approx)
{
approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_),
left_sub_, right_sub_, left_info_sub_, right_info_sub_) );
approximate_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4));
}
else
{
exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_),
left_sub_, right_sub_, left_info_sub_, right_info_sub_) );
exact_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4));
}
}
/**
* Constructor, subscribes to input topics using image transport and registers
* callbacks.
* \param transport The image transport to use
*/
MonoDepthProcessor(const std::string& transport) :
image_received_(0), depth_received_(0), image_info_received_(0), depth_info_received_(0), all_received_(0)
{
// Read local parameters
ros::NodeHandle local_nh("~");
// Resolve topic names
ros::NodeHandle nh;
std::string camera_ns = nh.resolveName("camera");
std::string image_topic = ros::names::clean(camera_ns + "/rgb/image_rect");
std::string depth_topic = ros::names::clean(camera_ns + "/depth_registered/image_rect");
std::string image_info_topic = camera_ns + "/rgb/camera_info";
std::string depth_info_topic = camera_ns + "/depth_registered/camera_info";
// Subscribe to four input topics.
ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s",
image_topic.c_str(), depth_topic.c_str(),
image_info_topic.c_str(), depth_info_topic.c_str());
image_transport::ImageTransport it(nh);
image_sub_.subscribe(it, image_topic, 1, transport);
depth_sub_.subscribe(it, depth_topic, 1, transport);
image_info_sub_.subscribe(nh, image_info_topic, 1);
depth_info_sub_.subscribe(nh, depth_info_topic, 1);
// Complain every 15s if the topics appear unsynchronized
image_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &image_received_));
depth_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &depth_received_));
image_info_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &image_info_received_));
depth_info_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &depth_info_received_));
check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
boost::bind(&MonoDepthProcessor::checkInputsSynchronized, this));
// Synchronize input topics. Optionally do approximate synchronization.
local_nh.param("queue_size", queue_size_, 5);
bool approx;
local_nh.param("approximate_sync", approx, true);
if (approx)
{
approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_),
image_sub_, depth_sub_, image_info_sub_, depth_info_sub_) );
approximate_sync_->registerCallback(boost::bind(&MonoDepthProcessor::dataCb, this, _1, _2, _3, _4));
}
else
{
exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_),
image_sub_, depth_sub_, image_info_sub_, depth_info_sub_) );
exact_sync_->registerCallback(boost::bind(&MonoDepthProcessor::dataCb, this, _1, _2, _3, _4));
}
}
laserScanToSamgar(ros::NodeHandle n) :
n_(n),
laser_sub_(n_, "base_scan", 10)
{
laser_sub_.registerCallback(boost::bind(&laserScanToSamgar::scanCallback, this, _1));
//scan_pub_ = n_.advertise<sensor_msgs::PointCloud>("/my_cloud",1);
}
