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);
	}
Пример #2
0
 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);
 }
Пример #3
0
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_));
}