Initial commit

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(iri_visual_gps)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm nav_msgs iri_blob_detector sensor_msgs geometry_msgs dynamic_reconfigure message_generation)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+# find_package(<dependency> REQUIRED)
+find_package(OpenCV REQUIRED)
+
+# ******************************************************************** 
+#           Add topic, service and action definition here
+# ******************************************************************** 
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  set_exposure.srv
+#   Service2.srv
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs  # Or other packages containing msgs
+)
+
+# ******************************************************************** 
+#                 Add the dynamic reconfigure file 
+# ******************************************************************** 
+generate_dynamic_reconfigure_options(cfg/VisualGps.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+ CATKIN_DEPENDS iri_base_algorithm nav_msgs iri_blob_detector sensor_msgs geometry_msgs dynamic_reconfigure message_runtime
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+#  DEPENDS 
+)
+
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${OpenCV_INCLUDE_DIRS})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/visual_gps_alg.cpp src/visual_gps_alg_node.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ******************************************************************** 
+#               Add message headers dependencies 
+# ******************************************************************** 
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} nav_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} iri_blob_detector_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})

cfg/VisualGps.cfg

+#! /usr/bin/env python
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of the Willow Garage nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='iri_visual_gps'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+#gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add("color1_id"             ,  str_t,     0,                               "Color 1 identifier",             "color1")
+gen.add("color1_x"              ,  double_t,  0,                               "Color 1 X position",             0.5,      -15.0,15.0)
+gen.add("color1_y"              ,  double_t,  0,                               "Color 1 Y position",             0.5,      -15.0,15.0)
+gen.add("color1_z"              ,  double_t,  0,                               "Color 1 Z position",             0.5,      -15.0,15.0)
+gen.add("color2_id"             ,  str_t,     0,                               "Color 2 identifier",             "color2")
+gen.add("color2_x"              ,  double_t,  0,                               "Color 2 X position",             0.5,      -15.0,15.0)
+gen.add("color2_y"              ,  double_t,  0,                               "Color 2 Y position",             0.5,      -15.0,15.0)
+gen.add("color2_z"              ,  double_t,  0,                               "Color 2 Z position",             0.5,      -15.0,15.0)
+gen.add("color3_id"             ,  str_t,     0,                               "Color 3 identifier",             "color3")
+gen.add("color3_x"              ,  double_t,  0,                               "Color 3 X position",             0.5,      -15.0,15.0)
+gen.add("color3_y"              ,  double_t,  0,                               "Color 3 Y position",             0.5,      -15.0,15.0)
+gen.add("color3_z"              ,  double_t,  0,                               "Color 3 Z position",             0.5,      -15.0,15.0)
+gen.add("color4_id"             ,  str_t,     0,                               "Color 4 identifier",             "color4")
+gen.add("color4_x"              ,  double_t,  0,                               "Color 4 X position",             0.5,      -15.0,15.0)
+gen.add("color4_y"              ,  double_t,  0,                               "Color 4 Y position",             0.5,      -15.0,15.0)
+gen.add("color4_z"              ,  double_t,  0,                               "Color 4 Z position",             0.5,      -15.0,15.0)
+
+exit(gen.generate(PACKAGE, "VisualGpsAlgorithm", "VisualGps"))

config/default_params.yaml

+color1_id: red
+color1_x: -2.0
+color1_y: -2.0
+color1_z: 3.0
+color2_id: green
+color2_x: 2.0
+color2_y: -2.0
+color2_z: 3.0
+color3_id: blue
+color3_x: 2.0
+color3_y: 2.0
+color3_z: 3.0
+color4_id: yellow
+color4_x: -2.0
+color4_y: 2.0
+color4_z: 3.0
+

include/visual_gps_alg.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _visual_gps_alg_h_
+#define _visual_gps_alg_h_
+
+#include <iri_visual_gps/VisualGpsConfig.h>
+#include <iri_blob_detector/blob.h>
+#include <geometry_msgs/Pose.h>
+
+//include visual_gps_alg main library
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class VisualGpsAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the VisualGpsConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    pthread_mutex_t access_;    
+
+    // private attributes and methods
+    cv::Mat cam_matrix;
+    geometry_msgs::Pose pose; 
+
+    void update_points(std::vector<iri_blob_detector::blob> &blobs, std::vector<cv::Point3f> &points_3d,std::vector<cv::Point2f> &points_img);
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the VisualGpsConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef iri_visual_gps::VisualGpsConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    VisualGpsAlgorithm(void);
+
+   /**
+    * \brief Lock Algorithm
+    *
+    * Locks access to the Algorithm class
+    */
+    void lock(void) { pthread_mutex_lock(&this->access_); };
+
+   /**
+    * \brief Unlock Algorithm
+    *
+    * Unlocks access to the Algorithm class
+    */
+    void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+   /**
+    * \brief Tries Access to Algorithm
+    *
+    * Tries access to Algorithm
+    * 
+    * \return true if the lock was adquired, false otherwise
+    */
+    bool try_enter(void) 
+    { 
+      if(pthread_mutex_trylock(&this->access_)==0)
+        return true;
+      else
+        return false;
+    };
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the 
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& config, uint32_t level=0);
+
+    // here define all visual_gps_alg interface methods to retrieve and set
+    // the driver parameters
+    void set_camera_parameters(double fx, double cx, double fy, double cy);
+    void compute_pos(std::vector<iri_blob_detector::blob> &blobs1,std::vector<iri_blob_detector::blob> &blobs2,std::vector<iri_blob_detector::blob> &blobs3,std::vector<iri_blob_detector::blob> &blobs4);
+    void get_pose(geometry_msgs::Pose &pose);
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~VisualGpsAlgorithm(void);
+};
+
+#endif

include/visual_gps_alg_node.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _visual_gps_alg_node_h_
+#define _visual_gps_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "visual_gps_alg.h"
+
+// [publisher subscriber headers]
+#include <nav_msgs/Odometry.h>
+#include <iri_blob_detector/blob_array.h>
+#include <sensor_msgs/CameraInfo.h>
+
+// [service client headers]
+#include <iri_visual_gps/set_exposure.h>
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class VisualGpsAlgNode : public algorithm_base::IriBaseAlgorithm<VisualGpsAlgorithm>
+{
+  private:
+    // [publisher attributes]
+    ros::Publisher odom_publisher_;
+    nav_msgs::Odometry odom_Odometry_msg_;
+
+    // [subscriber attributes]
+    ros::Subscriber color4_blobs_subscriber_;
+    void color4_blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg);
+    pthread_mutex_t color4_blobs_mutex_;
+    void color4_blobs_mutex_enter(void);
+    void color4_blobs_mutex_exit(void);
+    std::vector<iri_blob_detector::blob> color4_blobs;
+
+    ros::Subscriber color3_blobs_subscriber_;
+    void color3_blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg);
+    pthread_mutex_t color3_blobs_mutex_;
+    void color3_blobs_mutex_enter(void);
+    void color3_blobs_mutex_exit(void);
+    std::vector<iri_blob_detector::blob> color3_blobs;
+
+    ros::Subscriber color2_blobs_subscriber_;
+    void color2_blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg);
+    pthread_mutex_t color2_blobs_mutex_;
+    void color2_blobs_mutex_enter(void);
+    void color2_blobs_mutex_exit(void);
+    std::vector<iri_blob_detector::blob> color2_blobs;
+
+    ros::Subscriber color1_blobs_subscriber_;
+    void color1_blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg);
+    pthread_mutex_t color1_blobs_mutex_;
+    void color1_blobs_mutex_enter(void);
+    void color1_blobs_mutex_exit(void);
+    std::vector<iri_blob_detector::blob> color1_blobs;
+
+    ros::Subscriber camera_info_subscriber_;
+    void camera_info_callback(const sensor_msgs::CameraInfo::ConstPtr& msg);
+    pthread_mutex_t camera_info_mutex_;
+    void camera_info_mutex_enter(void);
+    void camera_info_mutex_exit(void);
+    bool cam_info_received;
+
+    // [service attributes]
+    ros::ServiceServer set_exposure_server_;
+    bool set_exposureCallback(iri_visual_gps::set_exposure::Request &req, iri_visual_gps::set_exposure::Response &res);
+    pthread_mutex_t set_exposure_mutex_;
+    void set_exposure_mutex_enter(void);
+    void set_exposure_mutex_exit(void);
+
+
+    // [client attributes]
+    ros::ServiceClient camera_dyn_reconf_client;
+    dynamic_reconfigure::Reconfigure camera_params_srv;
+
+    // [action server attributes]
+
+    // [action client attributes]
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+  public:
+   /**
+    * \brief Constructor
+    * 
+    * This constructor initializes specific class attributes and all ROS
+    * communications variables to enable message exchange.
+    */
+    VisualGpsAlgNode(void);
+
+   /**
+    * \brief Destructor
+    * 
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~VisualGpsAlgNode(void);
+
+  protected:
+   /**
+    * \brief main node thread
+    *
+    * This is the main thread node function. Code written here will be executed
+    * in every node loop while the algorithm is on running state. Loop frequency 
+    * can be tuned by modifying loop_rate attribute.
+    *
+    * Here data related to the process loop or to ROS topics (mainly data structs
+    * related to the MSG and SRV files) must be updated. ROS publisher objects 
+    * must publish their data in this process. ROS client servers may also
+    * request data to the corresponding server topics.
+    */
+    void mainNodeThread(void);
+
+   /**
+    * \brief dynamic reconfigure server callback
+    * 
+    * This method is called whenever a new configuration is received through
+    * the dynamic reconfigure. The derivated generic algorithm class must 
+    * implement it.
+    *
+    * \param config an object with new configuration from all algorithm 
+    *               parameters defined in the config file.
+    * \param level  integer referring the level in which the configuration
+    *               has been changed.
+    */
+    void node_config_update(Config &config, uint32_t level);
+
+   /**
+    * \brief node add diagnostics
+    *
+    * In this abstract function additional ROS diagnostics applied to the 
+    * specific algorithms may be added.
+    */
+    void addNodeDiagnostics(void);
+
+    // [diagnostic functions]
+    
+    // [test functions]
+};
+
+#endif

launch/visual_gps.launch

+<launch>
+  <node pkg="nodelet" type="nodelet" name="visual_gps_nodelet"  args="manager" output="screen">
+    
+  </node>
+
+  <node pkg="nodelet" type="nodelet" name="blob_detector_color1" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
+    <remap from="~/image_in/image_raw" to="/camera/image_raw"/>
+    <remap from="~/image_in/camera_info" to="/camera/camera_info"/>
+    <remap from="~/blobs" to="/visual_gps/color1_blobs"/>
+    <rosparam file="$(find iri_blob_detector)/config/default_params.yaml" command="load" />
+  </node>
+  <node pkg="nodelet" type="nodelet" name="blob_detector_color2" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
+    <remap from="~/image_in/image_raw" to="/camera/image_raw"/>
+    <remap from="~/image_in/camera_info" to="/camera/camera_info"/>
+    <remap from="~/blobs" to="/visual_gps/color2_blobs"/>
+    <rosparam file="$(find iri_blob_detector)/config/default_params.yaml" command="load" />
+  </node>
+  <node pkg="nodelet" type="nodelet" name="blob_detector_color3" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
+    <remap from="~/image_in/image_raw" to="/camera/image_raw"/>
+    <remap from="~/image_in/camera_info" to="/camera/camera_info"/>
+    <remap from="~/blobs" to="/visual_gps/color3_blobs"/>
+    <rosparam file="$(find iri_blob_detector)/config/default_params.yaml" command="load" />
+  </node>
+  <node pkg="nodelet" type="nodelet" name="blob_detector_color4" args="load iri_blob_detector/BlobDetectorNodelet visual_gps_nodelet" output="screen">
+    <remap from="~/image_in/image_raw" to="/camera/image_raw"/>
+    <remap from="~/image_in/camera_info" to="/camera/camera_info"/>
+    <remap from="~/blobs" to="/visual_gps/color4_blobs"/>
+    <rosparam file="$(find iri_blob_detector)/config/default_params.yaml" command="load" />
+  </node>
+  <node pkg="nodelet" type="nodelet" name="camera" args="load libuvc_camera/driver visual_gps_nodelet" output="screen">
+    <param name="vendor" value="0x046d"/>
+    <param name="product" value="0x0805"/>
+    <param name="serial" value="AB659260"/>
+    <!-- If the above parameters aren't unique, choose the first match: -->
+    <param name="index" value="0"/>
+    <!-- Image size and type -->
+    <param name="width" value="640"/>
+    <param name="height" value="480"/>
+    <!-- choose whichever uncompressed format the camera supports: -->
+    <param name="video_mode" value="uncompressed"/> <!-- or yuyv/nv12/mjpeg -->
+    <param name="frame_rate" value="15"/>
+    <param name="timestamp_method" value="start"/> <!-- start of frame -->
+    <param name="camera_info_url" value="file:///tmp/cam.yaml"/>
+    <param name="auto_exposure" value="0"/> <!-- use aperture_priority auto exposure -->
+    <param name="auto_exposure_priority" value="0"/>
+    <param name="exposure_absolute" value="0.90"/> <!-- use aperture_priority auto exposure -->
+    <remap from="/image_raw" to="/camera/image_raw"/>
+    <remap from="/camera_info" to="/camera/camera_info"/>
+  </node>
+
+  <node pkg="iri_visual_gps" 
+        type="iri_visual_gps" 
+        name="visual_gps" 
+        output="screen">
+    <remap from="~/camera/camera_info" to="/camera/camera_info"/>
+    <remap from="~/set_camera_params" to="/camera/set_parameters"/>
+    <rosparam file="$(find iri_visual_gps)/config/default_params.yaml" command="load" />
+  </node>
+</launch>

package.xml

+<?xml version="1.0"?>
+<package>
+  <name>iri_visual_gps</name>
+  <version>0.0.0</version>
+  <description>The iri_visual_gps package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="shernand@todo.todo">shernand</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>LGPL</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/iri_visual_gps</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>iri_blob_detector</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>message_generation</build_depend>
+  <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>iri_blob_detector</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/visual_gps_alg.cpp

+#include "visual_gps_alg.h"
+
+VisualGpsAlgorithm::VisualGpsAlgorithm(void)
+{
+  pthread_mutex_init(&this->access_,NULL);
+
+  this->cam_matrix = cv::Mat(3,3,CV_32FC1,cv::Scalar::all(0));
+}
+
+VisualGpsAlgorithm::~VisualGpsAlgorithm(void)
+{
+  pthread_mutex_destroy(&this->access_);
+}
+
+void VisualGpsAlgorithm::config_update(Config& config, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=config;
+  
+  this->unlock();
+}
+
+// VisualGpsAlgorithm Public API
+void VisualGpsAlgorithm::update_points(std::vector<iri_blob_detector::blob> &blobs, std::vector<cv::Point3f> &points_3d,std::vector<cv::Point2f> &points_img)
+{
+  unsigned int max_index=0;
+  double max_size=0;
+
+  if(blobs.size()!=0)// some blobs detected for color1
+  {
+    if(blobs[0].id.find(this->config_.color1_id))// blob 1 matches with color1
+      points_3d.push_back(cv::Point3f(this->config_.color1_x,this->config_.color1_y,this->config_.color1_z));
+    else if(blobs[0].id.find(this->config_.color2_id))// blob 1 matches with color2
+      points_3d.push_back(cv::Point3f(this->config_.color2_x,this->config_.color2_y,this->config_.color2_z));
+    else if(blobs[0].id.find(this->config_.color3_id))// blob 1 matches with color2
+      points_3d.push_back(cv::Point3f(this->config_.color3_x,this->config_.color3_y,this->config_.color3_z));
+    else if(blobs[0].id.find(this->config_.color4_id))// blob 1 matches with color2
+      points_3d.push_back(cv::Point3f(this->config_.color4_x,this->config_.color4_y,this->config_.color4_z));
+    // get the biggest blob
+    for(unsigned int i=0;i<blobs.size();i++)
+      if(blobs[i].size>max_size)
+      {
+        max_size=blobs[i].size;
+        max_index=i;
+      }
+    points_img.push_back(cv::Point2f(blobs[max_index].center_x,blobs[max_index].center_y));
+  }
+}
+
+void VisualGpsAlgorithm::set_camera_parameters(double fx, double cx, double fy, double cy)
+{
+  cam_matrix.at<float>(0,0) = fx;
+  cam_matrix.at<float>(0,2) = cx;
+  cam_matrix.at<float>(1,1) = fy;
+  cam_matrix.at<float>(1,2) = cy;
+  cam_matrix.at<float>(2,2) = 1.0f;
+}
+
+void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs1,std::vector<iri_blob_detector::blob> &blobs2,std::vector<iri_blob_detector::blob> &blobs3,std::vector<iri_blob_detector::blob> &blobs4)
+{
+  std::vector<cv::Point3f> points_3d;
+  std::vector<cv::Point2f> points_img;
+  cv::Mat tvec,rvec,R;
+  double w,x,y,z;
+
+  this->update_points(blobs1,points_3d,points_img);
+  this->update_points(blobs2,points_3d,points_img);
+  this->update_points(blobs3,points_3d,points_img);
+  this->update_points(blobs4,points_3d,points_img);
+  if(points_3d.size()>2 && points_img.size()>2)
+  {
+    if(cv::solvePnP(cv::Mat(points_3d),cv::Mat(points_img),this->cam_matrix,cv::Mat(),rvec,tvec,false,CV_ITERATIVE))
+    {
+      cv::Rodrigues(rvec,R);
+      // Matrix to quaternion
+      w = sqrt( std::max( 0.0, 1 + R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2) ) ) / 2;
+      x = sqrt( std::max( 0.0, 1 + R.at<double>(0, 0) - R.at<double>(1, 1) - R.at<double>(2, 2) ) ) / 2;
+      y = sqrt( std::max( 0.0, 1 - R.at<double>(0, 0) + R.at<double>(1, 1) - R.at<double>(2, 2) ) ) / 2;
+      z = sqrt( std::max( 0.0, 1 - R.at<double>(0, 0) - R.at<double>(1, 1) + R.at<double>(2, 2) ) ) / 2;
+      x = copysign( x, R.at<double>(2, 1) - R.at<double>(1, 2) );
+      y = copysign( y, R.at<double>(0, 2) - R.at<double>(2, 0) );
+      z = copysign( z, R.at<double>(1, 0) - R.at<double>(0, 1) );
+
+      this->pose.position.x = tvec.at<double>(0);
+      this->pose.position.y = tvec.at<double>(1);
+      this->pose.position.z = tvec.at<double>(2);
+      this->pose.orientation.x = x;
+      this->pose.orientation.y = y;
+      this->pose.orientation.z = z;
+      this->pose.orientation.w = w;
+    }
+  }
+}
+
+void VisualGpsAlgorithm::get_pose(geometry_msgs::Pose &pose)
+{
+  pose=this->pose;
+}

src/visual_gps_alg_node.cpp

+#include "visual_gps_alg_node.h"
+#include <dynamic_reconfigure/Reconfigure.h>
+
+VisualGpsAlgNode::VisualGpsAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<VisualGpsAlgorithm>()
+{
+  //init class attributes if necessary
+  this->loop_rate_ = 10;//in [Hz]
+
+  // [init publishers]
+  this->odom_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("odom", 1);
+  
+  // [init subscribers]
+  this->color4_blobs_subscriber_ = this->public_node_handle_.subscribe("color4_blobs", 1, &VisualGpsAlgNode::color4_blobs_callback, this);
+  pthread_mutex_init(&this->color4_blobs_mutex_,NULL);
+
+  this->color3_blobs_subscriber_ = this->public_node_handle_.subscribe("color3_blobs", 1, &VisualGpsAlgNode::color3_blobs_callback, this);
+  pthread_mutex_init(&this->color3_blobs_mutex_,NULL);
+
+  this->color2_blobs_subscriber_ = this->public_node_handle_.subscribe("color2_blobs", 1, &VisualGpsAlgNode::color2_blobs_callback, this);
+  pthread_mutex_init(&this->color2_blobs_mutex_,NULL);
+
+  this->color1_blobs_subscriber_ = this->public_node_handle_.subscribe("color1_blobs", 1, &VisualGpsAlgNode::color1_blobs_callback, this);
+  pthread_mutex_init(&this->color1_blobs_mutex_,NULL);
+
+  this->camera_info_subscriber_ = this->public_node_handle_.subscribe("camera/camera_info", 1, &VisualGpsAlgNode::camera_info_callback, this);
+  pthread_mutex_init(&this->camera_info_mutex_,NULL);
+  this->cam_info_received=false;
+  
+  // [init services]
+  this->set_exposure_server_ = this->public_node_handle_.advertiseService("set_exposure", &VisualGpsAlgNode::set_exposureCallback, this);
+  pthread_mutex_init(&this->set_exposure_mutex_,NULL);
+  
+  // [init clients]
+  this->camera_dyn_reconf_client=this->public_node_handle_.serviceClient<dynamic_reconfigure::Reconfigure>("set_camera_params");
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+VisualGpsAlgNode::~VisualGpsAlgNode(void)
+{
+  // [free dynamic memory]
+  pthread_mutex_destroy(&this->set_exposure_mutex_);
+  pthread_mutex_destroy(&this->color4_blobs_mutex_);
+  pthread_mutex_destroy(&this->color3_blobs_mutex_);
+  pthread_mutex_destroy(&this->color2_blobs_mutex_);
+  pthread_mutex_destroy(&this->color1_blobs_mutex_);
+  pthread_mutex_destroy(&this->camera_info_mutex_);
+}
+
+void VisualGpsAlgNode::mainNodeThread(void)
+{
+  // [fill msg structures]
+  
+  // [fill srv structure and make request to the server]
+  
+  // [fill action structure and make request to the action server]
+
+  // [publish messages]
+
+  if(this->cam_info_received)
+  {
+    this->alg_.lock();
+    this->alg_.compute_pos(this->color1_blobs,this->color2_blobs,this->color3_blobs,this->color4_blobs);
+    this->odom_Odometry_msg_.header.stamp=ros::Time::now();
+    this->odom_Odometry_msg_.header.frame_id="world";
+    this->odom_Odometry_msg_.child_frame_id="visual_gps";
+    this->alg_.get_pose(this->odom_Odometry_msg_.pose.pose);
+    this->odom_publisher_.publish(this->odom_Odometry_msg_);
+    this->alg_.unlock();
+  }
+}
+
+/*  [subscriber callbacks] */
+void VisualGpsAlgNode::camera_info_callback(const sensor_msgs::CameraInfo::ConstPtr& msg)
+{
+  this->camera_info_mutex_enter();
+
+  double fx,fy,cx,cy;
+  fx = msg->K[0];
+  fy = msg->K[4];
+  cx = msg->K[2];
+  cy = msg->K[5];
+  this->alg_.set_camera_parameters(fx,fy,cx,cy);
+  this->camera_info_subscriber_.shutdown();
+  cam_info_received = true;
+
+  this->camera_info_mutex_exit();
+}
+
+void VisualGpsAlgNode::camera_info_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->camera_info_mutex_);
+}
+
+void VisualGpsAlgNode::camera_info_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->camera_info_mutex_);
+}
+
+void VisualGpsAlgNode::color4_blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg)
+{
+  //use appropiate mutex to shared variables if necessary
+  this->alg_.lock();
+
+  this->color4_blobs=msg->blobs;
+
+  //unlock previously blocked shared variables
+  this->alg_.unlock();
+}
+
+void VisualGpsAlgNode::color4_blobs_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->color4_blobs_mutex_);
+}
+
+void VisualGpsAlgNode::color4_blobs_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->color4_blobs_mutex_);
+}
+
+void VisualGpsAlgNode::color3_blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg)
+{
+  //use appropiate mutex to shared variables if necessary
+  this->alg_.lock();
+
+  this->color3_blobs=msg->blobs;
+  
+  //unlock previously blocked shared variables
+  this->alg_.unlock();
+}
+
+void VisualGpsAlgNode::color3_blobs_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->color3_blobs_mutex_);
+}
+
+void VisualGpsAlgNode::color3_blobs_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->color3_blobs_mutex_);
+}
+
+void VisualGpsAlgNode::color2_blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg)
+{
+  //use appropiate mutex to shared variables if necessary
+  this->alg_.lock();
+
+  this->color2_blobs=msg->blobs;
+
+  //unlock previously blocked shared variables
+  this->alg_.unlock();
+}
+
+void VisualGpsAlgNode::color2_blobs_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->color2_blobs_mutex_);
+}
+
+void VisualGpsAlgNode::color2_blobs_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->color2_blobs_mutex_);
+}
+
+void VisualGpsAlgNode::color1_blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg)
+{
+  //use appropiate mutex to shared variables if necessary
+  this->alg_.lock();
+
+  this->color1_blobs=msg->blobs;
+
+  //unlock previously blocked shared variables
+  this->alg_.unlock();
+}
+
+void VisualGpsAlgNode::color1_blobs_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->color1_blobs_mutex_);
+}
+
+void VisualGpsAlgNode::color1_blobs_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->color1_blobs_mutex_);
+}
+
+
+/*  [service callbacks] */
+bool VisualGpsAlgNode::set_exposureCallback(iri_visual_gps::set_exposure::Request &req, iri_visual_gps::set_exposure::Response &res)
+{
+  //use appropiate mutex to shared variables if necessary
+  this->set_exposure_mutex_enter();
+  this->camera_params_srv.request.config.ints.resize(2);
+  this->camera_params_srv.request.config.ints[0].name="auto_exposure";
+  if(req.auto_exposure)
+    this->camera_params_srv.request.config.ints[0].value=1;
+  else
+    this->camera_params_srv.request.config.ints[0].value=0;
+  this->camera_params_srv.request.config.ints[1].name="auto_exposure_priority";
+  if(req.auto_exposure_priority)
+    this->camera_params_srv.request.config.ints[1].value=1;
+  else
+    this->camera_params_srv.request.config.ints[1].value=0;
+  this->camera_params_srv.request.config.doubles.resize(1);
+  this->camera_params_srv.request.config.doubles[0].name="exposure_absolute";
+  this->camera_params_srv.request.config.doubles[0].value=req.exposure_absolute;
+
+  if(this->camera_dyn_reconf_client.call(this->camera_params_srv))
+    res.success=true;
+  else
+    res.success=false;
+
+  //unlock previously blocked shared variables
+  this->set_exposure_mutex_exit();
+
+  return true;
+}
+
+void VisualGpsAlgNode::set_exposure_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->set_exposure_mutex_);
+}
+
+void VisualGpsAlgNode::set_exposure_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->set_exposure_mutex_);
+}
+
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+void VisualGpsAlgNode::node_config_update(Config &config, uint32_t level)
+{
+  this->alg_.lock();
+  this->config_=config;
+  this->alg_.unlock();
+}
+
+void VisualGpsAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return algorithm_base::main<VisualGpsAlgNode>(argc, argv, "visual_gps_alg_node");
+}

srv/set_exposure.srv

+bool auto_exposure
+bool auto_exposure_priority
+float32 exposure_absolute
+---
+bool success