diff --git a/pointgrey_camera_driver/cfg/PointGrey.cfg b/pointgrey_camera_driver/cfg/PointGrey.cfg index 4ec1385..d163b8c 100755 --- a/pointgrey_camera_driver/cfg/PointGrey.cfg +++ b/pointgrey_camera_driver/cfg/PointGrey.cfg @@ -147,7 +147,8 @@ gen.add("enable_trigger", bool_t, SensorLevels.RECONFIGURE_RUNNING, "E trigger_modes = gen.enum([gen.const("Mode0", str_t, "mode0", ""), gen.const("Mode1_bulb_trigger", str_t, "mode1", ""), gen.const("Mode3", str_t, "mode3", ""), - gen.const("Mode14", str_t, "mode14", "")], + gen.const("Mode14", str_t, "mode14", ""), + gen.const("Mode15", str_t, "mode15", "")], "IIDC v1.31 Trigger Modes") gen.add("trigger_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "IIDC v1.31 Trigger Modes", "mode0", edit_method = trigger_modes) @@ -155,7 +156,8 @@ gen.add("trigger_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "I gpio_pins = gen.enum([gen.const("GPIO0", str_t, "gpio0", ""), gen.const("GPIO1", str_t, "gpio1", ""), gen.const("GPIO2", str_t, "gpio2", ""), - gen.const("GPIO3", str_t, "gpio3", "")], + gen.const("GPIO3", str_t, "gpio3", ""), + gen.const("SOFTWARE", str_t, "software", "")], "GPIO Trigger Sources") gen.add("trigger_source", str_t, SensorLevels.RECONFIGURE_RUNNING, "GPIO Trigger Sources", "gpio0", edit_method = gpio_pins) diff --git a/pointgrey_camera_driver/include/pointgrey_camera_driver/PointGreyCamera.h b/pointgrey_camera_driver/include/pointgrey_camera_driver/PointGreyCamera.h index b8d91d6..e14b1ba 100644 --- a/pointgrey_camera_driver/include/pointgrey_camera_driver/PointGreyCamera.h +++ b/pointgrey_camera_driver/include/pointgrey_camera_driver/PointGreyCamera.h @@ -173,6 +173,8 @@ class PointGreyCamera uint getROIPosition(); + bool fireSoftwareTrigger(); + private: uint32_t serial_; ///< A variable to hold the serial number of the desired camera. diff --git a/pointgrey_camera_driver/src/PointGreyCamera.cpp b/pointgrey_camera_driver/src/PointGreyCamera.cpp index 33f3171..7e10a56 100644 --- a/pointgrey_camera_driver/src/PointGreyCamera.cpp +++ b/pointgrey_camera_driver/src/PointGreyCamera.cpp @@ -675,6 +675,10 @@ static int sourceNumberFromGpioName(const std::string s) { return 3; } + else if(s.compare("software") == 0) + { + return 7; + } else { // Unrecognized pin @@ -765,6 +769,10 @@ bool PointGreyCamera::setExternalTrigger(bool &enable, std::string &mode, std::s { triggerMode.mode = 14; } + else if(tmode.compare("mode15") == 0) + { + triggerMode.mode = 15; + } else { // Unrecognized mode @@ -774,6 +782,7 @@ bool PointGreyCamera::setExternalTrigger(bool &enable, std::string &mode, std::s } // Parameter is used for mode3 (return one out of every N frames). So if N is two, it returns every other frame. + // It is also used for mode15 (camera will acquire N images and stop) triggerMode.parameter = parameter; // Set trigger source @@ -819,6 +828,21 @@ bool PointGreyCamera::setExternalTrigger(bool &enable, std::string &mode, std::s return retVal; } +bool PointGreyCamera::fireSoftwareTrigger() { + const unsigned int k_softwareTrigger = 0x62C; + const unsigned int k_fireVal = 0x80000000; + Error error; + + error = cam_.WriteRegister(k_softwareTrigger, k_fireVal); + if (error != PGRERROR_OK) + { + PointGreyCamera::handleError("PointGreyCamera::fireSoftwareTrigger Could not fire software trigger.", error); + return false; + } + + return true; +} + void PointGreyCamera::setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay) { auto_packet_size_ = auto_packet_size; diff --git a/pointgrey_camera_driver/src/nodelet.cpp b/pointgrey_camera_driver/src/nodelet.cpp index a74db2d..92cdd02 100644 --- a/pointgrey_camera_driver/src/nodelet.cpp +++ b/pointgrey_camera_driver/src/nodelet.cpp @@ -41,6 +41,7 @@ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND #include #include // Message type for configuring gain and white balance. +#include //Message type for software trigger #include // Headers for publishing diagnostic messages. #include @@ -171,6 +172,7 @@ class PointGreyCameraNodelet: public nodelet::Nodelet scopedLock.lock(); pubThread_.reset(); sub_.shutdown(); + sub_trigger_.shutdown(); try { @@ -377,6 +379,7 @@ class PointGreyCameraNodelet: public nodelet::Nodelet { boost::mutex::scoped_lock scopedLock(connect_mutex_); sub_.shutdown(); + sub_trigger_.shutdown(); } try @@ -445,6 +448,12 @@ class PointGreyCameraNodelet: public nodelet::Nodelet sub_ = getMTNodeHandle().subscribe("image_exposure_sequence", 10, &pointgrey_camera_driver::PointGreyCameraNodelet::gainWBCallback, this); } + // Subscribe to software triggers (must be enabled and supported by the camera) + { + boost::mutex::scoped_lock scopedLock(connect_mutex_); + sub_trigger_ = getMTNodeHandle().subscribe("trigger", 10, &pointgrey_camera_driver::PointGreyCameraNodelet::triggerCallback, this); + } + state = CONNECTED; } catch(std::runtime_error& e) @@ -562,12 +571,26 @@ class PointGreyCameraNodelet: public nodelet::Nodelet } } + void triggerCallback(const std_msgs::Empty &msg) + { + try + { + NODELET_DEBUG("Trigger callback: Firing software trigger"); + pg_.fireSoftwareTrigger(); + } + catch(std::runtime_error& e) + { + NODELET_ERROR("triggerCallback failed with error: %s", e.what()); + } + } + boost::shared_ptr > srv_; ///< Needed to initialize and keep the dynamic_reconfigure::Server in scope. boost::shared_ptr it_; ///< Needed to initialize and keep the ImageTransport in scope. boost::shared_ptr cinfo_; ///< Needed to initialize and keep the CameraInfoManager in scope. image_transport::CameraPublisher it_pub_; ///< CameraInfoManager ROS publisher boost::shared_ptr > pub_; ///< Diagnosed publisher, has to be a pointer because of constructor requirements ros::Subscriber sub_; ///< Subscriber for gain and white balance changes. + ros::Subscriber sub_trigger_; ///< Subscriber for software trigger boost::mutex connect_mutex_;