usb_cam_hardware_nodelet.hpp
Go to the documentation of this file.
1 #ifndef USB_CAM_HARDWARE_USB_CAM_HARDWARE_NODELET
2 #define USB_CAM_HARDWARE_USB_CAM_HARDWARE_NODELET
3 
5 #include <nodelet/nodelet.h>
6 #include <ros/duration.h>
7 #include <ros/exception.h>
8 #include <ros/node_handle.h>
9 #include <ros/time.h>
10 #include <ros/timer.h>
12 
13 #include <boost/make_shared.hpp>
14 #include <boost/shared_ptr.hpp>
15 
16 namespace usb_cam_hardware {
17 
19 public:
21 
23  update_timer_.stop(); // stop updating the controllers and hardware first
24  controllers_.reset(); // unload the controllers using the hardware
25  hardware_.reset(); // finally close the hardware
26  }
27 
28 private:
29  virtual void onInit() {
31 
32  hardware_ = boost::make_shared< USBCamHardware >();
33  const ros::Duration time_per_frame(hardware_->init(pnh));
34  if (time_per_frame <= ros::Duration(0.)) {
35  ROS_ERROR("Cannot init usb cam hardware");
36  throw ros::Exception("Cannot init usb cam hardware");
37  }
38 
39  controllers_ = boost::make_shared< controller_manager::ControllerManager >(hardware_.get(), nh);
40 
41  update_timer_ = nh.createTimer(time_per_frame, &USBCamHardwareNodelet::update, this);
42 
44  }
45 
46  void update(const ros::TimerEvent &event) {
47  const ros::Time now(ros::Time::now());
48  const ros::Duration period(now - last_);
49  hardware_->read(now, period);
50  controllers_->update(now, period);
51  hardware_->write(now, period);
52  last_ = now;
53  }
54 
55 private:
59 
61 };
62 
63 } // namespace usb_cam_hardware
64 
65 #endif
boost::shared_ptr< controller_manager::ControllerManager > controllers_
void stop()
ros::NodeHandle & getMTNodeHandle() const
ros::NodeHandle & getMTPrivateNodeHandle() const
void update(const ros::TimerEvent &event)
static Time now()
#define ROS_ERROR(...)
boost::shared_ptr< USBCamHardware > hardware_


usb_cam_hardware
Author(s):
autogenerated on Tue Jul 14 2020 03:12:10