Added libuvc_camera nodelet

libuvc_camera/CMakeLists.txt

@@ -36,3 +36,5 @@ gencfg()
 rosbuild_add_executable(camera_node src/main.cpp src/camera_driver.cpp)
 target_link_libraries(camera_node uvc)
 
+rosbuild_add_library(libuvc_camera_nodelet src/nodelet.cpp src/camera_driver.cpp)
+target_link_libraries(libuvc_camera_nodelet uvc)

libuvc_camera/libuvc_camera_nodelet.xml

+<library path="lib/liblibuvc_camera_nodelet">
+  <class name="libuvc_camera/driver"
+         type="libuvc_camera::CameraNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description> 
+      UVC camera driver nodelet.
+    </description>
+  </class>
+</library>

libuvc_camera/manifest.xml

@@ -13,8 +13,12 @@
   <depend package="driver_base"/>
   <depend package="dynamic_reconfigure"/>
   <depend package="image_transport"/>
+  <depend package="nodelet"/>
   <depend package="sensor_msgs"/>
   <depend package="libuvc"/>
+  <export>
+    <nodelet plugin="${prefix}/libuvc_camera_nodelet.xml"/>
+  </export>
 </package>
 
 

libuvc_camera/src/camera_driver.cpp

@@ -125,15 +125,18 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) {
     return;
   }
 
-  sensor_msgs::Image image;
-  image.width = config_.width;
-  image.height = config_.height;
-  image.encoding = "rgb8";
-  image.step = image.width * 3;
-  image.data.resize(image.step * image.height);
-  memcpy(&(image.data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
-
-  cam_pub_.publish(image, cinfo_manager_.getCameraInfo());
+  sensor_msgs::Image::Ptr image(new sensor_msgs::Image());
+  image->width = config_.width;
+  image->height = config_.height;
+  image->encoding = "rgb8";
+  image->step = image->width * 3;
+  image->data.resize(image->step * image->height);
+  memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
+
+  sensor_msgs::CameraInfo::ConstPtr cinfo(
+    new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
+
+  cam_pub_.publish(image, cinfo);
 
   if (config_changed_) {
     config_server_.updateConfig(config_);

libuvc_camera/src/nodelet.cpp

+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include "libuvc_camera/camera_driver.h"
+
+namespace libuvc_camera {
+
+class CameraNodelet : public nodelet::Nodelet {
+public:
+  CameraNodelet() : running_(false) {}
+  ~CameraNodelet();
+
+private:
+  virtual void onInit();
+
+  volatile bool running_;
+  boost::shared_ptr<CameraDriver> driver_;
+};
+
+CameraNodelet::~CameraNodelet() {
+  if (running_) {
+    driver_->Stop();
+  }
+}
+
+void CameraNodelet::onInit() {
+  ros::NodeHandle nh(getNodeHandle());
+  ros::NodeHandle priv_nh(getPrivateNodeHandle());
+
+  driver_.reset(new CameraDriver(nh, priv_nh));
+  if (driver_->Start()) {
+    running_ = true;
+  } else {
+    NODELET_ERROR("Unable to open camera.");
+    driver_.reset();
+  }
+}
+
+};
+
+// Register this plugin with pluginlib.
+//
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(libuvc_camera, driver,
+                        libuvc_camera::CameraNodelet, nodelet::Nodelet);