First commit. Node creation. Two subscribers (IMU,px4). Working on read_params from YAML file

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(eskf_odom)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm px_comm sensor_msgs tf)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+find_package(eskf_odometry REQUIRED)
+find_package(Eigen 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
+#   Service1.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/EskfOdom.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+ CATKIN_DEPENDS iri_base_algorithm px_comm sensor_msgs tf
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+# DEPENDS eskf_odometry
+)
+
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+include_directories(include ${catkin_INCLUDE_DIRS} ${eskf_odometry_INCLUDE_DIR} ${Eigen_INCLUDE_DIRS})
+
+MESSAGE(${eskf_odometry_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/eskf_odom_alg.cpp src/eskf_odom_alg_node.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${eskf_odometry_LIBRARY} ${Eigen_LIBRARIES})
+
+# ******************************************************************** 
+#               Add message headers dependencies 
+# ******************************************************************** 
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} px_comm_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
+# Check G++ compiler version
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+else()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+
+set(CMAKE_BUILD_TYPE Release) 

cfg/EskfOdom.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='eskf_odom'
+
+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)
+
+exit(gen.generate(PACKAGE, "EskfOdomAlgorithm", "EskfOdom"))

include/eskf_odom_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 _eskf_odom_alg_h_
+#define _eskf_odom_alg_h_
+
+#include <eskf_odom/EskfOdomConfig.h>
+
+#include <eskf_odometry.h>
+
+//include eskf_odom_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class EskfOdomAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the EskfOdomConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    pthread_mutex_t access_;    
+
+    // private attributes and methods
+
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the EskfOdomConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef eskf_odom::EskfOdomConfig 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.
+    */
+    EskfOdomAlgorithm(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& new_cfg, uint32_t level=0);
+
+    // here define all eskf_odom_alg interface methods to retrieve and set
+    // the driver parameters
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~EskfOdomAlgorithm(void);
+};
+
+#endif

include/eskf_odom_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 _eskf_odom_alg_node_h_
+#define _eskf_odom_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "eskf_odom_alg.h"
+
+// [publisher subscriber headers]
+#include <px_comm/OpticalFlow.h>
+#include <sensor_msgs/Imu.h>
+
+// [service client headers]
+
+// [action server client headers]
+
+#include <Eigen/Dense>
+
+using namespace std;
+using namespace Eigen;
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>
+{
+  private:
+
+    // [publisher attributes]
+
+    // [subscriber attributes]
+    ros::Subscriber px4_of_subscriber_;
+    void px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg);
+    pthread_mutex_t px4_of_mutex_;
+    void px4_of_mutex_enter(void);
+    void px4_of_mutex_exit(void);
+    ros::Subscriber imu_subscriber_;
+    void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
+    pthread_mutex_t imu_mutex_;
+    void imu_mutex_enter(void);
+    void imu_mutex_exit(void);
+
+    // [service attributes]
+
+    // [client attributes]
+
+    // [action server attributes]
+
+    // [action client attributes]
+
+  public:
+   /**
+    * \brief Constructor
+    * 
+    * This constructor initializes specific class attributes and all ROS
+    * communications variables to enable message exchange.
+    */
+    EskfOdomAlgNode(void);
+
+   /**
+    * \brief Destructor
+    * 
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~EskfOdomAlgNode(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]
+
+   /**
+    * \brief Read Parameters
+    * 
+    * This function reads the parameters from the YAML file specified in the launch file
+    */
+    void read_params(void);
+
+   /**
+    * \brief Read 3-Vector parameter
+    * 
+    * This function reads a 3-Vector parameter specified by param_name 
+    */
+    Vector3d read_vec3(const string& param_name);
+};
+
+#endif

launch/eskf_odom.launch

+<launch>
+
+  <node name="eskf_odom" pkg="eskf_odom" type="eskf_odom" output="screen">
+    <remap from="imu" to="/imu" />
+    <remap from="px4_of" to="/px4_of" />
+    <rosparam file="$(find eskf_odom)/launch/params.yaml"/>
+  </node>
+
+</launch>
\ No newline at end of file

launch/params.yaml

+# Initial Error-State values (filter)
+dp0_std: [0.0, 0.0, 0.0]
+dv0_std: [0.0, 0.0, 0.0]
+dtheta0_std: [0.0, 0.0, 0.0]
+dab0_std: [0.0, 0.0, 0.0]
+dwb0_std: [0.0, 0.0, 0.0]
+dg0_std: [0.0, 0.0, 0.0]
+
+# Frame r.t. orientation derivation is computed (g:global, l:local)
+frame: global
\ No newline at end of file

package.xml

+<?xml version="1.0"?>
+<package>
+  <name>eskf_odom</name>
+  <version>0.0.1</version>
+  <description>The eskf_odom package</description>
+  <maintainer email="asantamaria@iri.upc.edu">asantamaria</maintainer>
+  <license>LGPL</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>px_comm</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>eskf_odometry</build_depend>
+
+  <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>px_comm</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>eskf_odometry</run_depend>
+
+  <export>
+  </export>
+</package>
\ No newline at end of file

src/eskf_odom_alg.cpp

+#include "eskf_odom_alg.h"
+
+EskfOdomAlgorithm::EskfOdomAlgorithm(void)
+{
+  pthread_mutex_init(&this->access_,NULL);
+}
+
+EskfOdomAlgorithm::~EskfOdomAlgorithm(void)
+{
+  pthread_mutex_destroy(&this->access_);
+}
+
+void EskfOdomAlgorithm::config_update(Config& new_cfg, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=new_cfg;
+  
+  this->unlock();
+}
+
+// EskfOdomAlgorithm Public API

src/eskf_odom_alg_node.cpp

+#include "eskf_odom_alg_node.h"
+
+using namespace std;
+using namespace Eigen;
+
+EskfOdomAlgNode::EskfOdomAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>()
+{
+  //init class attributes if necessary
+  //this->loop_rate_ = 2;//in [Hz]
+
+  // [init publishers]
+  
+  // [init subscribers]
+  this->px4_of_subscriber_ = this->public_node_handle_.subscribe("px4_of", 1, &EskfOdomAlgNode::px4_of_callback, this); 
+  pthread_mutex_init(&this->px4_of_mutex_,NULL);
+
+  this->imu_subscriber_ = this->public_node_handle_.subscribe("imu", 1, &EskfOdomAlgNode::imu_callback, this); 
+  pthread_mutex_init(&this->imu_mutex_,NULL);
+
+  read_params();
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+EskfOdomAlgNode::~EskfOdomAlgNode(void)
+{
+  // [free dynamic memory]
+  pthread_mutex_destroy(&this->px4_of_mutex_);
+  pthread_mutex_destroy(&this->imu_mutex_);
+}
+
+void EskfOdomAlgNode::read_params(void)
+{
+  // double kk;
+  // this->public_node_handle_.param<double>("kk", kk, 0.0);
+
+  // Filter parameters
+  params f_params;
+
+  // Initial Error-State values (filter)
+  f_params.dp0_std = read_vec3("dp0_std");
+  f_params.dv0_std = read_vec3("dv0_std");
+  f_params.dtheta0_std = read_vec3("dtheta0_std");
+  f_params.dab0_std = read_vec3("dab0_std");
+  f_params.dwb0_std = read_vec3("dwb0_std");
+  f_params.dg0_std = read_vec3("dg0_std");
+
+  // Frame r.t. orientation derivation is computed (g:global, l:local)
+  string frame;
+  this->public_node_handle_.param<string>("frame", frame, "global");
+  if (frame == "global")
+    f_params.frame = 'g';
+  else if (frame == "local")
+    f_params.frame = 'l';
+  else cout << "ERROR loading frame: Wrong frame name (should be local or global)" << endl;
+
+  // Print read values
+  cout << ">> ESKF_odom, loaded parameters: " << endl;
+  cout << "  ______Filter parameters" << endl;
+  cout << "  dp0_std: " << f_params.dp0_std(0) << "," << f_params.dp0_std(1) << "," << f_params.dp0_std(2)<< endl;
+  cout << "  dv0_std: " << f_params.dv0_std(0) << "," << f_params.dv0_std(1) << "," << f_params.dv0_std(2)<< endl;
+  cout << "  dtheta0_std: " << f_params.dtheta0_std(0) << "," << f_params.dtheta0_std(1) << "," << f_params.dtheta0_std(2)<< endl;
+  cout << "  dab0_std: " << f_params.dab0_std(0) << "," << f_params.dab0_std(1) << "," << f_params.dab0_std(2)<< endl;
+  cout << "  dwb0_std: " << f_params.dwb0_std(0) << "," << f_params.dwb0_std(1) << "," << f_params.dwb0_std(2)<< endl;
+  cout << "  dg0_std: " << f_params.dg0_std(0) << "," << f_params.dg0_std(1) << "," << f_params.dg0_std(2)<< endl;
+  cout << "  frame: " << f_params.frame << endl;
+}
+
+Vector3d EskfOdomAlgNode::read_vec3(const string& param_name)
+{
+  Vector3d params = Vector3d::Zero();
+
+  XmlRpc::XmlRpcValue my_list;
+  this->public_node_handle_.getParam(param_name, my_list);
+
+  if (my_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
+    int vec_size = int(my_list.size());
+    if (vec_size == 3){
+      for (int ii=0; ii<vec_size; ii++) 
+        params(ii) = my_list[ii]; 
+    }
+    else 
+      cout << "ERROR loading " << param_name << ": Wrong elements number" << endl;
+  }
+  else
+    cout << "ERROR loading " << param_name << ": Wrong element type" << endl; 
+  return params;
+}
+
+void EskfOdomAlgNode::mainNodeThread(void)
+{
+  // [fill msg structures]
+