From 2f51ae5f34e7dd81567dd905cc4378b56728d30e Mon Sep 17 00:00:00 2001 From: HelmutE89 Date: Mon, 9 Sep 2019 13:38:48 +0200 Subject: [PATCH 1/4] Added libirimager to include directories --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index cc01377..04fc367 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,6 +87,7 @@ catkin_package( ########### include_directories( ${catkin_INCLUDE_DIRS} + /usr/include/libirimager/ ) ## Declare a cpp executable From b51f23680b11fc192d1a8958a4fe802f5b538687 Mon Sep 17 00:00:00 2001 From: Marius Montebaur Date: Thu, 23 Jan 2020 13:12:44 +0100 Subject: [PATCH 2/4] added support for focus motor adjustments via rostopic. --- README.md | 23 ++++++++++++++++++++--- src/OptrisImager.cpp | 17 ++++++++++++++++- src/OptrisImager.h | 6 ++++++ src/optris_imager_node.cpp | 23 ++++++++++++++++++++--- 4 files changed, 62 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 342e41f..36329fc 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,21 @@ -optris_drivers -============== +# optris_drivers with ROS support for focus motor adjustments + +**ROS drivers for Optris thermal imagers** + +Forked from: [evocortex/optris_drivers](https://github.com/evocortex/optris_drivers) + +A fork of the optris-drivers ROS package which makes using optris cameras in ROS possible. This fork adds features like manual focus control via ROS topics. + + +## Running this node: + +See the instructions in the original repo. + + +### Subscribed topics: + +`/optris_focus_position` (`std_msgs/Float32`): +The `/optris_imager_node` subscribes to this topic and sets the focus accordingly. The position of the focus motor can be adjusted from 0% (for objects near the camera) to 100% by sending a value between `0.0` and `100.0`. + +One possible implementation of an autofocus for Optris cameras is a laserscanner that measures the distance in the cameras viewing direction and a corresponding node that has a lookup table with several distances and according focus positions which are then published to the `/optris_focus_position` topic. -ROS drivers for Optris thermal imagers \ No newline at end of file diff --git a/src/OptrisImager.cpp b/src/OptrisImager.cpp index 00afd52..eb69c8a 100644 --- a/src/OptrisImager.cpp +++ b/src/OptrisImager.cpp @@ -5,7 +5,15 @@ namespace optris_drivers OptrisImager::OptrisImager(evo::IRDevice* dev, evo::IRDeviceParams params) { - _imager.init(¶ms, dev->getFrequency(), dev->getWidth(), dev->getHeight(), dev->controlledViaHID()); + if (!_imager.init(¶ms, dev->getFrequency(), dev->getWidth(), dev->getHeight(), dev->controlledViaHID())) + { + std::cout << "\033[1;31mError:\033[0m"; + std::cout << " Image stream not available or wrongly configured. Check connection to camera and config file." << std::endl; + std::cout << " Enable debug output to narrow down the problem source. See optris_imager_node.cpp" << std::endl; + ros::shutdown(); + exit(EXIT_FAILURE); + } + _imager.setClient(this); _bufferRaw = new unsigned char[dev->getRawBufferSize()]; @@ -158,4 +166,11 @@ bool OptrisImager::onSetTemperatureRange(TemperatureRange::Request &req, Tempera return true; } +bool OptrisImager::setFocus(float focusMotorPosition) +{ + float pos = std::min(100.0f, std::max(0.0f, focusMotorPosition)); + bool state = _imager.setFocusmotorPos(pos); + return state; +} + } diff --git a/src/OptrisImager.h b/src/OptrisImager.h index 58715d1..5ade991 100644 --- a/src/OptrisImager.h +++ b/src/OptrisImager.h @@ -4,6 +4,7 @@ #include "libirimager/IRDevice.h" #include "libirimager/IRImager.h" #include "libirimager/IRImagerClient.h" +//#include "libirimager/IRLogger.h" #include "ros/ros.h" #include @@ -105,6 +106,11 @@ class OptrisImager : public evo::IRImagerClient */ bool onSetTemperatureRange(TemperatureRange::Request &req, TemperatureRange::Response &res); + /** + * Set focus + */ + bool setFocus(float focusMotorPosition); + private: evo::IRImager _imager; diff --git a/src/optris_imager_node.cpp b/src/optris_imager_node.cpp index 38bafd5..c205880 100644 --- a/src/optris_imager_node.cpp +++ b/src/optris_imager_node.cpp @@ -39,8 +39,24 @@ #include #include "OptrisImager.h" -int main(int argc, char **argv) + +optris_drivers::OptrisImager *optImager = NULL; + + +void setFocusPosition(const std_msgs::Float32::ConstPtr& msg) { + if (optImager == NULL) { + return; + } + optImager->setFocus(msg->data); +} + + +int main(int argc, char **argv) +{ + // uncomment to enable debug output: + //evo::IRLogger::setVerbosity(evo::IRLOG_DEBUG, evo::IRLOG_OFF); + ros::init(argc, argv, "optris_imager_node"); // private node handle to support command line parameters for rosrun @@ -59,6 +75,7 @@ int main(int argc, char **argv) } ros::NodeHandle n; + ros::Subscriber sub = n.subscribe("optris_focus_position", 1, setFocusPosition); // Read parameters from xml file evo::IRDeviceParams params; @@ -74,8 +91,8 @@ int main(int argc, char **argv) } // Give control to class instance - optris_drivers::OptrisImager imager(dev, params); - imager.run(); + optImager = new optris_drivers::OptrisImager(dev, params); + optImager->run(); return 0; } From 18a6c68e3d7d763ede8bfb2f04bd47b718da492b Mon Sep 17 00:00:00 2001 From: Marius M Date: Mon, 10 Feb 2020 12:41:02 +0100 Subject: [PATCH 3/4] Updating authors in package.xml --- package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 1b7d93a..afddb0f 100644 --- a/package.xml +++ b/package.xml @@ -22,7 +22,8 @@ Stefan May (Nuremberg Institute of Technology Georg Simon Ohm - www.th-nuernberg.de) 64-Bit platform supported by Fraunhofer IPA (www.ipa.fraunhofer.de) Support for ROS hydro migration by Christopher-Eyk Hrabia (DAI-Labor, Technische Universität Berlin) - + Support for focus motor control by Marius Montebaur (DAI-Labor, Technische Universität Berlin) + catkin From 1395e28128b6cd50db1cfc81cc12e7c0f86f5149 Mon Sep 17 00:00:00 2001 From: Marius Montebaur Date: Mon, 10 Feb 2020 13:25:09 +0100 Subject: [PATCH 4/4] using ros error log on failed camera initialization --- CMakeLists.txt | 1 - src/OptrisImager.cpp | 5 ++--- src/optris_imager_node.cpp | 5 +++-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 04fc367..cc01377 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,7 +87,6 @@ catkin_package( ########### include_directories( ${catkin_INCLUDE_DIRS} - /usr/include/libirimager/ ) ## Declare a cpp executable diff --git a/src/OptrisImager.cpp b/src/OptrisImager.cpp index eb69c8a..a58980a 100644 --- a/src/OptrisImager.cpp +++ b/src/OptrisImager.cpp @@ -7,9 +7,8 @@ OptrisImager::OptrisImager(evo::IRDevice* dev, evo::IRDeviceParams params) { if (!_imager.init(¶ms, dev->getFrequency(), dev->getWidth(), dev->getHeight(), dev->controlledViaHID())) { - std::cout << "\033[1;31mError:\033[0m"; - std::cout << " Image stream not available or wrongly configured. Check connection to camera and config file." << std::endl; - std::cout << " Enable debug output to narrow down the problem source. See optris_imager_node.cpp" << std::endl; + ROS_ERROR("Image stream not available or wrongly configured. Check connection to camera and config file."); + ROS_ERROR("Enable debug output to narrow down the problem source. See optris_imager_node.cpp"); ros::shutdown(); exit(EXIT_FAILURE); } diff --git a/src/optris_imager_node.cpp b/src/optris_imager_node.cpp index c205880..5264e91 100644 --- a/src/optris_imager_node.cpp +++ b/src/optris_imager_node.cpp @@ -45,9 +45,9 @@ optris_drivers::OptrisImager *optImager = NULL; void setFocusPosition(const std_msgs::Float32::ConstPtr& msg) { - if (optImager == NULL) { + if (optImager == NULL) return; - } + optImager->setFocus(msg->data); } @@ -56,6 +56,7 @@ int main(int argc, char **argv) { // uncomment to enable debug output: //evo::IRLogger::setVerbosity(evo::IRLOG_DEBUG, evo::IRLOG_OFF); + // also uncomment #include "libirimager/IRLogger.h" from OptrisImager.h ros::init(argc, argv, "optris_imager_node");