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/package.xml b/package.xml
index 1b7d93a..afddb0f 100644
--- a/package.xml
+++ b/package.xml
@@ -22,7 +22,8 @@
   <author>Stefan May (Nuremberg Institute of Technology Georg Simon Ohm - www.th-nuernberg.de)</author>
   <author>64-Bit platform supported by Fraunhofer IPA (www.ipa.fraunhofer.de)</author>
   <author>Support for ROS hydro migration by Christopher-Eyk Hrabia (DAI-Labor, Technische Universität Berlin)</author>
-
+  <author>Support for focus motor control by Marius Montebaur (DAI-Labor, Technische Universität Berlin)</author>
+  
   <!-- Buildtool dependencies -->
   <buildtool_depend>catkin</buildtool_depend>
 
diff --git a/src/OptrisImager.cpp b/src/OptrisImager.cpp
index 00afd52..a58980a 100644
--- a/src/OptrisImager.cpp
+++ b/src/OptrisImager.cpp
@@ -5,7 +5,14 @@ namespace optris_drivers
 
 OptrisImager::OptrisImager(evo::IRDevice* dev, evo::IRDeviceParams params)
 {
-  _imager.init(&params, dev->getFrequency(), dev->getWidth(), dev->getHeight(), dev->controlledViaHID());
+  if (!_imager.init(&params, dev->getFrequency(), dev->getWidth(), dev->getHeight(), dev->controlledViaHID())) 
+  {
+    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);
+  }
+
   _imager.setClient(this);
 
   _bufferRaw = new unsigned char[dev->getRawBufferSize()];
@@ -158,4 +165,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 <image_transport/image_transport.h>
@@ -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..5264e91 100644
--- a/src/optris_imager_node.cpp
+++ b/src/optris_imager_node.cpp
@@ -39,8 +39,25 @@
 #include <sys/stat.h>
 #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);
+  // also uncomment #include "libirimager/IRLogger.h" from OptrisImager.h
+
   ros::init(argc, argv, "optris_imager_node");
 
   // private node handle to support command line parameters for rosrun
@@ -59,6 +76,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 +92,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;
 }