Merge branch 'devel' into 'main'

After cmake and const refactor

See merge request !5

.gitlab-ci.yml

@@ -4,6 +4,14 @@ stages:
   - demos
   
 ############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+  # Print variables
+  - echo $WOLF_CORE_BRANCH
+  - echo $WOLF_IMU_BRANCH
+  - echo $WOLF_ROS_NODE_BRANCH
+  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_ROS_IMU_BRANCH
+
 .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)
@@ -107,17 +115,13 @@ stages:
 .clone_wolfrosnode_template: &clone_wolfrosnode_definition
   - roscd
   - cd ../src
-  - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git
-  - cd wolf_ros_node
-  - git checkout $WOLF_ROS_CORE_BRANCH
+  - git clone -b $WOLF_ROS_NODE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git
 
 .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_imu
-  - git checkout $CI_COMMIT_BRANCH
-  - cd ../..
+  - git clone -b $CI_COMMIT_BRANCH ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git
+  - cd ..
   - catkin_make
 
 ############ LICENSE HEADERS ############
@@ -125,9 +129,8 @@ license_headers:
   stage: license
   image: labrobotica/wolf_deps_ros:20.04
   cache: []
-  except:
-    - master
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
   script:
@@ -144,9 +147,8 @@ build_and_test:bionic:
     - key: imu-bionic
       paths:
       - ci_deps/imu/
-  except:
-    - master
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_wolfimu_definition
@@ -166,9 +168,8 @@ build_and_test:focal:
     - key: imu-focal
       paths:
       - ci_deps/imu/
-  except:
-    - master
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_wolfimu_definition
@@ -183,7 +184,7 @@ demo_imu:
   variables:
     WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
     WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
-    WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH
+    WOLF_ROS_NODE_BRANCH: $WOLF_ROS_NODE_BRANCH
     WOLF_ROS_IMU_BRANCH: $CI_COMMIT_BRANCH
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_imu2d

CMakeLists.txt

-cmake_minimum_required(VERSION 2.8.3)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 project(wolf_ros_imu)
 
 ## Compile as C++14
-add_compile_options(-std=c++14)
+add_compile_options(-std=c++14) 
+# -fPIC and -rdynamic ensure unique singleton instance across shared libraries (for factories) see: https://stackoverflow.com/a/8626922
+SET(CMAKE_CXX_FLAGS "-fPIC -rdynamic")
 
-# SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/wolf_ros_wrapper/cmake_modules")
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -18,100 +19,14 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 ## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-# find_package(Ceres REQUIRED)
-# find_package(Eigen3 REQUIRED)
 find_package(wolfcore REQUIRED)
 find_package(wolfimu REQUIRED)
 
-## 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
-#)
-
-################################################
-## 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/WolfROS.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
+# produces the XXXConfig.cmake file among other things
 catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES wolf_ros1
-#  CATKIN_DEPENDS roscpp sensor_msgs std_msgs
-#  DEPENDS system_lib
 )
 
 ###########
@@ -120,104 +35,28 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-message("Wolf include path: ${wolfcore_INCLUDE_DIRS}")
 include_directories(
-    include
-  ${EIGEN_INCLUDE_DIRS}
-  ${wolfcore_INCLUDE_DIRS}
-  ${wolfimu_INCLUDE_DIRS}
+  include
   ${catkin_INCLUDE_DIRS}
-  ${CERES_INCLUDE_DIRS}
 )
-# link_directories(/usr/local/lib/iri-algorithms)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/wolf_ros.cpp
-# )
+
 add_library(subscriber_${PROJECT_NAME}
   src/subscriber_compass.cpp
   src/subscriber_imu.cpp
   src/subscriber_imu_enableable.cpp)
+
 add_library(publisher_${PROJECT_NAME}
   src/publisher_imu_bias.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}_node ${PROJECT_NAME}_gencfg)
-# add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
-
-## 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
 
-## 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"
-# set_target_properties(${PROJECT_NAME} 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}_gencfg)
-#add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
-
-## Specify libraries to link a library or executable target against
 target_link_libraries(subscriber_${PROJECT_NAME}
-            ${wolfcore_LIBRARIES}
-            ${wolfimu_LIBRARIES}
+            wolfcore
+            wolfimu
             ${sensor_msgs_LIBRARIES}
             )
+
 target_link_libraries(publisher_${PROJECT_NAME}
-            ${wolfcore_LIBRARIES}
-            ${wolfimu_LIBRARIES}
+            wolfcore
+            wolfimu
             ${sensor_msgs_LIBRARIES}
-            )
-
+            )
\ No newline at end of file
-#############
-## 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
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_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_wolf_ros.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)

README.md

-# wolf_ros_imu
+# WOLF ROS IMU Package
 
+For installation guide and code documentation, please visit the [documentation website](http://www.iri.upc.edu/wolf).

include/publisher_imu_bias.h

@@ -37,12 +37,12 @@ class PublisherImuBias: public Publisher
 {
         std::string map_frame_id_;
         ros::Publisher pub_accel_, pub_gyro_;
-        SensorImuPtr sensor_imu_;
+        SensorImuConstPtr sensor_imu_;
 
     public:
         PublisherImuBias(const std::string& _unique_name,
-                      const ParamsServer& _server,
-                      const ProblemPtr _problem);
+                         const ParamsServer& _server,
+                         ProblemConstPtr _problem);
         WOLF_PUBLISHER_CREATE(PublisherImuBias);
 
         virtual ~PublisherImuBias(){};

package.xml

@@ -15,39 +15,6 @@
   <!--   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/wolf_ros1</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>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>

src/publisher_imu_bias.cpp

@@ -29,10 +29,10 @@ namespace wolf
 
 PublisherImuBias::PublisherImuBias(const std::string& _unique_name,
                                    const ParamsServer& _server,
-                                   const ProblemPtr _problem) :
+                                   ProblemConstPtr _problem) :
         Publisher(_unique_name, _server, _problem)
 {
-    sensor_imu_ = std::static_pointer_cast<SensorImu>(_problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
+    sensor_imu_ = std::static_pointer_cast<const SensorImu>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
 }
 
 void PublisherImuBias::initialize(ros::NodeHandle& nh, const std::string& topic)

src/publisher_map_grid_2d_gravity.cpp

@@ -33,7 +33,7 @@ PublisherMapGrid2dGravity::PublisherMapGrid2dGravity(const std::string& _unique_
                              const ProblemPtr _problem) :
         Publisher(_unique_name, _server, _problem)
 {
-    sensor_imu_ = std::static_pointer_cast<SensorImu>(_problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
+    sensor_imu_ = std::static_pointer_cast<SensorImu>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
 }
 
 void PublisherMapGrid2dGravity::initialize(ros::NodeHandle& nh, const std::string& topic)

src/subscriber_imu.cpp

@@ -19,15 +19,15 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-#include "../include/subscriber_imu.h"
-#include "imu/capture/capture_imu.h"
-#include "imu/sensor/sensor_imu.h"
-#include "imu/sensor/sensor_imu2d.h"
+#include "subscriber_imu.h"
+#include <imu/capture/capture_imu.h>
+#include <imu/sensor/sensor_imu.h>
+#include <imu/sensor/sensor_imu2d.h>
 #include <geometry_msgs/Vector3.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PoseStamped.h>
-#include "core/math/SE3.h"
-#include "core/math/SE2.h"
+#include <core/math/SE3.h>
+#include <core/math/SE2.h>
 
 using namespace wolf;
 
@@ -66,9 +66,18 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name,
         imu_x_axis_ == imu_y_axis_ or
         imu_x_axis_ == imu_z_axis_ or
         imu_y_axis_ == imu_z_axis_)
-        //TODO: add check right-handed axes
     {
-        throw(std::runtime_error("SubscriberImu: parameters 'imu_x_axis'm, 'imu_y_axis' and 'imu_z_axis' wrongly specified. Should be +/-1, +/-2 or +/-3 without repetitions."));
+        throw(std::runtime_error("SubscriberImu: parameters 'imu_x_axis', 'imu_y_axis' and 'imu_z_axis' wrongly specified. Should be +/-1, +/-2 or +/-3 without repetitions."));
+    }
+    // check for right-handed system
+    Matrix3d R;
+    R.setZero();
+    R(0,imu_x_axis_) = (imu_x_neg_ ? -1 : 1);
+    R(1,imu_y_axis_) = (imu_y_neg_ ? -1 : 1);
+    R(2,imu_z_axis_) = (imu_z_neg_ ? -1 : 1);
+    if (R.determinant() < 0)
+    {
+        throw(std::runtime_error("SubscriberImu: parameters 'imu_x_axis', 'imu_y_axis' and 'imu_z_axis' define a left-handed reference system!"));
     }
 }
 

src/subscriber_imu_enableable.cpp

@@ -155,11 +155,11 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
                         Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
                         if(sensor_ptr_->getProblem()->getDim() == 3) 
                           zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
-                        for (auto frm_pair =  sensor_ptr_->getProblem()->getTrajectory()->begin();
-                             frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
-                             frm_pair++)
+                        
+                        auto frame_map = sensor_ptr_->getProblem()->getTrajectory()->getFrameMap();
+                        for (auto frm_pair : frame_map)
                         {
-                            if (frm_pair->second == last_frame_)
+                            if (frm_pair.second == last_frame_)
                                 break;
                             auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
                             auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
@@ -169,14 +169,14 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
                             if(sensor_ptr_->getProblem()->getDim() == 3) 
                               FactorBase::emplace<FactorRelativePose3d>(feature_zero,
                                                                         feature_zero,
-                                                                        frm_pair->second,
+                                                                        frm_pair.second,
                                                                         nullptr,
                                                                         false,
                                                                         TOP_MOTION);
                             else
                               FactorBase::emplace<FactorRelativePose2d>(feature_zero,
                                                                         feature_zero,
-                                                                        frm_pair->second,
+                                                                        frm_pair.second,
                                                                         nullptr,
                                                                         false,
                                                                         TOP_MOTION);