From 2bc8f76c4f4bc87731d0c82361bc2908abc7dba0 Mon Sep 17 00:00:00 2001
From: Ken Tossell <ken@tossell.net>
Date: Thu, 16 Feb 2012 20:06:13 -0500
Subject: [PATCH] Added libuvc_camera nodelet
---
libuvc_camera/CMakeLists.txt | 2 ++
libuvc_camera/libuvc_camera_nodelet.xml | 9 +++++
libuvc_camera/manifest.xml | 4 +++
libuvc_camera/src/camera_driver.cpp | 21 ++++++-----
libuvc_camera/src/nodelet.cpp | 46 +++++++++++++++++++++++++
5 files changed, 73 insertions(+), 9 deletions(-)
create mode 100644 libuvc_camera/libuvc_camera_nodelet.xml
create mode 100644 libuvc_camera/src/nodelet.cpp
diff --git a/libuvc_camera/CMakeLists.txt b/libuvc_camera/CMakeLists.txt
index 272cdfa..ce17734 100644
--- a/libuvc_camera/CMakeLists.txt
+++ b/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)
diff --git a/libuvc_camera/libuvc_camera_nodelet.xml b/libuvc_camera/libuvc_camera_nodelet.xml
new file mode 100644
index 0000000..c236305
--- /dev/null
+++ b/libuvc_camera/libuvc_camera_nodelet.xml
@@ -0,0 +1,9 @@
+<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>
diff --git a/libuvc_camera/manifest.xml b/libuvc_camera/manifest.xml
index 9e1086e..80dd070 100644
--- a/libuvc_camera/manifest.xml
+++ b/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>
diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp
index 1b476d0..82989c0 100644
--- a/libuvc_camera/src/camera_driver.cpp
+++ b/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_);
diff --git a/libuvc_camera/src/nodelet.cpp b/libuvc_camera/src/nodelet.cpp
new file mode 100644
index 0000000..0850195
--- /dev/null
+++ b/libuvc_camera/src/nodelet.cpp
@@ -0,0 +1,46 @@
+#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);
--
GitLab