From 5b86f9b099b5e8904652b31e6c0df31ed70a14ed Mon Sep 17 00:00:00 2001 From: Will Baker Date: Tue, 5 Jun 2018 15:19:31 -0500 Subject: [PATCH] Add nodelet --- CMakeLists.txt | 14 +++++++++++-- include/avt_vimba_camera/mono_camera.h | 2 +- .../avt_vimba_camera/mono_camera_nodelet.h | 16 +++++++++++++++ .../avt_vimba_camera/stereo_camera_nodelet.h | 16 +++++++++++++++ launch/mono_camera_nodelet.launch | 5 +++++ package.xml | 6 ++++++ plugins.xml | 12 +++++++++++ src/mono_camera.cpp | 4 ++-- src/nodes/mono_camera_nodelet.cpp | 18 +++++++++++++++++ src/nodes/stereo_camera_nodelet.cpp | 20 +++++++++++++++++++ 10 files changed, 108 insertions(+), 5 deletions(-) create mode 100644 include/avt_vimba_camera/mono_camera_nodelet.h create mode 100644 include/avt_vimba_camera/stereo_camera_nodelet.h create mode 100644 launch/mono_camera_nodelet.launch create mode 100644 plugins.xml create mode 100644 src/nodes/mono_camera_nodelet.cpp create mode 100644 src/nodes/stereo_camera_nodelet.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a5e45d6b..81af9007 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs polled_camera + nodelet ) #Get architecture @@ -113,12 +114,20 @@ add_executable(sync_node add_dependencies_and_linkings(sync_node) +add_library(avt_camera_nodelets + src/nodes/mono_camera_nodelet.cpp + src/nodes/stereo_camera_nodelet.cpp + src/stereo_camera.cpp + src/avt_vimba_camera.cpp + src/frame_observer.cpp) +add_dependencies_and_linkings(avt_camera_nodelets) + ############# ## Install ## ############# ## Mark executables and/or libraries for installation -install(TARGETS mono_camera_node stereo_camera_node +install(TARGETS mono_camera_node stereo_camera_node avt_camera_nodelets ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -133,7 +142,9 @@ install(DIRECTORY include ## Mark other files for installation (e.g. launch and bag files, etc.) install(FILES + plugins.xml launch/mono_camera.launch + launch/mono_camera_nodelet.launch launch/stereo_camera_one_node.launch launch/stereo_camera_two_nodes.launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} @@ -164,4 +175,3 @@ elseif("${ARCH}" STREQUAL armv8) DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) endif() - diff --git a/include/avt_vimba_camera/mono_camera.h b/include/avt_vimba_camera/mono_camera.h index 0b6e6b70..97fb6109 100644 --- a/include/avt_vimba_camera/mono_camera.h +++ b/include/avt_vimba_camera/mono_camera.h @@ -49,7 +49,7 @@ namespace avt_vimba_camera { class MonoCamera { public: - MonoCamera(ros::NodeHandle nh, ros::NodeHandle nhp); + MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp); ~MonoCamera(void); private: diff --git a/include/avt_vimba_camera/mono_camera_nodelet.h b/include/avt_vimba_camera/mono_camera_nodelet.h new file mode 100644 index 00000000..7b3c761a --- /dev/null +++ b/include/avt_vimba_camera/mono_camera_nodelet.h @@ -0,0 +1,16 @@ +#include +#include "avt_vimba_camera/mono_camera.h" + +namespace avt_vimba_camera +{ + + class MonoCameraNodelet : public nodelet::Nodelet + { + public: + virtual void onInit(); + virtual ~MonoCameraNodelet(); + private: + MonoCamera* camera_; + }; + +} diff --git a/include/avt_vimba_camera/stereo_camera_nodelet.h b/include/avt_vimba_camera/stereo_camera_nodelet.h new file mode 100644 index 00000000..c068c6d8 --- /dev/null +++ b/include/avt_vimba_camera/stereo_camera_nodelet.h @@ -0,0 +1,16 @@ +#include +#include "avt_vimba_camera/stereo_camera.h" + +namespace avt_vimba_camera +{ + + class StereoCameraNodelet : public nodelet::Nodelet + { + public: + virtual void onInit(); + virtual ~StereoCameraNodelet(); + private: + StereoCamera* camera_; + }; + +} diff --git a/launch/mono_camera_nodelet.launch b/launch/mono_camera_nodelet.launch new file mode 100644 index 00000000..9c1de795 --- /dev/null +++ b/launch/mono_camera_nodelet.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/package.xml b/package.xml index 423bd4dc..2e2feaae 100644 --- a/package.xml +++ b/package.xml @@ -24,6 +24,7 @@ sensor_msgs std_msgs polled_camera + nodelet camera_info_manager diagnostic_updater @@ -35,6 +36,11 @@ sensor_msgs std_msgs polled_camera + nodelet + + + + diff --git a/plugins.xml b/plugins.xml new file mode 100644 index 00000000..f70b98ed --- /dev/null +++ b/plugins.xml @@ -0,0 +1,12 @@ + + + + MonoCamera Nodelet + + + + + StereoCamera Nodelet + + + diff --git a/src/mono_camera.cpp b/src/mono_camera.cpp index a632ed88..73d1dc2f 100644 --- a/src/mono_camera.cpp +++ b/src/mono_camera.cpp @@ -36,7 +36,7 @@ namespace avt_vimba_camera { -MonoCamera::MonoCamera(ros::NodeHandle nh, ros::NodeHandle nhp) : nh_(nh), nhp_(nhp), it_(nhp), cam_(ros::this_node::getName()) { +MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp_(nhp), it_(nhp), cam_(ros::this_node::getName()) { // Prepare node handle for the camera // TODO use nodelets with getMTNodeHandle() @@ -127,7 +127,7 @@ void MonoCamera::updateCameraInfo(const avt_vimba_camera::AvtVimbaCameraConfig& int binning_or_decimation_y = std::max(config.binning_y, config.decimation_y); // Set the operational parameters in CameraInfo (binning, ROI) - ci.height = config.height; + ci.height = config.height; ci.width = config.width; ci.binning_x = binning_or_decimation_x; ci.binning_y = binning_or_decimation_y; diff --git a/src/nodes/mono_camera_nodelet.cpp b/src/nodes/mono_camera_nodelet.cpp new file mode 100644 index 00000000..9b44b8e6 --- /dev/null +++ b/src/nodes/mono_camera_nodelet.cpp @@ -0,0 +1,18 @@ +#include +#include "avt_vimba_camera/mono_camera_nodelet.h" + +namespace avt_vimba_camera +{ + void MonoCameraNodelet::onInit() + { + NODELET_DEBUG("Initializing nodelet..."); + camera_ = new MonoCamera(getMTNodeHandle(), getMTPrivateNodeHandle()); + } + + MonoCameraNodelet::~MonoCameraNodelet() + { + delete camera_; + } +} + +PLUGINLIB_EXPORT_CLASS(avt_vimba_camera::MonoCameraNodelet, nodelet::Nodelet) diff --git a/src/nodes/stereo_camera_nodelet.cpp b/src/nodes/stereo_camera_nodelet.cpp new file mode 100644 index 00000000..19bb3a0e --- /dev/null +++ b/src/nodes/stereo_camera_nodelet.cpp @@ -0,0 +1,20 @@ +#include +#include "avt_vimba_camera/stereo_camera_nodelet.h" +#include + +namespace avt_vimba_camera +{ + void StereoCameraNodelet::onInit() + { + NODELET_DEBUG("Initializing nodelet..."); + camera_ = new StereoCamera(getMTNodeHandle(), getMTPrivateNodeHandle()); + boost::thread stereoThread(&avt_vimba_camera::StereoCamera::run, camera_); + } + + StereoCameraNodelet::~StereoCameraNodelet() + { + delete camera_; + } +} + +PLUGINLIB_EXPORT_CLASS(avt_vimba_camera::StereoCameraNodelet, nodelet::Nodelet)