From 8b1e175102e9bf2b8b5cae4ad4ae9dcf119b3a03 Mon Sep 17 00:00:00 2001 From: Kazumi Malhan Date: Mon, 7 Nov 2016 17:28:34 -0500 Subject: [PATCH 1/2] Added support to run as nodelet. In order to run as nodelet, manual call of spin function with sleep is replaced by timer callback with separate node handle. Also, node handle is passed by main.cpp and no longer created in pylon_camera_node.cpp --- CMakeLists.txt | 10 +++++ README.rst | 10 ++++- include/pylon_camera/pylon_camera_node.h | 11 ++++-- launch/pylon_camera_nodelet.launch | 13 +++++++ nodelet_plugins.xml | 9 +++++ package.xml | 6 +++ src/pylon_camera/main.cpp | 15 ++------ src/pylon_camera/pylon_camera_node.cpp | 46 ++++++++++++----------- src/pylon_camera/pylon_camera_nodelet.cpp | 30 +++++++++++++++ 9 files changed, 113 insertions(+), 37 deletions(-) create mode 100644 launch/pylon_camera_nodelet.launch create mode 100644 nodelet_plugins.xml create mode 100644 src/pylon_camera/pylon_camera_nodelet.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2da0a088..65c51d9b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ set( roslaunch sensor_msgs #std_srvs + nodelet ) find_package(Pylon QUIET) @@ -49,6 +50,7 @@ roslint_cpp( src/${PROJECT_NAME}/encoding_conversions.cpp src/${PROJECT_NAME}/main.cpp src/${PROJECT_NAME}/pylon_camera_node.cpp + src/${PROJECT_NAME}/pylon_camera_nodelet.cpp src/${PROJECT_NAME}/pylon_camera_parameter.cpp src/${PROJECT_NAME}/pylon_camera.cpp src/${PROJECT_NAME}/write_device_user_id_to_camera.cpp @@ -79,6 +81,7 @@ add_library( src/${PROJECT_NAME}/encoding_conversions.cpp src/${PROJECT_NAME}/pylon_camera.cpp src/${PROJECT_NAME}/pylon_camera_node.cpp + src/${PROJECT_NAME}/pylon_camera_nodelet.cpp src/${PROJECT_NAME}/pylon_camera_parameter.cpp ) @@ -170,6 +173,13 @@ install( PATTERN "internal" EXCLUDE ) +install( + FILES + nodelet_plugins.xml + DESTINATION + ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + ## Testing ## # All Jenkins-Tests are now in the pylon_camera_tests-pkg ############ diff --git a/README.rst b/README.rst index 387d3d94..61edd5bd 100644 --- a/README.rst +++ b/README.rst @@ -26,7 +26,7 @@ This means that the image acquisition is triggered with a certain rate and the c The package opens either a predefined camera (using a given 'device_user_id' parameter) or, if no camera id is predefined the first camera device it can find. -| +The package supports running as nodelet. ****** **Installation** @@ -127,6 +127,14 @@ Images were only published if another node connects to the image topic. The publ ``rosrun image_view image_view image:=/pylon_camera_node/image_raw`` +The pylon_camera package also supports running as nodelet. To run as nodelet, use following launch file + +``roslaunch pylon_camera pylon_camera_nodelet.launch`` + +The image topic can be viewed using image_view using + +``rosrun image_view image_view image:=/pylon_camera_nodelet/image_raw`` + ****** **Questions** ****** diff --git a/include/pylon_camera/pylon_camera_node.h b/include/pylon_camera/pylon_camera_node.h index a405490f..003af537 100644 --- a/include/pylon_camera/pylon_camera_node.h +++ b/include/pylon_camera/pylon_camera_node.h @@ -64,7 +64,7 @@ typedef actionlib::SimpleActionServer Gra class PylonCameraNode { public: - PylonCameraNode(); + PylonCameraNode(ros::NodeHandle &nh_private, ros::NodeHandle &nh_image); virtual ~PylonCameraNode(); /** @@ -74,9 +74,9 @@ class PylonCameraNode void init(); /** - * spin the node + * spin the node (with timer callback) */ - virtual void spin(); + virtual void spinCallback(const ros::TimerEvent& event); /** * Getter for the frame rate set by the launch script or from the ros parameter @@ -317,7 +317,10 @@ class PylonCameraNode */ bool waitForCamera(const ros::Duration& timeout) const; - ros::NodeHandle nh_; + ros::NodeHandle* nh_private_; + ros::NodeHandle* nh_image_; + ros::Timer grab_image_timer_; + PylonCameraParameter pylon_camera_parameter_set_; ros::ServiceServer set_binning_srv_; ros::ServiceServer set_exposure_srv_; diff --git a/launch/pylon_camera_nodelet.launch b/launch/pylon_camera_nodelet.launch new file mode 100644 index 00000000..c775bfbf --- /dev/null +++ b/launch/pylon_camera_nodelet.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml new file mode 100644 index 00000000..a76b52b7 --- /dev/null +++ b/nodelet_plugins.xml @@ -0,0 +1,9 @@ + + + + description + + + diff --git a/package.xml b/package.xml index 1b073baa..5324e060 100644 --- a/package.xml +++ b/package.xml @@ -35,6 +35,7 @@ roslaunch roslint + nodelet actionlib camera_control_msgs @@ -47,5 +48,10 @@ roslaunch sensor_msgs + nodelet + + + + diff --git a/src/pylon_camera/main.cpp b/src/pylon_camera/main.cpp index 12b44bc9..6a36e151 100644 --- a/src/pylon_camera/main.cpp +++ b/src/pylon_camera/main.cpp @@ -40,22 +40,15 @@ int main(int argc, char **argv) { ros::init(argc, argv, "pylon_camera_node"); + ros::NodeHandle nh_private("~"); + ros::NodeHandle nh_image; - pylon_camera::PylonCameraNode pylon_camera_node; - - ros::Rate r(pylon_camera_node.frameRate()); + pylon_camera::PylonCameraNode pylon_camera_node(nh_private, nh_image); ROS_INFO_STREAM("Start image grabbing if node connects to topic with " << "a frame_rate of: " << pylon_camera_node.frameRate() << " Hz"); - // Main thread and brightness-service thread - boost::thread th(boost::bind(&ros::spin)); - - while ( ros::ok() ) - { - pylon_camera_node.spin(); - r.sleep(); - } + ros::spin(); ROS_INFO("Terminate PylonCameraNode"); return EXIT_SUCCESS; diff --git a/src/pylon_camera/pylon_camera_node.cpp b/src/pylon_camera/pylon_camera_node.cpp index 2dab3127..d80097fa 100644 --- a/src/pylon_camera/pylon_camera_node.cpp +++ b/src/pylon_camera/pylon_camera_node.cpp @@ -40,34 +40,33 @@ namespace pylon_camera using sensor_msgs::CameraInfo; using sensor_msgs::CameraInfoPtr; -PylonCameraNode::PylonCameraNode() - : nh_("~"), - pylon_camera_parameter_set_(), - set_binning_srv_(nh_.advertiseService("set_binning", +PylonCameraNode::PylonCameraNode(ros::NodeHandle &nh_private, ros::NodeHandle &nh_image) + : pylon_camera_parameter_set_(), + set_binning_srv_(nh_private.advertiseService("set_binning", &PylonCameraNode::setBinningCallback, this)), - set_exposure_srv_(nh_.advertiseService("set_exposure", + set_exposure_srv_(nh_private.advertiseService("set_exposure", &PylonCameraNode::setExposureCallback, this)), - set_gain_srv_(nh_.advertiseService("set_gain", + set_gain_srv_(nh_private.advertiseService("set_gain", &PylonCameraNode::setGainCallback, this)), - set_gamma_srv_(nh_.advertiseService("set_gamma", + set_gamma_srv_(nh_private.advertiseService("set_gamma", &PylonCameraNode::setGammaCallback, this)), - set_brightness_srv_(nh_.advertiseService("set_brightness", + set_brightness_srv_(nh_private.advertiseService("set_brightness", &PylonCameraNode::setBrightnessCallback, this)), - set_sleeping_srv_(nh_.advertiseService("set_sleeping", + set_sleeping_srv_(nh_private.advertiseService("set_sleeping", &PylonCameraNode::setSleepingCallback, this)), set_user_output_srvs_(), pylon_camera_(nullptr), - it_(new image_transport::ImageTransport(nh_)), - img_raw_pub_(it_->advertiseCamera("image_raw", 1)), + it_(new image_transport::ImageTransport(nh_image)), + img_raw_pub_(it_->advertiseCamera(nh_private.getNamespace() + "/image_raw", 1)), img_rect_pub_(nullptr), grab_imgs_raw_as_( - nh_, + nh_image, "grab_images_raw", boost::bind(&PylonCameraNode::grabImagesRawActionExecuteCB, this, @@ -76,11 +75,14 @@ PylonCameraNode::PylonCameraNode() grab_imgs_rect_as_(nullptr), pinhole_model_(nullptr), cv_bridge_img_rect_(nullptr), - camera_info_manager_(new camera_info_manager::CameraInfoManager(nh_)), + camera_info_manager_(new camera_info_manager::CameraInfoManager(nh_image)), sampling_indices_(), brightness_exp_lut_(), is_sleeping_(false) { + // Pointer to node handler + nh_private_ = &nh_private; + nh_image_ = &nh_image; init(); } @@ -91,7 +93,7 @@ void PylonCameraNode::init() // detected, the interface will reset them to the default values. // These parameters furthermore contain the intrinsic calibration matrices, // in case they are provided - pylon_camera_parameter_set_.readFromRosParameterServer(nh_); + pylon_camera_parameter_set_.readFromRosParameterServer(*nh_private_); // creating the target PylonCamera-Object with the specified // device_user_id, registering the Software-Trigger-Mode, starting the @@ -108,6 +110,8 @@ void PylonCameraNode::init() ros::shutdown(); return; } + // setup timer + grab_image_timer_ = nh_image_->createTimer(ros::Duration(1/frameRate()), &PylonCameraNode::spinCallback, this); } bool PylonCameraNode::initAndRegister() @@ -174,7 +178,7 @@ bool PylonCameraNode::startGrabbing() for ( int i = 0; i < set_user_output_srvs_.size(); ++i ) { std::string srv_name = "set_user_output_" + std::to_string(i); - set_user_output_srvs_.at(i) = nh_.advertiseService< camera_control_msgs::SetBool::Request, + set_user_output_srvs_.at(i) = nh_private_->advertiseService< camera_control_msgs::SetBool::Request, camera_control_msgs::SetBool::Response >( srv_name, boost::bind(&PylonCameraNode::setUserOutputCB, @@ -224,7 +228,7 @@ bool PylonCameraNode::startGrabbing() !camera_info_manager_->validateURL(pylon_camera_parameter_set_.cameraInfoURL()) ) { ROS_INFO_STREAM("CameraInfoURL needed for rectification! ROS-Param: " - << "'" << nh_.getNamespace() << "/camera_info_url' = '" + << "'" << nh_private_->getNamespace() << "/camera_info_url' = '" << pylon_camera_parameter_set_.cameraInfoURL() << "' is invalid!"); ROS_DEBUG_STREAM("CameraInfoURL should have following style: " << "'file:///full/path/to/local/file.yaml' or " @@ -340,12 +344,12 @@ bool PylonCameraNode::startGrabbing() pylon_camera_parameter_set_.frameRate(), pylon_camera_->maxPossibleFramerate()); pylon_camera_parameter_set_.setFrameRate( - nh_, + *nh_private_, pylon_camera_->maxPossibleFramerate()); } else if ( pylon_camera_parameter_set_.frameRate() == -1 ) { - pylon_camera_parameter_set_.setFrameRate(nh_, + pylon_camera_parameter_set_.setFrameRate(*nh_private_, pylon_camera_->maxPossibleFramerate()); ROS_INFO("Max possible framerate is %.2f Hz", pylon_camera_->maxPossibleFramerate()); @@ -356,10 +360,10 @@ bool PylonCameraNode::startGrabbing() void PylonCameraNode::setupRectification() { img_rect_pub_ = - new ros::Publisher(nh_.advertise("image_rect", 1)); + new ros::Publisher(nh_image_->advertise(nh_private_->getNamespace() + "/image_rect", 1)); grab_imgs_rect_as_ = - new GrabImagesAS(nh_, + new GrabImagesAS(*nh_image_, "grab_images_rect", boost::bind( &PylonCameraNode::grabImagesRectActionExecuteCB, @@ -376,7 +380,7 @@ void PylonCameraNode::setupRectification() cv_bridge_img_rect_->encoding = img_raw_msg_.encoding; } -void PylonCameraNode::spin() +void PylonCameraNode::spinCallback(const ros::TimerEvent& event) { if ( camera_info_manager_->isCalibrated() ) { diff --git a/src/pylon_camera/pylon_camera_nodelet.cpp b/src/pylon_camera/pylon_camera_nodelet.cpp new file mode 100644 index 00000000..9fd3c17c --- /dev/null +++ b/src/pylon_camera/pylon_camera_nodelet.cpp @@ -0,0 +1,30 @@ +#include +#include + +#include + +namespace pylon_camera +{ + +class PylonCameraNodelet : public nodelet::Nodelet +{ +public: + PylonCameraNodelet() + { + } + ~PylonCameraNodelet() + { + } + + void onInit(void) + { + node_.reset(new pylon_camera::PylonCameraNode(getPrivateNodeHandle(), getMTNodeHandle())); + } + +private: + boost::shared_ptr node_; +}; + +} // namespace pylon_camera + +PLUGINLIB_DECLARE_CLASS(pylon_camera, PylonCameraNodelet, pylon_camera::PylonCameraNodelet, nodelet::Nodelet); From 66cb03d8650e64515820330cfe3cb2b6dec85037 Mon Sep 17 00:00:00 2001 From: Kazumi Malhan Date: Mon, 7 Nov 2016 17:32:59 -0500 Subject: [PATCH 2/2] minor update to readme file. --- README.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.rst b/README.rst index 61edd5bd..18782383 100644 --- a/README.rst +++ b/README.rst @@ -28,6 +28,8 @@ The package opens either a predefined camera (using a given 'device_user_id' par The package supports running as nodelet. +| + ****** **Installation** ******