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..18782383 100644 --- a/README.rst +++ b/README.rst @@ -26,6 +26,8 @@ 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. + | ****** @@ -127,6 +129,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);