NavGraphRosRetriever(ros::NodeHandle &n)
: n(n)
{
ros::NodeHandle privn("~");
GET_CONFIG(privn, n, "navgraph_file", cfg_navgraph_file_);
if (cfg_navgraph_file_.empty()) {
throw std::runtime_error("No navgraph file given");
}
boost::filesystem::path p(cfg_navgraph_file_);
p = boost::filesystem::absolute(p);
cfg_navgraph_file_ = p.string();
ROS_INFO("Using navgraph path: %s", p.string().c_str());
if (boost::filesystem::exists(p)) {
boost::filesystem::remove(p);
}
boost::filesystem::create_directories(p.parent_path());
sub_has_received_ = false;
sub_navgraph_ = n.subscribe<fawkes_msgs::NavGraph>("navgraph", 10, &NavGraphRosRetriever::cb_navgraph, this);
sub_init_timer_ =
n.createWallTimer(ros::WallDuration(5.0), &NavGraphRosRetriever::cb_sub_init_timer, this);
}
SubscriberTest()
{
// Setup a one-shot timer to initialize the node after a brief
// delay so that /rosout is always fully initialized.
ROS_INFO("Starting initialization timer...");
init_timer_ = nh_.createWallTimer(ros::WallDuration(1.0),
&SubscriberTest::initialize,
this,
true);
}
CameraSubscriber::CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh,
const std::string& base_topic, uint32_t queue_size,
const Callback& callback, const ros::VoidPtr& tracked_object,
const TransportHints& transport_hints)
: impl_(new Impl(queue_size))
{
// Must explicitly remap the image topic since we then do some string manipulation on it
// to figure out the sibling camera_info topic.
std::string image_topic = info_nh.resolveName(base_topic);
std::string info_topic = getCameraInfoTopic(image_topic);
impl_->image_sub_.subscribe(image_it, image_topic, queue_size, transport_hints);
impl_->info_sub_ .subscribe(info_nh, info_topic, queue_size, transport_hints.getRosHints());
impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_);
// need for Boost.Bind here is kind of broken
impl_->sync_.registerCallback(boost::bind(callback, _1, _2));
// Complain every 10s if it appears that the image and info topics are not synchronized
impl_->image_sub_.registerCallback(boost::bind(increment, &impl_->image_received_));
impl_->info_sub_.registerCallback(boost::bind(increment, &impl_->info_received_));
impl_->sync_.registerCallback(boost::bind(increment, &impl_->both_received_));
impl_->check_synced_timer_ = info_nh.createWallTimer(ros::WallDuration(10.0),
boost::bind(&Impl::checkImagesSynchronized, impl_));
}