First commit

CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+project(data_synchronizer_pkg)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  camera_info_manager
+  cv_bridge
+  dynamic_reconfigure
+  image_transport
+  msg_srv_action_gestion
+  trajectory_msgs
+  roscpp
+  rospy
+  sensor_msgs
+  tf2_ros
+  ros_wrap
+  thread_management_pkg
+  std_srvs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## 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
+#   sensor_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES data_synchronizer_pkg
+#  CATKIN_DEPENDS camera_info_manager cv_bridge dynamic_reconfigure image_transport msg_srv_action_gestion roscpp rospy sensor_msgs tf2_ros
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/data_synchronizer_pkg.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/data_synchronizer_pkg_node.cpp)
+
+
+add_executable(combine_data_node 
+      src/combine_data_node.cpp
+      src/combine_data.cpp
+      )
+target_link_libraries(combine_data_node PUBLIC ${catkin_LIBRARIES})
+
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_data_synchronizer_pkg.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

include/data_synchronizer_pkg/combine_data.h

+#include "ros/ros.h"
+#include "std_msgs/String.h"
+
+#include "std_msgs/Float64MultiArray.h"
+
+
+#include "std_msgs/Float64.h"
+
+#include <sstream>
+
+#include <vector>
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/LU>
+
+#include <tf2_ros/transform_listener.h>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+#include <std_srvs/SetBool.h>
+#include <msg_srv_action_gestion/SetCamInfo.h>
+
+#include <msg_srv_action_gestion/Float64MultiArrayStamped.h>
+#include <msg_srv_action_gestion/BoolArrayStamped.h>
+#include <msg_srv_action_gestion/poseVelAcc.h>
+#include <msg_srv_action_gestion/LastSynchDataSrv.h>
+#include <trajectory_msgs/JointTrajectory.h>
+#include <msg_srv_action_gestion/SynchronizedDatasStamped.h>
+#include <std_srvs/SetBool.h>
+
+#include "ros_wrap/rosparam_getter.h"
+
+#include "thread_management_pkg/utils.h"
+
+namespace dataCombine
+{
+
+//typedef const boost::function< void(const std_msgs::Float64MultiArrayConstPtr &)>  floatCallbackFunction;
+
+typedef const boost::function< void(const msg_srv_action_gestion::Float64MultiArrayStampedConstPtr &)>  floatCallbackFunction;
+
+typedef const boost::function< void(const msg_srv_action_gestion::BoolArrayStampedConstPtr &)>  boolCallbackFunction;
+
+typedef const boost::function< void(const msg_srv_action_gestion::poseVelAccConstPtr &)>  PVACallbackFunction;
+
+typedef const boost::function< void(const trajectory_msgs::JointTrajectoryConstPtr &)>  JTCallbackFunction;
+
+class dataSynchronizer
+{
+    public:
+      template <typename T> 
+      struct bufferType
+      {
+        std::vector<std::map<double, T > > data;
+        std::vector<int> maxBufferSize;
+      };
+
+
+        template <class T>
+        /**
+         * Checks that the max duration between the first and last element of the buffer is always inferior to maxDur; otherwise, it supress
+         * all the first elements until this condition is satisfied.
+        */
+        void manageBufferSizeByTime(double maxDur, double tol, std::map<double,T> & map)
+        {
+          if (map.size() > 1)
+          {
+              
+            typename std::map<double,T>::iterator itB;
+            typename std::map<double,T>::iterator itE;          
+
+            itB = map.begin();    
+            itE = map.begin();
+            std::advance(itE,map.size()-1);          
+                       // std::cout << "time min: " << itB->first<< std::endl;          
+            //std::cout << "time max : " << itE->first<< std::endl;
+
+            // for (typename std::map<double,T>::iterator it = map.begin(); it!=map.end(); it++)
+            // {
+            //   std::cout << "map_time : " << it->first << std::endl;
+            // }
+            //std::cout << "===" << std::endl;
+
+            if (itE->first - itB->first >= maxDur)
+            {
+            //             std::cout << "time min: " << itB->first<< std::endl;          
+            // std::cout << "time max : " << itE->first<< std::endl;
+              double masterTime = itE->first - maxDur;
+              double slaveTime;
+              T slave;
+              // except if there is an incident, the iterative solution should be the fastest in this application
+              bool found = getSlavePastClosest_it(masterTime, tol, map, slaveTime,slave );
+            // std::cout << "masterTime - buf : " << masterTime << std::endl;          
+            // std::cout << "slaveTime - buf : " << slaveTime << std::endl;
+              if (found)
+              {
+                //std::cout << "found! reduce size" << std::endl;
+                itB = map.find(slaveTime);
+                std::map<double,T> newMap(itB,map.end());
+                map = newMap;
+              }
+              else
+              {
+                // only keep the last element
+               // ROS_INFO("NOT FOUND. CLEAR ALL");
+                map.clear();
+                map[itE->first] = itE->second;
+
+              }
+              
+            }
+            else
+            {
+              //std::cout << "ok" << std::endl;
+              ;;
+            }
+          }
+          
+
+
+        };
+
+        template <class T>
+        /*
+        
+          Takes the closest element from the past, according to a certain tolerance tol; this means that slaveTime < masterTime
+          It also assumes that the map is sorted from lower to higher value.
+
+        */
+        bool getSlavePastClosest(double masterTime, double tol, std::map<double,T> map, double & slaveTime, T & slave)
+        {
+          bool res = getSlavePastClosest_rec(masterTime,tol,map,slaveTime,slave);
+          //std::cout << "masterTime : " << masterTime << std::endl;          
+          // std::cout << "slaveTime : " << slaveTime << std::endl;
+
+          return(res);
+        };
+
+
+      
+        template <class T>
+        /*
+        
+          Takes the closest element from the past, according to a certain tolerance tol; this means that slaveTime < masterTime
+          It also assumes that the map is sorted from lower to higher value.
+
+        */
+        bool getSlavePastClosest_it(double masterTime, double tol, std::map<double,T> map, double & slaveTime, T & slave)
+        {
+          
+          typename std::map<double,T>::iterator it;
+          int i =0;
+          int index = -1;
+
+
+          while (i < map.size() )
+          {
+            it = map.begin();
+            std::advance(it,i);
+            double timeDiff = masterTime-it->first;
+            if (timeDiff <= 0)
+            {
+              index = std::max(i-1,0);
+              i = map.size();
+            }
+            i++;
+
+          }
+
+          it = map.begin();
+          std::advance(it,index);
+
+          if ( masterTime - it->first <= tol )
+          {
+            slave = it->second;
+            slaveTime = it->first;
+            return(true);
+          }
+          else
+          {
+            return(false);
+          }
+
+        };
+
+        template <class T>
+        /*
+          Takes the closest element from the past, according to a certain tolerance tol; this means that slaveTime < masterTime
+          It also assumes that the map is sorted from lower to higher value.
+        */
+        bool getSlavePastClosest_rec(double masterTime, double tol, std::map<double,T> map, double & slaveTime, T & slave)
+        {
+          
+          typename std::map<double,T>::iterator it;
+          typename std::map<double,T>::iterator it2;          
+
+          bool sendFirst = false;
+
+          it = map.begin();    
+
+          if (map.size() > 1)
+          { 
+            it2 = map.begin();  
+            std::advance(it2,map.size()-1);
+            if (it->first <= (masterTime) && it2->first >= (masterTime-tol) )
+            {
+              // we can have a doubt : it is necessary to cut in half the elements of the map.
+              int half_size = map.size()/2;
+              std::advance(it,half_size);
+
+              std::map<double,T> leftMap(map.begin(),it);
+              double leftSlaveTime;
+              T leftSlave;
+
+              std::map<double,T> rightMap(it,map.end()); 
+              double rightSlaveTime;
+              T rightSlave;              
+
+              bool leftRes = getSlavePastClosest_rec(masterTime, tol, leftMap, leftSlaveTime, leftSlave);
+              bool rightRes = getSlavePastClosest_rec(masterTime, tol, rightMap, rightSlaveTime, rightSlave);
+
+              if (leftRes && rightRes)
+              {
+                // two results are true; let's check which one is closest to masterTime
+                if (leftSlaveTime < rightSlaveTime)
+                {
+                  // right has a higher time; it should be the closest
+                  leftRes = false;
+                }
+                else
+                {
+                  rightRes = false;
+                }
+              }
+
+              if (rightRes)
+              {
+                slaveTime = rightSlaveTime;
+                slave = rightSlave;
+              }
+              else
+              {
+                // it is either left result or there is no solution;
+                // anyway, just send the left result (will be filter by next step)
+                slaveTime = leftSlaveTime;
+                slave = leftSlave;
+              }
+            }
+            else
+            {
+              // the closest element to masterTime is not into this submap; 
+              // send first element, which will be whether superior to masterTime (because first > masterTime)
+              // or too much inferior to masterTime (because first < last < masterTime-tol)
+              sendFirst = true;
+            }
+
+          }
+          else
+          {
+            sendFirst = true;
+          }
+
+          if (sendFirst)
+          {
+            slaveTime = it->first;
+            slave = it->second;
+          }
+          
+          // check if the slaveTime that we selected respects the conditions
+          if ( masterTime - slaveTime <= tol && masterTime - slaveTime >= 0 )
+          {
+            return(true);
+          }
+          else
+          {
+            return(false);
+          }
+
+        };        
+
+
+        dataSynchronizer(ros::NodeHandle & nh);
+
+        void runOnce(tf2_ros::Buffer & tfBuffer);
+
+        // callbacks
+        void floatCallback(const msg_srv_action_gestion::Float64MultiArrayStamped::ConstPtr& msg, int index);
+        void boolCallback(const  msg_srv_action_gestion::BoolArrayStampedConstPtr& msg, int index);
+        void poseVelAccCallback( const msg_srv_action_gestion::poseVelAccConstPtr & msg, int index);
+        void jointTrajectoryCallback(const trajectory_msgs::JointTrajectoryConstPtr & msg, int index);
+        bool sendLastSynchMsg(msg_srv_action_gestion::LastSynchDataSrv::Request &req,
+               msg_srv_action_gestion::LastSynchDataSrv::Response &res);
+        // datas synchronizers
+        void synchronizeMasterToDatas(ros::Time masterTime,   
+            msg_srv_action_gestion::SynchronizedDatasStamped msg,
+        tf2_ros::Buffer & tfBuffer
+        );
+
+        void synchronizeTFMasterToDatas(geometry_msgs::TransformStamped transformStampedMaster, tf2_ros::Buffer & tfBuffer);
+        void synchronizeJTMasterToDatas(trajectory_msgs::JointTrajectory JTMaster, tf2_ros::Buffer & tfBuffer);
+        void synchronizePVAMasterToDatas(msg_srv_action_gestion::poseVelAcc PVAMaster, tf2_ros::Buffer & tfBuffer);
+      
+        ros::NodeHandle nh_;
+
+        // Buffer for all the subscribers of Float64MultiArray type
+        // for each float, save a time:value map.
+
+        bufferType< std::vector<float> >floatBuffer;
+        bufferType< std::vector<bool>  >boolBuffer;
+        bufferType<msg_srv_action_gestion::poseVelAcc> PVABuffer;
+        bufferType<trajectory_msgs::JointTrajectory> JTBuffer;
+
+
+        std::string masterType;
+        std::vector<std::string> masterIDS;
+
+
+        std::vector<std::string> allTFSlave;
+
+        std::string TFMaster;
+        std::string globalRef;
+        
+        
+        // contains all the subscribers of Float64MultiArray type
+        std::vector<ros::Subscriber> Float64MultiArraySub;
+
+        std::vector<ros::Subscriber> BoolArraySub;
+
+        ros::Subscriber JTSub;
+        ros::Subscriber PVASub;
+
+        double maxDelayMasterSlave;
+
+        std::string pubType;
+        std::string pubName;
+
+        ros::Publisher synchPub;
+        ros::ServiceServer synchServer;
+        bool isPublishingAuthorized;
+
+        msg_srv_action_gestion::SynchronizedDatasStamped lastSynchMsg;
+        bool synchActive;
+
+
+
+
+};
+
+}
\ No newline at end of file

package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>data_synchronizer_pkg</name>
+  <version>0.0.0</version>
+  <description>The data_synchronizer_pkg package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="auctus@todo.todo">auctus</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>TODO</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/data_synchronizer_pkg</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>camera_info_manager</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>msg_srv_action_gestion</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_export_depend>camera_info_manager</build_export_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>dynamic_reconfigure</build_export_depend>
+  <build_export_depend>image_transport</build_export_depend>
+  <build_export_depend>msg_srv_action_gestion</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
+  <exec_depend>camera_info_manager</exec_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>image_transport</exec_depend>
+  <exec_depend>msg_srv_action_gestion</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+
+  <depend>trajectory_msgs</depend>
+
+  <depend>ros_wrap</depend>
+  <depend>std_srvs</depend>
+
+
+  <depend>thread_management_pkg</depend>
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/combine_data.cpp

+#include "data_synchronizer_pkg/combine_data.h"
+
+
+/* RECALLS : 
+  1920x1080 : 
+    1920 : width
+    1080 : height
+https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#gafa5aa3f40a472bda956b4c27bec67438
+
+image : M[0:height,0:width] ==> confirmed
+image.size() : [1920 x 1080] : [width x height]
+x = M[:,i] (in 0:width = 1920)
+y = M[i,:] (in 0:height = 1080)
+==> cx : on col : width
+==> cy : on row : height
+
+==> u : on col
+==> v : on row
+
+Move on x axis ==> stay at same row, move on cols
+Move on y axis ==> stay at same col, move on rows
+
+*/
+
+namespace dataCombine
+{
+
+    bool dataSynchronizer::sendLastSynchMsg(msg_srv_action_gestion::LastSynchDataSrv::Request &req,
+               msg_srv_action_gestion::LastSynchDataSrv::Response &res)
+    {
+        res.success = synchActive;
+        if (synchActive)
+        {
+            res.lastSynchMsg = lastSynchMsg;
+        }
+
+        return(true);
+    }
+
+    void dataSynchronizer::poseVelAccCallback(const msg_srv_action_gestion::poseVelAccConstPtr & msg, int index)
+    {
+        PVABuffer.data[index][msg->header.stamp.toSec()] =  *msg ;
+
+        manageBufferSizeByTime(maxDelayMasterSlave*1.5, maxDelayMasterSlave, PVABuffer.data[index]);
+
+    }
+
+    void dataSynchronizer::jointTrajectoryCallback(const trajectory_msgs::JointTrajectoryConstPtr & msg, int index)
+    {
+        JTBuffer.data[index][msg->header.stamp.toSec()] = *msg;
+
+        manageBufferSizeByTime(maxDelayMasterSlave*1.5, maxDelayMasterSlave, JTBuffer.data[index]);
+    }
+
+    void dataSynchronizer::boolCallback(const  msg_srv_action_gestion::BoolArrayStampedConstPtr& msg, int index)
+    {
+        std::vector<bool> allData;
+        
+        for (int i = 0; i < msg->data.size(); i++)
+        {
+            allData.push_back(msg->data[i]);
+        }
+
+        boolBuffer.data[index][msg->header.stamp.toSec()] = allData;
+
+        manageBufferSizeByTime(maxDelayMasterSlave*1.5, maxDelayMasterSlave, boolBuffer.data[index]);
+        
+    }
+
+    void dataSynchronizer::floatCallback(const  msg_srv_action_gestion::Float64MultiArrayStampedConstPtr& msg, int index)
+    {
+        std::vector<float> allData;
+        
+        for (int i = 0; i < msg->array.data.size(); i++)
+        {
+            allData.push_back(msg->array.data[i]);
+        }
+
+        floatBuffer.data[index][msg->header.stamp.toSec()] = allData;
+        //ROS_INFO("GOT NEW DATA");
+
+        manageBufferSizeByTime(maxDelayMasterSlave, maxDelayMasterSlave, floatBuffer.data[index]);
+        
+    }
+
+    void dataSynchronizer::synchronizeMasterToDatas(ros::Time masterTime,   
+        msg_srv_action_gestion::SynchronizedDatasStamped msg,
+     tf2_ros::Buffer & tfBuffer
+     )
+     {
+        msg.masterType = masterType;
+        msg.header.stamp = masterTime;
+
+        std::vector<geometry_msgs::TransformStamped> allTFSlaveSynch;
+        std::vector< std::pair<double,std::vector<float> > > allFloatSlaveSynch;
+        std::vector< std::pair<double,std::vector<bool> > > allBoolSlaveSynch;
+        std::vector<msg_srv_action_gestion::poseVelAcc> PVASynch;
+        std::vector<trajectory_msgs::JointTrajectory> JTSynch;  
+
+
+        bool guarantedSynch = true;
+        try
+        {
+
+            int i = 0;        
+            double slaveTime;                
+
+            // TF
+
+            while (i < allTFSlave.size() && guarantedSynch)
+            {
+                try
+                {
+                    geometry_msgs::TransformStamped transformStamped;                
+                    transformStamped = tfBuffer.lookupTransform(globalRef, allTFSlave[i], masterTime, ros::Duration(1.0));
+                    double timeDiff = masterTime.toSec() - transformStamped.header.stamp.toSec();
+
+                    guarantedSynch = ( timeDiff<maxDelayMasterSlave && timeDiff > 0);
+                    if (guarantedSynch)
+                    {
+                        allTFSlaveSynch.push_back(transformStamped);
+                    }
+                    i++;
+                }
+                catch (tf2::TransformException &ex) {
+                    guarantedSynch = false;
+ 
+                }       
+            }
+
+            //float
+            i = 0;
+            while (i < floatBuffer.data.size() && guarantedSynch)
+            {
+                        //ROS_INFO("PROCESS FLOAT DATA");
+
+                std::vector<float> floatVec;
+                if (floatBuffer.data[i].size()>0 )
+                {
+                            //ROS_INFO("GET NEWEST DATA");
+
+                    guarantedSynch = getSlavePastClosest(masterTime.toSec(),maxDelayMasterSlave, floatBuffer.data[i], slaveTime, floatVec  );
+
+                    if (guarantedSynch)
+                    {
+                                //ROS_INFO("ADD DATA");
+
+                        allFloatSlaveSynch.push_back( std::pair<double,std::vector<float> >(slaveTime,floatVec) );
+                    }
+                    i++;
+                }
+                else
+                {
+                    guarantedSynch = false;
+                }
+
+                
+            }
+
+            // bool
+            i = 0;
+            while (i < boolBuffer.data.size() && guarantedSynch)
+            {
+                std::vector<bool> boolVec;
+                guarantedSynch = getSlavePastClosest(masterTime.toSec(),maxDelayMasterSlave, boolBuffer.data[i], slaveTime, boolVec  );
+                if (guarantedSynch)
+                {
+                    allBoolSlaveSynch.push_back( std::pair<double,std::vector<bool> >(slaveTime,boolVec) );
+                }
+                i++;
+            }
+
+            // msg_srv_action_gestion::poseVelAcc
+            if (masterType == "PVA")
+            {
+                i = 1;
+            }
+            else
+            {
+                i = 0;
+            }
+            while (i < PVABuffer.data.size() && guarantedSynch)
+            {
+                msg_srv_action_gestion::poseVelAcc PVAmsg;
+                guarantedSynch = getSlavePastClosest(masterTime.toSec(),maxDelayMasterSlave, PVABuffer.data[i], slaveTime, PVAmsg);
+                if (guarantedSynch)
+                {
+                    PVASynch.push_back( PVAmsg );
+                }
+                i++;                
+            }
+
+
+            // trajectory_msgs::JointTrajectory
+            if (masterType == "JT")
+            {
+                i = 1;
+            }
+            else
+            {
+                i = 0;
+            }
+            while (i < JTBuffer.data.size() && guarantedSynch)
+            {
+                trajectory_msgs::JointTrajectory JTmsg;
+                double slaveTime;
+                guarantedSynch = getSlavePastClosest(masterTime.toSec(),maxDelayMasterSlave, JTBuffer.data[i], slaveTime, JTmsg  );
+                if (guarantedSynch)
+                {
+                    JTSynch.push_back( JTmsg );
+                }
+                i++;        
+            }
+
+
+        }
+
+        catch (tf2::TransformException &ex) {
+            //std::cout << &ex << std::endl;
+            guarantedSynch = false;
+        }
+
+        if (guarantedSynch)
+        {
+            // add TF slave infos for msg
+            for (int i = 0; i < allTFSlaveSynch.size(); i++)
+            {
+                msg.TF.push_back(allTFSlaveSynch[i]);
+            }
+            // add float infos for msg
+            for (int i = 0; i < allFloatSlaveSynch.size(); i++)
+            {
+                msg_srv_action_gestion::Float64MultiArrayStamped floatMsg;  
+                for (int j = 0; j < allFloatSlaveSynch[i].second.size(); j++)              
+                {
+                    floatMsg.array.data.push_back( allFloatSlaveSynch[i].second[j] );
+                }
+                floatMsg.header.stamp = ros::Time(allFloatSlaveSynch[i].first);
+                
+                msg.floatArray.push_back(floatMsg);
+            }
+            // add bool infos for msg               
+            for (int i = 0; i < allBoolSlaveSynch.size(); i++)
+            {
+                msg_srv_action_gestion::BoolArrayStamped boolMsg;  
+                for (int j = 0; j < allBoolSlaveSynch[i].second.size(); j++)              
+                {
+                    boolMsg.data.push_back( allBoolSlaveSynch[i].second[j] );
+                }
+                boolMsg.header.stamp = ros::Time(allFloatSlaveSynch[i].first);
+                
+                msg.boolArray.push_back(boolMsg);
+            }
+            // add PVA slave infos for msg            
+            for (int i = 0; i < PVASynch.size(); i++)
+            {
+                msg.PVA = PVASynch[0];
+            }
+            // add JT slave infos for msg            
+            for (int i = 0; i < JTSynch.size(); i++)
+            {
+                msg.JT = JTSynch[0];
+            }    
+
+            // manage publication
+            lastSynchMsg = msg;
+            synchActive = true;
+
+            if (isPublishingAuthorized && pubType == "Active")
+            {
+                synchPub.publish(lastSynchMsg);
+            }
+            
+        }
+        else
+        {
+            synchActive = false;
+        }
+
+    }
+
+
+
+
+    void dataSynchronizer::synchronizePVAMasterToDatas(msg_srv_action_gestion::poseVelAcc PVAMaster, tf2_ros::Buffer & tfBuffer)
+    {
+        ros::Time masterTime = PVAMaster.header.stamp;
+
+        msg_srv_action_gestion::SynchronizedDatasStamped msg;
+
+        msg.PVA = PVAMaster;        
+
+        synchronizeMasterToDatas(masterTime,msg,tfBuffer);
+
+    }
+
+    void dataSynchronizer::synchronizeJTMasterToDatas(trajectory_msgs::JointTrajectory JTMaster, tf2_ros::Buffer & tfBuffer)
+    {
+        ros::Time masterTime = JTMaster.header.stamp;
+
+        msg_srv_action_gestion::SynchronizedDatasStamped msg;
+
+        msg.JT = JTMaster;        
+
+        synchronizeMasterToDatas(masterTime,msg,tfBuffer);
+
+    }    
+
+    void dataSynchronizer::synchronizeTFMasterToDatas(geometry_msgs::TransformStamped transformStampedMaster, tf2_ros::Buffer & tfBuffer)
+    {
+        ros::Time masterTime = transformStampedMaster.header.stamp;
+
+        msg_srv_action_gestion::SynchronizedDatasStamped msg;
+
+        msg.TF.push_back( transformStampedMaster );
+
+        synchronizeMasterToDatas(masterTime,msg,tfBuffer);
+    }    
+
+    void dataSynchronizer::runOnce(tf2_ros::Buffer & tfBuffer)
+    {
+        if (masterType == "TF")
+        {
+            geometry_msgs::TransformStamped transformStamped;
+
+            try
+            {
+                transformStamped = tfBuffer.lookupTransform(globalRef, TFMaster, ros::Time(0));
+                //ROS_INFO("Time : %f", transformStamped.header.stamp.toSec());
+
+                if  (transformStamped.header.stamp.toSec() == 0.0)
+                {
+                    transformStamped.header.stamp = ros::Time::now();
+                }
+                
+                synchronizeTFMasterToDatas(transformStamped,tfBuffer);
+            }
+
+            catch (tf2::TransformException &ex) {
+                //std::cout << &ex << std::endl;
+                ;;
+            }       
+        }
+
+        else if (masterType == "PVA")
+        {
+            if (PVABuffer.data[0].size() > 0)
+            {
+                synchronizePVAMasterToDatas(PVABuffer.data[0].begin()->second,tfBuffer);
+
+                if (synchActive)
+                {
+                    PVABuffer.data[0].erase(PVABuffer.data[0].begin());
+                }
+
+            }
+        }
+
+        else if (masterType == "JT")
+        {
+            synchronizeJTMasterToDatas(JTBuffer.data[0].begin()->second,tfBuffer);
+
+            if (synchActive)
+            {
+                JTBuffer.data[0].erase(JTBuffer.data[0].begin());
+            }            
+
+        }
+
+        ros::spinOnce();
+    };
+
+    dataSynchronizer::dataSynchronizer(ros::NodeHandle & nh
+    ):
+    nh_(nh)
+    {   
+
+        std::string nodeName = ros::this_node::getName();
+        nodeName += "/";
+        std::string configNS;
+        rosWrap::getRosParam(nodeName + "configNS", configNS);
+
+
+        rosWrap::getRosParam(configNS+"/master_type", masterType);        
+
+        /*
+        This is just the mean necessary to get the master informations : 
+            * For TF, it is <name of master frame> that will be put into
+            lookupTransform
+            * For PoseVelAcc (PVA), it is <topic name>
+            * For JointTrajectory (JT), it is <topic name>
+        */
+        rosWrap::getRosParam(configNS+"/master_ids", masterIDS);
+
+        int noNeedGlobalRef = 1;
+        std::string PVATopic;       
+        std::string JTTopic;
+        bool gotTopic_PVA = false; 
+        bool gotTopic_JT = false;
+
+        if (masterType == "TF")
+        {
+            noNeedGlobalRef = 0;
+            TFMaster = masterIDS[0];
+        }
+        else if (masterType == "PVA")
+        {
+            PVATopic = masterIDS[0];
+            gotTopic_PVA = true;
+        }
+        else if (masterType=="JT")
+        {
+            JTTopic = masterIDS[0];
+            gotTopic_JT = true;
+        }
+
+        if (!gotTopic_PVA)
+        {
+            gotTopic_PVA = rosWrap::getRosParam(configNS+"/poseVelAcc_slave", PVATopic,1);
+        }
+
+        if (!gotTopic_JT)
+        {
+            gotTopic_JT = rosWrap::getRosParam(configNS+"/jointTrajectory_slave", JTTopic,1);
+        }
+
+        if (gotTopic_PVA)
+        {
+            dataCombine::PVACallbackFunction callback = boost::bind(&dataSynchronizer::poseVelAccCallback,this,_1,0);             
+            PVASub = nh.subscribe(PVATopic,1000,callback );
+        }
+
+        if (gotTopic_JT)
+        {
+            dataCombine::JTCallbackFunction callback = boost::bind(&dataSynchronizer::jointTrajectoryCallback,this,_1,0);             
+            JTSub = nh.subscribe(JTTopic,1000,callback );
+        }        
+
+
+        // names of TFSlaves
+        rosWrap::getRosParam(configNS+"/TF_slave", allTFSlave,1);
+        // names of globalRef to which all TF will be expressed
+        rosWrap::getRosParam(configNS+"/global_ref", globalRef,noNeedGlobalRef);
+
+        // names of float64MultiArray topics
+        std::vector<std::string> float64MultiArray_slave;
+        rosWrap::getRosParam(configNS+"/float64MultiArray_slave", float64MultiArray_slave,1);
+        // names of boolArray topics
+        std::vector<std::string> boolArray_slave;
+        rosWrap::getRosParam(configNS+"/boolArray_slave", boolArray_slave,1);
+
+
+        // Maximum authorized duration between Master and Slave, to consider
+        // that those two datas are still "synchronized".
+        // (could be the time to get 2 elements of the slowest topic)
+        rosWrap::getRosParam(configNS+"/maxSynchDelay", maxDelayMasterSlave);
+
+        // Publication type of the synchronized datas : 
+        // * Active : publish all datas on a topic
+        // * Passive : wait for a service call to send the information; or just keep the lastSynchMsg
+        // into the object to be taken.
+        rosWrap::getRosParam(configNS+"/pubType", pubType);
+
+        // Indicates the topic or service name. Could be left as blank if we do not want to have any ROS
+        // mean to publish available.
+        rosWrap::getRosParam(configNS+"/pubName", pubName);
+
+        if (pubName != "")
+        {
+            isPublishingAuthorized = true;
+            if (pubType == "Active")
+            {
+                synchPub = nh.advertise<msg_srv_action_gestion::SynchronizedDatasStamped>(pubName,10);
+            }
+
+            else if (pubType == "Passive")
+            {
+                synchServer = nh.advertiseService(pubName, &dataSynchronizer::sendLastSynchMsg, this);
+            }
+        }
+        else
+        {
+            isPublishingAuthorized = false;
+        }
+
+
+        // create all subscribers
+
+        
+
+        for (int i = 0; i < float64MultiArray_slave.size(); i++)
+        {
+            std::map<double,std::vector<float> > oneBufFloat;
+            floatBuffer.data.push_back(oneBufFloat);
+
+            // inspired from https://answers.ros.org/question/68434/get-topic-name-in-callback/
+            // and https://answers.ros.org/question/11810/how-to-pass-arguments-tofrom-subscriber-callback-functions/
+            dataCombine::floatCallbackFunction callback = boost::bind(&dataSynchronizer::floatCallback,this,_1,i); 
+            Float64MultiArraySub.push_back( nh.subscribe(float64MultiArray_slave[i],1000,callback ) );
+        }
+
+        for (int i = 0; i < boolArray_slave.size(); i++)
+        {
+            std::map<double,std::vector<bool> > oneBufBool;
+            boolBuffer.data.push_back(oneBufBool);
+
+            // inspired from https://answers.ros.org/question/68434/get-topic-name-in-callback/
+            // and https://answers.ros.org/question/11810/how-to-pass-arguments-tofrom-subscriber-callback-functions/
+            dataCombine::boolCallbackFunction callback = boost::bind(&dataSynchronizer::boolCallback,this,_1,i); 
+            BoolArraySub.push_back( nh.subscribe(float64MultiArray_slave[i],1000,callback ) );
+        }        
+
+        synchActive = false;
+        ROS_INFO("Initialization done");
+    }
+
+};

src/combine_data_node.cpp

+#include "data_synchronizer_pkg/combine_data.h"
+
+
+/* RECALLS : 
+  1920x1080 : 
+    1920 : width
+    1080 : height
+https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#gafa5aa3f40a472bda956b4c27bec67438
+
+image : M[0:height,0:width] ==> confirmed
+image.size() : [1920 x 1080] : [width x height]
+x = M[:,i] (in 0:width = 1920)
+y = M[i,:] (in 0:height = 1080)
+==> cx : on col : width
+==> cy : on row : height
+
+==> u : on col
+==> v : on row
+
+Move on x axis ==> stay at same row, move on cols
+Move on y axis ==> stay at same col, move on rows
+
+*/
+
+/**
+ * This tutorial demonstrates simple sending of messages over the ROS system.
+ */
+int main(int argc, char **argv)
+{
+  /**
+   * The ros::init() function needs to see argc and argv so that it can perform
+   * any ROS arguments and name remapping that were provided at the command line.
+   * For programmatic remappings you can use a different version of init() which takes
+   * remappings directly, but for most command-line programs, passing argc and argv is
+   * the easiest way to do it.  The third argument to init() is the name of the node.
+   *
+   * You must call one of the versions of ros::init() before using any other
+   * part of the ROS system.
+   */
+  ros::init(argc, argv, "talker");
+  ros::NodeHandle nh;
+
+  tf2_ros::Buffer buffer(ros::Duration(3.0));
+  tf2_ros::TransformListener listener(buffer);
+
+
+  dataCombine::dataSynchronizer fovM(nh);
+
+  ros::Rate r(100);
+
+  while (ros::ok() )
+  {
+    fovM.runOnce(buffer);
+    ros::spinOnce();
+    r.sleep();
+  }
+  return 0;
+}
\ No newline at end of file