diff --git a/libuvc_camera/CMakeLists.txt b/libuvc_camera/CMakeLists.txt index 272cdfa112cd72e4e646087b526bffe4ef506f33..ce1773481c9b94e3702ce4aeba845e20acddb9ce 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 0000000000000000000000000000000000000000..c236305f1ecccd09a33947ef2f3be18b72b72c38 --- /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 9e1086e1926fef9d79395410f819a76ce088b08a..80dd070937b4a1bc2330bd2d6b72ebd444226bea 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 1b476d0957773d7ac1c23416b3bca07cd510ee6e..82989c0f25fb2b3fb3c35ea5fe8d31c7974b9eba 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 0000000000000000000000000000000000000000..0850195478d83a205c589260231a22fd3c41a0a8 --- /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);