Merge branch 'devel' into 'master'

new release

See merge request !10

.gitignore

 /.settings/
 /.cproject
 /.project
+/.clangd
\ No newline at end of file

.gitlab-ci.yml

+stages:
+  - license
+  - build_and_test
+  - deploy_packages
+  
+############ YAML ANCHORS ############
+.preliminaries_template: &preliminaries_definition
+  ## Install ssh-agent if not already installed, it is required by Docker.
+  ## (change apt-get to yum if you use an RPM-based image)
+  - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
+
+  ## Run ssh-agent (inside the build environment)
+  - eval $(ssh-agent -s)
+
+  ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
+  ## We're using tr to fix line endings which makes ed25519 keys work
+  ## without extra base64 encoding.
+  ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
+  - mkdir -p ~/.ssh
+  - chmod 700 ~/.ssh  
+  - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
+  # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
+  - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
+
+  # update apt
+  - apt-get update
+
+  # create ci_deps folder (if not exists)
+  - mkdir -pv ci_deps
+
+  # manually source ros setup.bash
+  - source /root/catkin_ws/devel/setup.bash
+  - roscd # check that it works
+
+.license_header_template: &license_header_definition
+  - cd $CI_PROJECT_DIR
+
+  # configure git
+  - export CI_NEW_BRANCH=ci_processing$RANDOM
+  - echo creating new temporary branch... $CI_NEW_BRANCH
+  - git config --global user.email "${CI_EMAIL}"
+  - git config --global user.name "${CI_USERNAME}"
+  - git checkout -b $CI_NEW_BRANCH # temporary branch
+
+  # license headers
+  - export CURRENT_YEAR=$( date +'%Y' )
+  - echo "current year:" ${CURRENT_YEAR}
+  - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
+      # add license headers to new files
+  -   echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
+  -   ./ci_deps/wolf/wolf_scripts/license_manager.sh --add --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
+  - else
+      # update license headers of all files
+  -   export PREV_YEAR=$(( CURRENT_YEAR-1 ))
+  -   echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
+  -   git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
+  -   sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
+  -   ./ci_deps/wolf/wolf_scripts/license_manager.sh --update --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
+  - fi
+
+  # push changes (if any)
+  - if git commit -a -m "[skip ci] license headers added or modified" ; then
+  -   git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+  -   git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
+  - else
+  -   echo "No changes, nothing to commit!"
+  - fi
+
+.install_wolf_template: &install_wolf_definition
+  - cd ${CI_PROJECT_DIR}/ci_deps
+  - if [ -d wolf ]; then
+  -   echo "directory wolf exists"
+  -   cd wolf
+  -   git checkout devel
+  -   git pull
+  -   git checkout $WOLF_CORE_BRANCH
+  -   git pull
+  - else
+  -   git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+  -   cd wolf
+  - fi
+  - mkdir -pv build
+  - cd build
+  - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF ..
+  - make -j$(nproc)
+  - make install
+
+.build_and_test_template: &build_and_test_definition
+  - roscd
+  - cd ../src
+  - git clone ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git
+  - cd wolf_ros_node
+  - git checkout $CI_COMMIT_BRANCH
+  - cd ../..
+  - catkin_make
+
+############ LICENSE HEADERS ############
+license_headers:
+  stage: license
+  image: labrobotica/wolf_deps_ros:20.04
+  cache: []
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+  script:
+    - *license_header_definition
+
+############ UBUNTU 18.04 TEST ############
+build_and_test:bionic:
+  stage: build_and_test
+  image: labrobotica/wolf_deps_ros:18.04
+  cache:
+    - key: wolf-bionic
+      paths:
+      - ci_deps/wolf/
+    - key: catkinws-src-bionic
+      paths:
+      - catkin_ws/src
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+  script:
+    - *build_and_test_definition
+
+############ UBUNTU 20.04 TEST ############
+build_and_test:focal:
+  stage: build_and_test
+  image: labrobotica/wolf_deps_ros:20.04
+  cache:
+    - key: wolf-focal
+      paths:
+      - ci_deps/wolf/
+    - key: catkinws-src-focal
+      paths:
+      - catkin_ws/src
+  except:
+    - master
+  before_script:
+    - *preliminaries_definition
+    - *install_wolf_definition
+  script:
+    - *build_and_test_definition
+
+############ DEPLOY ROS PACKAGES ############
+deploy_imu:
+  stage: deploy_packages
+  variables:
+    WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
+    WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
+    WOLF_ROS_CORE_BRANCH: $CI_COMMIT_BRANCH
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_imu
+
+deploy_gnss:
+  stage: deploy_packages
+  variables:
+    WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
+    WOLF_APRILTAG_BRANCH: $WOLF_GNSS_BRANCH
+    WOLF_ROS_CORE_BRANCH: $CI_COMMIT_BRANCH
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_gnss
+
+deploy_laser:
+  stage: deploy_packages
+  variables:
+    WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
+    WOLF_LASER_BRANCH: $WOLF_LASER_BRANCH
+    WOLF_ROS_CORE_BRANCH: $CI_COMMIT_BRANCH
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_laser
+
+deploy_apriltag:
+  stage: deploy_packages
+  variables:
+    WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
+    WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
+    WOLF_APRILTAG_BRANCH: $WOLF_APRILTAG_BRANCH
+    WOLF_ROS_CORE_BRANCH: $CI_COMMIT_BRANCH
+  trigger: 
+    project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_apriltag

CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(wolf_ros_node)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
+add_compile_options(-std=c++14)
 
 # SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/wolf_ros_wrapper/cmake_modules")
 ## Find catkin macros and libraries
@@ -13,15 +13,18 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   sensor_msgs
   std_msgs
+  nav_msgs
   tf
-  dynamic_reconfigure
+  tf_conversions
+  tf2_ros
+#  dynamic_reconfigure
 )
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 # find_package(Ceres REQUIRED)
 # find_package(Eigen3 REQUIRED)
-find_package(wolf REQUIRED)
+find_package(wolfcore REQUIRED)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -94,9 +97,9 @@ find_package(wolf REQUIRED)
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-generate_dynamic_reconfigure_options(
-  cfg/WolfROS.cfg
-)
+##generate_dynamic_reconfigure_options(
+##  cfg/WolfROS.cfg
+##)
 
 ###################################
 ## catkin specific configuration ##
@@ -120,11 +123,11 @@ INCLUDE_DIRS include
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-message("Wolf include path: ${wolf_INCLUDE_DIR}")
+message("Wolf include path: ${wolfcore_INCLUDE_DIRS}")
 include_directories(
     include
   ${EIGEN_INCLUDE_DIRS}
-  ${wolf_INCLUDE_DIRS}
+  ${wolfcore_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
   ${CERES_INCLUDE_DIRS}
 )
@@ -144,10 +147,17 @@ include_directories(
 ## 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} src/wolf_ros_node.cpp)
-add_library(wolf_subscriber_odom2D src/wolf_ros_subscriber_odom2D.cpp)
-add_library(wolf_subscriber_other src/wolf_ros_subscriber_other.cpp)
-add_library(wolf_ros_visualizer src/wolf_ros_visualizer.cpp)
+add_executable(${PROJECT_NAME} src/node.cpp)
+
+add_library(subscriber_${PROJECT_NAME}
+  			src/subscriber_diffdrive.cpp
+  			src/subscriber_odom2d.cpp)
+add_library(publisher_${PROJECT_NAME}
+  			src/publisher_graph.cpp
+  			src/publisher_pose.cpp
+  			src/publisher_trajectory.cpp
+  			src/publisher_state_block.cpp
+  			src/publisher_tf.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -157,17 +167,29 @@ add_library(wolf_ros_visualizer src/wolf_ros_visualizer.cpp)
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
-add_dependencies(wolf_ros_visualizer ${PROJECT_NAME}_gencfg)
+## add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(wolf_ros_visualizer ${wolf_LIBRARIES})
+target_link_libraries(subscriber_${PROJECT_NAME}
+                      ${catkin_LIBRARIES}
+                      ${CERES_LIBRARIES}
+                      ${wolfcore_LIBRARIES}
+                      yaml-cpp
+                      dl
+                      )
+
+target_link_libraries(publisher_${PROJECT_NAME}
+                      ${catkin_LIBRARIES}
+                      ${CERES_LIBRARIES}
+                      ${wolfcore_LIBRARIES}
+                      yaml-cpp
+                      dl
+                      )
 
 target_link_libraries(${PROJECT_NAME}
-                      wolf_ros_visualizer
                       ${catkin_LIBRARIES}
                       ${CERES_LIBRARIES}
-                      ${wolf_LIBRARIES}
+                      ${wolfcore_LIBRARIES}
                       yaml-cpp
                       dl
                       )

cfg/WolfROS.cfg

-#!/usr/bin/env python
-PACKAGE = "wolf_demo"
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-# gen.add("int_param",    int_t,    0, "An Integer parameter", 50,  0, 100)
-# gen.add("double_param", double_t, 0, "A double parameter",    .5, 0,   1)
-# gen.add("str_param",    str_t,    0, "A string parameter",  "Hello World")
-gen.add("publish_markers_",   bool_t,   0, "Publish KeyFrame markers",  True)
-
-# size_enum = gen.enum([ gen.const("Small",      int_t, 0, "A small constant"),
-#                        gen.const("Medium",     int_t, 1, "A medium constant"),
-#                        gen.const("Large",      int_t, 2, "A large constant"),
-#                        gen.const("ExtraLarge", int_t, 3, "An extra large constant")],
-#                      "An enum to set size")
-
-# gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum)
-
-exit(gen.generate(PACKAGE, "wolf_demo", "wolf_demo"))

include/factory_publisher.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTORY_PUBLISHER_H_
+#define FACTORY_PUBLISHER_H_
+
+// wolf
+#include <core/common/factory.h>
+#include <core/utils/params_server.h>
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+
+// #include "wolf_ros_subscriber.h"
+// std
+
+
+namespace wolf
+{
+/** \brief Processor factory class
+ *
+ * This factory can create objects of classes deriving from ProcessorBase.
+ *
+ * Specific object creation is invoked by create(TYPE, params), and the TYPE of processor is identified with a string.
+ * For example, the following processor types are implemented,
+ *   - "ODOM 3d" for ProcessorOdom3d
+ *   - "ODOM 2d" for ProcessorOdom2d
+ *   - "GPS"     for ProcessorGPS
+ *
+ * The rule to make new TYPE strings unique is that you skip the prefix 'Processor' from your class name,
+ * and you build a string in CAPITALS with space separators.
+ *   - ProcessorImageFeature -> ````"IMAGE"````
+ *   - ProcessorLaser2d -> ````"LASER 2d"````
+ *   - etc.
+ *
+ * The methods to create specific processors are called __creators__.
+ * Creators must be registered to the factory before they can be invoked for processor creation.
+ *
+ * This documentation shows you how to:
+ *   - Access the Factory
+ *   - Register and unregister creators
+ *   - Create processors
+ *   - Write a processor creator for ProcessorOdom2d (example).
+ *
+ * #### Accessing the Factory
+ * The FactoryProcessor class is a singleton: it can only exist once in your application.
+ * To obtain an instance of it, use the static method get(),
+ *
+ *     \code
+ *     FactoryProcessor::get()
+ *     \endcode
+ *
+ * You can then call the methods you like, e.g. to create a processor, you type:
+ *
+ *     \code
+ *     FactoryProcessor::create(...); // see below for creating processors ...
+ *     \endcode
+ *
+ * #### Registering processor creators
+ * Prior to invoking the creation of a processor of a particular type,
+ * you must register the creator for this type into the factory.
+ *
+ * Registering processor creators into the factory is done through registerCreator().
+ * You provide a processor type string (above), and a pointer to a static method
+ * that knows how to create your specific processor, e.g.:
+ *
+ *     \code
+ *     FactoryProcessor::registerCreator("ODOM 2d", ProcessorOdom2d::create);
+ *     \endcode
+ *
+ * The method ProcessorOdom2d::create() exists in the ProcessorOdom2d class as a static method.
+ * All these ProcessorXxx::create() methods need to have exactly the same API, regardless of the processor type.
+ * This API includes a processor name, and a pointer to a base struct of parameters, ParamsProcessorBasePtr,
+ * that can be derived for each derived processor.
+ *
+ * Here is an example of ProcessorOdom2d::create() extracted from processor_odom_2d.h:
+ *
+ *     \code
+ *     static ProcessorBasePtr create(const std::string& _name, ParamsProcessorBasePtr _params)
+ *     {
+ *         // cast _params to good type
+ *         ParamsProcessorOdom2d* params = (ParamsProcessorOdom2d*)_params;
+ *
+ *         ProcessorBasePtr prc = new ProcessorOdom2d(params);
+ *         prc->setName(_name); // pass the name to the created ProcessorOdom2d.
+ *         return prc;
+ *     }
+ *     \endcode
+ *
+ * #### Achieving automatic registration
+ * Currently, registering is performed in each specific ProcessorXxxx source file, processor_xxxx.cpp.
+ * For example, in processor_odom_2d.cpp we find the line:
+ *
+ *     \code
+ *     const bool registered_odom_2d = FactoryProcessor::registerCreator("ODOM 2d", ProcessorOdom2d::create);
+ *     \endcode
+ *
+ * which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2d class).
+ * Therefore, at application level, all processors that have a .cpp file compiled are automatically registered.
+ *
+ * #### Unregister processor creators
+ * The method unregisterCreator() unregisters the ProcessorXxx::create() method.
+ * It only needs to be passed the string of the processor type.
+ *
+ *     \code
+ *     FactoryProcessor::unregisterCreator("ODOM 2d");
+ *     \endcode
+ *
+ * #### Creating processors
+ * Prior to invoking the creation of a processor of a particular type,
+ * you must register the creator for this type into the factory.
+ *
+ * To create a ProcessorOdom2d, you type:
+ *
+ *     \code
+ *     FactoryProcessor::create("ODOM 2d", "main odometry", params_ptr);
+ *     \endcode
+ *
+ * #### Example 1 : using the Factories alone
+ * We provide the necessary steps to create a processor of class ProcessorOdom2d in our application,
+ * and bind it to a SensorOdom2d:
+ *
+ *     \code
+ *     #include "sensor_odom_2d.h"      // provides SensorOdom2d    and FactorySensor
+ *     #include "processor_odom_2d.h"   // provides ProcessorOdom2d and FactoryProcessor
+ *
+ *     // Note: SensorOdom2d::create()    is already registered, automatically.
+ *     // Note: ProcessorOdom2d::create() is already registered, automatically.
+ *
+ *     // First create the sensor (See FactorySensor for details)
+ *     SensorBasePtr sensor_ptr = FactorySensor::create ( "ODOM 2d" , "Main odometer" , extrinsics , &intrinsics );
+ *
+ *     // To create a odometry integrator, provide a type="ODOM 2d", a name="main odometry", and a pointer to the parameters struct:
+ *
+ *     ParamsProcessorOdom2d  params({...});   // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
+ *
+ *     ProcessorBasePtr processor_ptr =
+ *         FactoryProcessor::create ( "ODOM 2d" , "main odometry" , &params );
+ *
+ *     // Bind processor to sensor
+ *     sensor_ptr->addProcessor(processor_ptr);
+ *     \endcode
+ *
+ * #### Example 2: Using the helper API in class Problem
+ * The WOLF uppermost node, Problem, makes the creation of sensors and processors, and the binding between them, even simpler.
+ *
+ * The creation is basically replicating the factories' API. The binding is accomplished by passing the sensor name to the Processor installer.
+ *
+ * The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation),
+ *
+ *     \code
+ *     #include "sensor_odom_2d.h"
+ *     #include "processor_odom_2d.h"
+ *     #include "problem.h"
+ *
+ *     Problem problem(FRM_PO_2d);
+ *     problem.installSensor    ( "ODOM 2d" , "Main odometer" , extrinsics      , &intrinsics );
+ *     problem.installProcessor ( "ODOM 2d" , "Odometry"      , "Main odometer" , &params     );
+ *     \endcode
+ *
+ * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
+ */
+class Publisher;
+typedef Factory<Publisher,
+                const std::string&,
+                const ParamsServer&,
+                const ProblemPtr,
+                ros::NodeHandle&> FactoryPublisher;
+
+template<>
+inline std::string FactoryPublisher::getClass() const
+{
+  return "FactoryPublisher";
+}
+
+#define WOLF_REGISTER_PUBLISHER(PublisherType)                        \
+    namespace{ const bool WOLF_UNUSED PublisherType##Registered =      \
+            wolf::FactoryPublisher::registerCreator(#PublisherType, PublisherType::create); } \
+
+} /* namespace wolf */
+#endif /* FACTORY_PUBLISHER_H_ */

include/subscriber_factory.h → include/factory_subscriber.h

-#ifndef SUBSCRIBER_FACTORY_H_
-#define SUBSCRIBER_FACTORY_H_
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTORY_SUBSCRIBER_H_
+#define FACTORY_SUBSCRIBER_H_
 
 // wolf
 #include <core/common/factory.h>
-#include <core/utils/params_server.hpp>
+#include <core/utils/params_server.h>
 #include <ros/ros.h>
 #include <nav_msgs/Odometry.h>
 
@@ -19,14 +40,14 @@ namespace wolf
  *
  * Specific object creation is invoked by create(TYPE, params), and the TYPE of processor is identified with a string.
  * For example, the following processor types are implemented,
- *   - "ODOM 3D" for ProcessorOdom3D
- *   - "ODOM 2D" for ProcessorOdom2D
+ *   - "ODOM 3d" for ProcessorOdom3d
+ *   - "ODOM 2d" for ProcessorOdom2d
  *   - "GPS"     for ProcessorGPS
  *
  * The rule to make new TYPE strings unique is that you skip the prefix 'Processor' from your class name,
  * and you build a string in CAPITALS with space separators.
  *   - ProcessorImageFeature -> ````"IMAGE"````
- *   - ProcessorLaser2D -> ````"LASER 2D"````
+ *   - ProcessorLaser2d -> ````"LASER 2d"````
  *   - etc.
  *
  * The methods to create specific processors are called __creators__.
@@ -36,20 +57,20 @@ namespace wolf
  *   - Access the Factory
  *   - Register and unregister creators
  *   - Create processors
- *   - Write a processor creator for ProcessorOdom2D (example).
+ *   - Write a processor creator for ProcessorOdom2d (example).
  *
  * #### Accessing the Factory
- * The ProcessorFactory class is a singleton: it can only exist once in your application.
+ * The FactoryProcessor class is a singleton: it can only exist once in your application.
  * To obtain an instance of it, use the static method get(),
  *
  *     \code
- *     ProcessorFactory::get()
+ *     FactoryProcessor::get()
  *     \endcode
  *
  * You can then call the methods you like, e.g. to create a processor, you type:
  *
  *     \code
- *     ProcessorFactory::get().create(...); // see below for creating processors ...
+ *     FactoryProcessor::create(...); // see below for creating processors ...
  *     \endcode
  *
  * #### Registering processor creators
@@ -61,37 +82,37 @@ namespace wolf
  * that knows how to create your specific processor, e.g.:
  *
  *     \code
- *     ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create);
+ *     FactoryProcessor::registerCreator("ODOM 2d", ProcessorOdom2d::create);
  *     \endcode
  *
- * The method ProcessorOdom2D::create() exists in the ProcessorOdom2D class as a static method.
+ * The method ProcessorOdom2d::create() exists in the ProcessorOdom2d class as a static method.
  * All these ProcessorXxx::create() methods need to have exactly the same API, regardless of the processor type.
- * This API includes a processor name, and a pointer to a base struct of parameters, ProcessorParamsBasePtr,
+ * This API includes a processor name, and a pointer to a base struct of parameters, ParamsProcessorBasePtr,
  * that can be derived for each derived processor.
  *
- * Here is an example of ProcessorOdom2D::create() extracted from processor_odom_2D.h:
+ * Here is an example of ProcessorOdom2d::create() extracted from processor_odom_2d.h:
  *
  *     \code
- *     static ProcessorBasePtr create(const std::string& _name, ProcessorParamsBasePtr _params)
+ *     static ProcessorBasePtr create(const std::string& _name, ParamsProcessorBasePtr _params)
  *     {
  *         // cast _params to good type
- *         ProcessorParamsOdom2D* params = (ProcessorParamsOdom2D*)_params;
+ *         ParamsProcessorOdom2d* params = (ParamsProcessorOdom2d*)_params;
  *
- *         ProcessorBasePtr prc = new ProcessorOdom2D(params);
- *         prc->setName(_name); // pass the name to the created ProcessorOdom2D.
+ *         ProcessorBasePtr prc = new ProcessorOdom2d(params);
+ *         prc->setName(_name); // pass the name to the created ProcessorOdom2d.
  *         return prc;
  *     }
  *     \endcode
  *
  * #### Achieving automatic registration
  * Currently, registering is performed in each specific ProcessorXxxx source file, processor_xxxx.cpp.
- * For example, in processor_odom_2D.cpp we find the line:
+ * For example, in processor_odom_2d.cpp we find the line:
  *
  *     \code
- *     const bool registered_odom_2D = ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create);
+ *     const bool registered_odom_2d = FactoryProcessor::registerCreator("ODOM 2d", ProcessorOdom2d::create);
  *     \endcode
  *
- * which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2D class).
+ * which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2d class).
  * Therefore, at application level, all processors that have a .cpp file compiled are automatically registered.
  *
  * #### Unregister processor creators
@@ -99,39 +120,39 @@ namespace wolf
  * It only needs to be passed the string of the processor type.
  *
  *     \code
- *     ProcessorFactory::get().unregisterCreator("ODOM 2D");
+ *     FactoryProcessor::unregisterCreator("ODOM 2d");
  *     \endcode
  *
  * #### Creating processors
  * Prior to invoking the creation of a processor of a particular type,
  * you must register the creator for this type into the factory.
  *
- * To create a ProcessorOdom2D, you type:
+ * To create a ProcessorOdom2d, you type:
  *
  *     \code
- *     ProcessorFactory::get().create("ODOM 2D", "main odometry", params_ptr);
+ *     FactoryProcessor::create("ODOM 2d", "main odometry", params_ptr);
  *     \endcode
  *
  * #### Example 1 : using the Factories alone
- * We provide the necessary steps to create a processor of class ProcessorOdom2D in our application,
- * and bind it to a SensorOdom2D:
+ * We provide the necessary steps to create a processor of class ProcessorOdom2d in our application,
+ * and bind it to a SensorOdom2d:
  *
  *     \code
- *     #include "sensor_odom_2D.h"      // provides SensorOdom2D    and SensorFactory
- *     #include "processor_odom_2D.h"   // provides ProcessorOdom2D and ProcessorFactory
+ *     #include "sensor_odom_2d.h"      // provides SensorOdom2d    and FactorySensor
+ *     #include "processor_odom_2d.h"   // provides ProcessorOdom2d and FactoryProcessor
  *
- *     // Note: SensorOdom2D::create()    is already registered, automatically.
- *     // Note: ProcessorOdom2D::create() is already registered, automatically.
+ *     // Note: SensorOdom2d::create()    is already registered, automatically.
+ *     // Note: ProcessorOdom2d::create() is already registered, automatically.
  *
- *     // First create the sensor (See SensorFactory for details)
- *     SensorBasePtr sensor_ptr = SensorFactory::get().create ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics );
+ *     // First create the sensor (See FactorySensor for details)
+ *     SensorBasePtr sensor_ptr = FactorySensor::create ( "ODOM 2d" , "Main odometer" , extrinsics , &intrinsics );
  *
- *     // To create a odometry integrator, provide a type="ODOM 2D", a name="main odometry", and a pointer to the parameters struct:
+ *     // To create a odometry integrator, provide a type="ODOM 2d", a name="main odometry", and a pointer to the parameters struct:
  *
- *     ProcessorParamsOdom2D  params({...});   // fill in the derived struct (note: ProcessorOdom2D actually has no input params)
+ *     ParamsProcessorOdom2d  params({...});   // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
  *
  *     ProcessorBasePtr processor_ptr =
- *         ProcessorFactory::get().create ( "ODOM 2D" , "main odometry" , &params );
+ *         FactoryProcessor::create ( "ODOM 2d" , "main odometry" , &params );
  *
  *     // Bind processor to sensor
  *     sensor_ptr->addProcessor(processor_ptr);
@@ -145,32 +166,33 @@ namespace wolf
  * The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation),
  *
  *     \code
- *     #include "sensor_odom_2D.h"
- *     #include "processor_odom_2D.h"
+ *     #include "sensor_odom_2d.h"
+ *     #include "processor_odom_2d.h"
  *     #include "problem.h"
  *
- *     Problem problem(FRM_PO_2D);
- *     problem.installSensor    ( "ODOM 2D" , "Main odometer" , extrinsics      , &intrinsics );
- *     problem.installProcessor ( "ODOM 2D" , "Odometry"      , "Main odometer" , &params     );
+ *     Problem problem(FRM_PO_2d);
+ *     problem.installSensor    ( "ODOM 2d" , "Main odometer" , extrinsics      , &intrinsics );
+ *     problem.installProcessor ( "ODOM 2d" , "Odometry"      , "Main odometer" , &params     );
  *     \endcode
  *
  * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
  */
-    class WolfSubscriberWrapper;
-    typedef Factory<WolfSubscriberWrapper,
+    class Subscriber;
+    typedef Factory<Subscriber,
                     const std::string&,
                     const ParamsServer&,
-                    const SensorBasePtr> SubscriberFactory;
+                    const SensorBasePtr,
+                    ros::NodeHandle&> FactorySubscriber;
 template<>
-inline std::string SubscriberFactory::getClass()
+inline std::string FactorySubscriber::getClass() const
 {
-  return "SubscriberFactory";
+  return "FactorySubscriber";
 }
 
 
 #define WOLF_REGISTER_SUBSCRIBER(SubscriberType)                        \
     namespace{ const bool WOLF_UNUSED SubscriberType##Registered =      \
-            wolf::SubscriberFactory::get().registerCreator(#SubscriberType, SubscriberType::create); } \
+            wolf::FactorySubscriber::registerCreator(#SubscriberType, SubscriberType::create); } \
 
 } /* namespace wolf */
-#endif /* SUBSCRIBER_FACTORY_H_ */
+#endif /* FACTORY_SUBSCRIBER_H_ */

include/wolf_ros_node.h → include/node.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
 /**************************
  *      WOLF includes     *
  **************************/
-#include "core/common/node_base.h"
-#include "core/common/wolf.h"
-#include <core/capture/capture_odom_2D.h>
-#include <core/sensor/sensor_odom_2D.h>
-#include <core/processor/processor_odom_2D.h>
+#include <core/common/node_base.h>
+#include <core/common/wolf.h>
+#include <core/capture/capture_odom_2d.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/sensor/sensor_odom_2d.h>
+#include <core/processor/processor_odom_2d.h>
 #include <core/problem/problem.h>
-#include <core/utils/loader.hpp>
-#include <core/yaml/parser_yaml.hpp>
+#include <core/utils/loader.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/solver/factory_solver.h>
 
 
 /**************************
  *     CERES includes     *
  **************************/
-#include <core/ceres_wrapper/ceres_manager.h>
-//#include "glog/logging.h"
-
-/**************************
- *      ROS includes      *
- **************************/
 #include <ros/ros.h>
 #include <ros/package.h>
 #include <nav_msgs/Odometry.h>
@@ -37,11 +54,11 @@
 #include <iomanip>
 #include <queue>
 #include <memory>
+#include <fstream>
+#include <string>
 
-#include "subscriber_factory.h"
-#include "wolf_ros_subscriber.h"
-#include "wolf_ros_visualizer.h"
-//#include "wolf_ros_scan_visualizer.h"
+#include "subscriber.h"
+#include "publisher.h"
 
 using namespace wolf;
 using namespace std;
@@ -55,33 +72,41 @@ class WolfRosNode
         // ROS node handle
         ros::NodeHandle nh_;
 
+        // subscribers
+        std::vector<SubscriberPtr> subscribers_;
+        // publishers
+        std::vector<PublisherPtr> publishers_;
+
 
     protected:
         // solver
-        CeresManagerPtr ceres_manager_ptr_;
+        SolverManagerPtr solver_;
+        ros::Time last_cov_stamp_;
 
-        // visualizer
-        std::shared_ptr<WolfRosVisualizer> wolf_viz_;
+        // profiling
+        bool profiling_;
+        std::ofstream profiling_file_;
+        std::chrono::time_point<std::chrono::high_resolution_clock> start_experiment_;
 
-        // subscribers
-        std::vector<WolfSubscriberWrapperPtr> subscribers_;
-
-        // transforms
-        tf::TransformBroadcaster tfb_;
-        tf::TransformListener    tfl_;
-        std::string base_frame_id_, map_frame_id_, odom_frame_id_;
-        tf::Transform T_map2odom;
+        // print
+        bool print_problem_;
+        double print_period_;
+        ros::Time last_print_;
+        int print_depth_;
+        bool print_constr_by_, print_metric_, print_state_blocks_;
 
       public:
+
+        double node_rate_;
+
         WolfRosNode();
 
         virtual ~WolfRosNode(){};
 
         void solve();
+        void solveLoop();
 
-        void broadcastTf();
-
-        void visualize();
+        void print();
 
-        void updateTf();
+        void createProfilingFile();
 };

include/publisher.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+#ifndef WOLF_PUBLISHER_H
+#define WOLF_PUBLISHER_H
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include "factory_publisher.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(Publisher);
+
+/*
+ * Macro for defining Autoconf publisher creator for WOLF's high level API.
+ *
+ * Place a call to this macro inside your class declaration (in the publisher_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived publisher class, PublisherClass,
+ * must have a constructor available with the API:
+ *
+ *   PublisherClass(const std::string& _unique_name,
+ *                  const ParamsServer& _server,
+ *                  const ProblemPtr _problem);
+ */
+#define WOLF_PUBLISHER_CREATE(PublisherClass)                                               \
+        static PublisherPtr create(const std::string& _unique_name,                         \
+                                   const ParamsServer& _server,                             \
+                                   const ProblemPtr _problem,                               \
+                                   ros::NodeHandle& _nh)                                    \
+                                   {                                                        \
+    PublisherPtr pub = std::make_shared<PublisherClass>(_unique_name, _server, _problem);   \
+    pub->initialize(_nh, pub->getTopic());                                                  \
+    return pub;                                                                             \
+}                                                                                           \
+
+class Publisher
+{
+    public:
+
+        Publisher(const std::string& _unique_name,
+                  const ParamsServer& _server,
+                  const ProblemPtr _problem) :
+                      problem_(_problem),
+                      first_publish_time_(ros::Time(0)),
+                      last_n_period_(0),
+                      name_(_unique_name),
+                      prefix_("ROS publisher/" + _unique_name),
+                      n_publish_(0),
+                      acc_duration_(0),
+                      max_duration_(0)
+        {
+            period_ = _server.getParam<double>(prefix_ + "/period");
+            topic_  = _server.getParam<std::string>(prefix_ + "/topic");
+        };
+
+        virtual ~Publisher(){};
+
+        virtual void run() final;
+        virtual void stop() final;
+        virtual void loop() final;
+
+        virtual void initialize(ros::NodeHandle& nh, const std::string& topic) = 0;
+
+        virtual void publish() final;
+
+        virtual void publishDerived() = 0;
+
+        virtual bool ready();
+
+        std::string getTopic() const;
+
+    protected:
+
+        template<typename T>
+        T getParamWithDefault(const ParamsServer &_server,
+                   const std::string &_param_name,
+                   const T _default_value) const;
+
+        ProblemPtr problem_;
+        ros::Publisher publisher_;
+        double period_;
+        ros::Time first_publish_time_;
+        long unsigned int last_n_period_;
+        std::string name_;
+        std::string prefix_;
+        std::string topic_;
+
+        std::thread pub_thread_;
+
+        // PROFILING
+        unsigned int n_publish_;
+        std::chrono::microseconds acc_duration_;
+        std::chrono::microseconds max_duration_;
+
+    public:
+        void printProfiling(std::ostream& stream = std::cout) const;
+};
+
+inline std::string Publisher::getTopic() const
+{
+    return topic_;
+}
+
+inline void Publisher::publish()
+{
+    if (last_n_period_ == 0)
+        first_publish_time_ = ros::Time::now();
+
+    auto start = std::chrono::high_resolution_clock::now();
+
+    publishDerived();
+
+    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start);
+    acc_duration_+= duration;
+    max_duration_ = std::max(max_duration_,duration);
+    n_publish_++;
+}
+
+inline bool Publisher::ready()
+{
+    return true;
+}
+
+inline void Publisher::printProfiling(std::ostream &_stream) const
+{
+    _stream << "\n" << name_ << ":"
+            << "\n\ttotal time:           " << 1e-6 * acc_duration_.count() << " s"
+            << "\n\texecutions:           " << n_publish_
+            << "\n\taverage time:         " << 1e-3 * acc_duration_.count() / n_publish_ << " ms"
+            << "\n\tmax time:             " << 1e-3 * max_duration_.count() << " ms" << std::endl;
+}
+
+inline void Publisher::run()
+{
+    pub_thread_ = std::thread(&Publisher::loop, this);
+}
+
+inline void Publisher::stop()
+{
+    pub_thread_.join();
+}
+
+inline void Publisher::loop()
+{
+    auto awake_time = std::chrono::system_clock::now();
+    auto period = std::chrono::duration<int,std::milli>((int)(period_*1e3));
+
+    WOLF_DEBUG("Started publisher ", name_, " loop");
+
+    while (ros::ok())
+    {
+        if (ready())
+            publish();
+
+        // relax to fit output rate
+        awake_time += period;
+        std::this_thread::sleep_until(awake_time);
+    }
+    WOLF_DEBUG("Publisher ", name_, " loop finished");
+}
+
+template<typename T>
+inline T Publisher::getParamWithDefault(const ParamsServer &_server,
+                                        const std::string &_param_name,
+                                        const T _default_value) const
+{
+    try
+    {
+        return _server.getParam<T>(_param_name);
+    }
+    catch (...)
+    {
+        WOLF_INFO("Publisher: Parameter ", _param_name, " is missing. Taking default value: ", _default_value);
+        return _default_value;
+    }
+}
+
+}  // namespace wolf
+#endif

include/publisher_graph.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+#ifndef PUBLISHER_GRAPH_H
+#define PUBLISHER_GRAPH_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+namespace wolf
+{
+
+class PublisherGraph: public Publisher
+{
+    public:
+        PublisherGraph(const std::string& _unique_name,
+                      const ParamsServer& _server,
+                      const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherGraph);
+
+        virtual ~PublisherGraph(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        void publishDerived() override;
+
+    protected:
+
+        void publishLandmarks();
+        void publishFactors();
+        void publishTrajectory();
+
+        virtual bool fillLandmarkMarkers(LandmarkBaseConstPtr        lmk,
+                                         visualization_msgs::Marker& lmk_marker,
+                                         visualization_msgs::Marker& lmk_text_marker);
+        virtual bool fillFactorMarker(FactorBaseConstPtr          fac,
+                                      visualization_msgs::Marker& fac_marker,
+                                      visualization_msgs::Marker& fac_text_marker);
+        virtual bool fillFrameMarker(FrameBaseConstPtr           frm,
+                                     visualization_msgs::Marker& frm_marker,
+                                     visualization_msgs::Marker& frm_text_marker);
+
+        std::string factorString(FactorBaseConstPtr fac) const;
+
+        // publishers
+        ros::Publisher landmarks_publisher_;
+        ros::Publisher trajectory_publisher_;
+        ros::Publisher factors_publisher_;
+
+        // Marker arrayss
+        visualization_msgs::MarkerArray landmarks_marker_array_;
+        visualization_msgs::MarkerArray trajectory_marker_array_;
+        visualization_msgs::MarkerArray factors_marker_array_;
+
+        // Markers
+        visualization_msgs::Marker landmark_marker_, landmark_text_marker_;
+        visualization_msgs::Marker frame_marker_, frame_text_marker_;
+        visualization_msgs::Marker factor_marker_, factor_text_marker_;
+
+        // Options
+        std::string map_frame_id_;
+        bool        viz_overlapped_factors_, viz_inactive_factors_;
+        double      viz_scale_, text_scale_, factors_width_, factors_absolute_height_, landmark_text_z_offset_, landmark_width_, landmark_length_, frame_width_, frame_length_, frame_vel_scale_;
+        std_msgs::ColorRGBA frame_vel_color_, factor_abs_color_, factor_motion_color_, factor_loop_color_, factor_lmk_color_, factor_geom_color_, factor_other_color_;
+
+        // auxiliar variables
+        unsigned int landmark_max_hits_;
+        ros::Time    last_markers_publish_;
+        std::set<std::string> factors_drawn_;
+
+
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherGraph)
+
+}
+
+#endif

include/publisher_pose.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+#ifndef PUBLISHER_POSE_H
+#define PUBLISHER_POSE_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <tf/transform_listener.h>
+
+namespace wolf
+{
+
+class PublisherPose: public Publisher
+{
+        bool extrinsics_;
+        int max_points_;
+        double line_size_;
+
+        geometry_msgs::PoseArray pose_array_msg_;
+        visualization_msgs::Marker marker_msg_;
+        geometry_msgs::PoseWithCovarianceStamped pose_with_cov_msg_;
+        std_msgs::ColorRGBA marker_color_;
+        SensorBasePtr sensor_;
+        std::string frame_id_, map_frame_id_;
+
+        ros::Publisher pub_pose_array_, pub_marker_, pub_pose_with_cov_;
+
+    public:
+        PublisherPose(const std::string& _unique_name,
+                      const ParamsServer& _server,
+                      const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherPose);
+
+        virtual ~PublisherPose(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        void publishDerived() override;
+
+        void publishPose();
+
+    protected:
+
+        bool listenTf();
+        Eigen::Quaterniond q_frame_;
+        Eigen::Vector3d t_frame_;
+        tf::TransformListener tfl_;
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherPose)
+}
+
+#endif

include/publisher_state_block.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+#ifndef PUBLISHER_STATE_BLOCK_H
+#define PUBLISHER_STATE_BLOCK_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <std_msgs/Float64MultiArray.h>
+
+namespace wolf
+{
+
+class PublisherStateBlock: public Publisher
+{
+    protected:
+        std_msgs::Float64MultiArray state_msg_;
+        SensorBasePtr sensor_;
+        char key_;
+        bool msg_init_;
+
+    public:
+        PublisherStateBlock(const std::string& _unique_name,
+                      const ParamsServer& _server,
+                      const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherStateBlock);
+
+        virtual ~PublisherStateBlock(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        void publishDerived() override;
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherStateBlock)
+}
+
+#endif

include/publisher_tf.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+#ifndef PUBLISHER_TF_H
+#define PUBLISHER_TF_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+
+namespace wolf
+{
+
+tf::Transform stateToTfTransform(const VectorComposite& state, const int& dim)
+{
+    assert(state.includesStructure("PO"));
+
+    // 2D
+    if (dim == 2)
+    {
+        return tf::Transform (tf::createQuaternionFromYaw(state.at('O')(0)),
+                              tf::Vector3(state.at('P')(0), state.at('P')(1), 0) );
+    }
+    // 3D
+    else
+    {
+        return tf::Transform (tf::Quaternion(state.at('O')(0), state.at('O')(1), state.at('O')(2), state.at('O')(3)),
+                              tf::Vector3(state.at('P')(0), state.at('P')(1), state.at('P')(2)) );
+    }
+}
+
+class PublisherTf: public Publisher
+{
+    protected:
+        std::string base_frame_id_, odom_frame_id_, map_frame_id_;
+        tf::TransformBroadcaster tfb_;
+        tf2_ros::StaticTransformBroadcaster stfb_;
+
+        tf::TransformListener tfl_;
+
+        tf::StampedTransform T_odom2base_;//, T_map2odom_;
+        geometry_msgs::TransformStamped Tmsg_map2odom_;
+
+        bool publish_odom_tf_;
+        bool state_available_; // used to not repeat warnings regarding availability of state
+
+    public:
+        PublisherTf(const std::string& _unique_name,
+                      const ParamsServer& _server,
+                      const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherTf);
+
+        virtual ~PublisherTf(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        void publishDerived() override;
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherTf)
+}
+
+#endif

include/publisher_trajectory.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+/*
+ * publisher_trajectory.h
+ *
+ *  Created on: Feb 03, 2022
+ *      Author: igeer
+ */
+//--------LICENSE_END--------
+#ifndef PUBLISHER_TRAJECTORY_H
+#define PUBLISHER_TRAJECTORY_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <nav_msgs/Path.h>
+
+namespace wolf
+{
+
+class PublisherTrajectory: public Publisher
+{
+        nav_msgs::Path path_msg_;
+        
+        std::string frame_id_;
+
+    public:
+        PublisherTrajectory(const std::string& _unique_name,
+                      const ParamsServer& _server,
+                      const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherTrajectory);
+
+        virtual ~PublisherTrajectory(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        void publishDerived() override;
+
+        void publishTrajectory();
+
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherTrajectory)
+}
+
+#endif

include/subscriber.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+#ifndef WOLF_SUBSCRIBER_H_
+#define WOLF_SUBSCRIBER_H_
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <core/sensor/sensor_base.h>
+#include "factory_subscriber.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+
+namespace wolf {
+WOLF_PTR_TYPEDEFS(Subscriber);
+
+/*
+ * Macro for defining Autoconf subscriber creator for WOLF's high level API.
+ *
+ * Place a call to this macro inside your class declaration (in the subscriber_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived subscriber class, SubscriberClass,
+ * must have a constructor available with the API:
+ *
+ *   SubscriberClass(const std::string& _unique_name,
+ *                   const ParamsServer& _server,
+ *                   const SensorBasePtr _sensor_ptr);
+ */
+#define WOLF_SUBSCRIBER_CREATE(SubscriberClass)                                                 \
+static SubscriberPtr create(const std::string& _unique_name,                                    \
+                            const ParamsServer& _server,                                        \
+                            const SensorBasePtr _sensor_ptr,                                    \
+                            ros::NodeHandle& _nh)                                               \
+{                                                                                               \
+    SubscriberPtr sub = std::make_shared<SubscriberClass>(_unique_name, _server, _sensor_ptr);  \
+    sub->initialize(_nh, sub->getTopic());                                                      \
+    return sub;                                                                                 \
+}                                                                                               \
+
+class Subscriber
+{
+    protected:
+        //wolf
+        SensorBasePtr sensor_ptr_;
+        std::string prefix_;
+        std::string name_;
+        std::string topic_;
+
+        // ros
+        ros::Subscriber sub_;
+        ros::Time last_stamp_;
+        int last_seq_;
+
+    public:
+        Subscriber(const std::string& _unique_name,
+                   const ParamsServer& _server,
+                   const SensorBasePtr _sensor_ptr) :
+            sensor_ptr_(_sensor_ptr),
+            prefix_("ROS subscriber/" + _unique_name),
+            name_(_unique_name),
+            last_stamp_(0),
+            last_seq_(-1)
+        {
+            topic_  = _server.getParam<std::string>(prefix_ + "/topic");
+        }
+
+        virtual ~Subscriber(){};
+
+        virtual void initialize(ros::NodeHandle& nh, const std::string& topic) = 0;
+
+        std::string getTopic() const;
+
+        std::string getName() const;
+
+        ros::Time getLastStamp() const;
+
+        virtual double secondsSinceLastCallback();
+
+    protected:
+
+        void updateLastHeader(const std_msgs::Header& _header);
+
+        template<typename T>
+        T getParamWithDefault(const ParamsServer &_server,
+                              const std::string &_param_name,
+                              const T _default_value) const;
+};
+
+inline std::string Subscriber::getTopic() const
+{
+    return topic_;
+}
+
+inline std::string Subscriber::getName() const
+{
+    return name_;
+}
+
+inline ros::Time Subscriber::getLastStamp() const
+{
+    return last_stamp_;
+}
+
+inline double Subscriber::secondsSinceLastCallback()
+{
+    if (last_stamp_ == ros::Time(0))
+    {
+        WOLF_WARN("Subscriber: 'last_stamp_` not initialized. No messages have been received or ", name_, " is not updating headers (be sure to add 'updateLastHeader(msg->header)' in your subscriber callback).");
+        return 0;
+    }
+    return (ros::Time::now() - last_stamp_).toSec();
+}
+
+inline void Subscriber::updateLastHeader(const std_msgs::Header& _header)
+{
+    // stamp
+    if ((ros::Time::now() - _header.stamp).toSec() > 1) // in case use_sim_time == false
+        last_stamp_ = ros::Time::now();
+    else
+        last_stamp_ = _header.stamp;
+
+    // seq
+    if (last_seq_ >= 0 and _header.seq - last_seq_ > 1)
+        ROS_ERROR("Subscriber %s lost %i messages!", name_.c_str(), _header.seq - last_seq_ - 1);
+    last_seq_ = _header.seq;
+}
+
+template<typename T>
+inline T Subscriber::getParamWithDefault(const ParamsServer &_server,
+                                         const std::string &_param_name,
+                                         const T _default_value) const
+{
+    try
+    {
+        return _server.getParam<T>(_param_name);
+    }
+    catch (...)
+    {
+        WOLF_INFO("Subscriber: Parameter ", _param_name, " is missing. Taking default value: ", _default_value);
+        return _default_value;
+    }
+}
+
+}
+#endif

include/subscriber_diffdrive.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <core/common/wolf.h>
+#include "subscriber.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include "sensor_msgs/JointState.h"
+
+namespace wolf
+{
+class SubscriberDiffdrive : public Subscriber
+{
+   protected:
+        ros::Time last_odom_stamp_;
+        Eigen::Vector2d last_angles_;
+        int last_odom_seq_;
+        int last_kf = -1;
+        double ticks_cov_factor_;
+
+   public:
+
+    SubscriberDiffdrive(const std::string& _unique_name,
+                        const ParamsServer& _server,
+                        const SensorBasePtr _sensor_ptr);
+    WOLF_SUBSCRIBER_CREATE(SubscriberDiffdrive);
+
+    virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
+
+    void callback(const sensor_msgs::JointState::ConstPtr& msg);
+};
+
+WOLF_REGISTER_SUBSCRIBER(SubscriberDiffdrive)
+}  // namespace wolf

include/subscriber_odom2d.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.
+//
+//--------LICENSE_END--------
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <core/common/wolf.h>
+#include <core/utils/params_server.h>
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+
+#include "subscriber.h"
+
+namespace wolf
+{
+class SubscriberOdom2d : public Subscriber
+{
+   protected:
+      ros::Time last_odom_stamp_;
+      SensorOdom2dPtr sensor_odom_;
+
+   public:
+
+    SubscriberOdom2d(const std::string& _unique_name,
+                     const ParamsServer& _server,
+                     const SensorBasePtr _sensor_ptr);
+    WOLF_SUBSCRIBER_CREATE(SubscriberOdom2d);
+
+    virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
+
+    void callback(const nav_msgs::Odometry::ConstPtr& msg);
+};
+
+WOLF_REGISTER_SUBSCRIBER(SubscriberOdom2d)
+}  // namespace wolf

include/wolf_ros_subscriber.h

-#ifndef WOLF_ROS_SUBSCRIBER_H_
-#define WOLF_ROS_SUBSCRIBER_H_
-
-/**************************
- *      WOLF includes     *
- **************************/
-#include <core/sensor/sensor_base.h>
-
-/**************************
- *      ROS includes      *
- **************************/
-#include <ros/ros.h>
-#include <nav_msgs/Odometry.h>
-
-namespace wolf {
-WOLF_PTR_TYPEDEFS(WolfSubscriberWrapper);
-
-class WolfSubscriberWrapper
-{
-    protected:
-        //wolf
-        SensorBasePtr sensor_ptr_;
-
-        // ros
-        ros::Subscriber sub_;
-
-    public:
-        WolfSubscriberWrapper(const SensorBasePtr& sensor_ptr) :
-            sensor_ptr_(sensor_ptr)
-        {
-        }
-        virtual ~WolfSubscriberWrapper(){};
-
-        virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic) = 0;
-};
-}
-#endif

include/wolf_ros_visualizer.h

-/**************************
- *      ROS includes      *
- **************************/
-#include <ros/ros.h>
-#include <visualization_msgs/Marker.h>
-#include <visualization_msgs/MarkerArray.h>
-#include <tf/transform_datatypes.h>
-
-/**************************
- *      WOLF includes     *
- **************************/
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-
-using namespace wolf;
-
-class WolfRosVisualizer
-{
-    public:
-        bool publish_markers_;
-
-        WolfRosVisualizer();
-
-        void initialize(ros::NodeHandle& nh);
-
-        virtual ~WolfRosVisualizer(){};
-
-        void visualize(const ProblemPtr problem);
-
-    private:
-
-        void publishLandmarks(const ProblemPtr problem);
-        void publishFactors(const ProblemPtr problem);
-        void publishTrajectory(const ProblemPtr problem);
-
-        void fillLandmarkMarkers(LandmarkBaseConstPtr lmk,
-                                 visualization_msgs::Marker& lmk_marker,
-                                 visualization_msgs::Marker& lmk_text_marker);
-        void fillFactorMarker(FactorBaseConstPtr fac,
-                              visualization_msgs::Marker& fac_marker);
-        void fillFrameMarker(FrameBaseConstPtr frm,
-                             visualization_msgs::Marker& frm_marker);
-
-
-        // publishers
-        ros::Publisher landmarks_publisher_;
-        ros::Publisher trajectory_publisher_;
-        ros::Publisher factors_publisher_;
-
-        // Marker arrayss
-        visualization_msgs::MarkerArray landmarks_marker_array_;
-        visualization_msgs::MarkerArray trajectory_marker_array_;
-        visualization_msgs::MarkerArray factors_marker_array_;
-
-        // Markers
-        visualization_msgs::Marker landmark_marker_, landmark_text_marker_;
-        visualization_msgs::Marker frame_marker_, frame_text_marker_;
-        visualization_msgs::Marker factor_marker_;
-
-        // Options
-        std::string map_frame_id_;
-        bool viz_factors_, viz_landmarks_, viz_trajectory_;
-        double factors_width_, landmark_text_z_offset_, landmark_width_, landmark_length_, frame_width_, frame_length_;
-        std_msgs::ColorRGBA color_active_, color_inactive_;
-
-        // auxiliar variables
-        unsigned int landmark_max_hits_;
-        double viz_period_;
-        ros::Time last_markers_publish_;
-};

license_header_2022.txt

+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF 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/>.