Feature/ros2 mip sdk (#190)

* First pass at using the MIP SDK for ROS2

* Resets forced flag at end of cmake and updates submodule

* Updates submoduler and CMakeLists.txt for refactored device interface

* Adds version information

* Updates submodule to properly handle timestamps

* Updates to use camel case ROS functions

* Updates submodule

* Updates submodule to hopefully fix build errors

* Removes deprecated launch parameters

* Updates submodule to hopefully fix foxy build

* Updates submodule to add MIP SDK logging

* Updates submodule to main

.devcontainer/.vscode-docker/c_cpp_properties.json

@@ -3,6 +3,7 @@
         {
             "name": "Linux",
             "includePath": [
+                "/usr/local/include/**",
                 "${workspaceFolder}/**",
                 "/opt/ros/**"
             ],

microstrain_inertial_driver/CMakeLists.txt

@@ -1,38 +1,77 @@
 cmake_minimum_required(VERSION 3.5)
 project(microstrain_inertial_driver)
 
+# C++ 14 required
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_C_STANDARD 99)
+set(CMAKE_C_STANDARD_REQUIRED ON)
+
+# Force some options for this module and the MIP SDK
+set(BUILD_SHARED_LIBS_TEMP "${BUILD_SHARED_LIBS}")
+set(BUILD_EXAMPLES_TEMP "${BUILD_EXAMPLES}")
+set(BUILD_TESTING_TEMP "${BUILD_TESTING}")
+set(BUILD_SHARED_LIBS OFF CACHE BOOL "" FORCE)
+set(BUILD_EXAMPLES OFF CACHE BOOL "" FORCE)
+set(BUILD_TESTING OFF CACHE BOOL "" FORCE)
+
 # Locate the common code and messages
 set(COMMON_NAME "microstrain_inertial_driver_common")
 set(COMMON_DIR "${${PROJECT_NAME}_SOURCE_DIR}/${COMMON_NAME}")
 set(COMMON_SRC_DIR "${COMMON_DIR}/src")
 set(COMMON_INC_DIR "${COMMON_DIR}/include/${COMMON_NAME}")
+set(MIP_SDK_DIR "${COMMON_DIR}/mip_sdk")
+set(MIP_SDK_SRC_DIR "${MIP_SDK_DIR}/src")
+
+# Some helpful options for debugging
+add_compile_options($<$<CONFIG:DEBUG>:-ggdb>)
+
+# Try to find Git
+find_package(Git)
 
 # Make sure that the submodule has been properly initialized
-if(NOT EXISTS "${COMMON_DIR}/src")
+if(NOT EXISTS "${COMMON_SRC_DIR}")
   message(STATUS "Initializing ${COMMON_DIR} submodule. This should only happen once.")
 
   # Make sure we can find the git executable
-  find_package(Git)
   if(NOT Git_FOUND)
     message(FATAL_ERROR "Unable to initialize submodule because we could not find the git executable. Please clone this repo using 'git clone --recursive'")
   endif()
 
   # Initialize and update the submodule
   execute_process(
     WORKING_DIRECTORY "${${PROJECT_NAME}_SOURCE_DIR}"
-    COMMAND ${CMAKE_COMMAND} -E env ${GIT_EXECUTABLE} submodule update --init --recursive
+    COMMAND ${CMAKE_COMMAND} -E env ${GIT_EXECUTABLE} submodule update --recursive --init
   )
 endif()
 
-# Download and install MSCL
-include(${COMMON_DIR}/cmake/download_mscl.cmake)
-download_mscl(VERSION "64.2.2")
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+# Use Git to find the version
+set(DEFAULT_DRIVER_GIT_VERSION "unknown")
+if(NOT GIT_FOUND)
+  message(STATUS "Unable to find git, will build with unknown version")
+  set(DRIVER_GIT_VERSION ${DEFAULT_DRIVER_GIT_VERSION})
+else()
+  execute_process(
+    COMMAND ${CMAKE_COMMAND} -E env ${GIT_EXECUTABLE} describe --tags
+    WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+    OUTPUT_VARIABLE DRIVER_GIT_VERSION_OUT
+    ERROR_VARIABLE DRIVER_GIT_VERSION_ERR
+    RESULT_VARIABLE DRIVER_GIT_VERSION_RET
+  )
+  if(NOT ${DRIVER_GIT_VERSION_RET} EQUAL 0)
+    message(STATUS "Unable to determine version from Git, defaulting to version ${DEFAULT_DRIVER_GIT_VERSION}")
+    set(DRIVER_GIT_VERSION ${DEFAULT_DRIVER_GIT_VERSION})
+  else()
+    set(DRIVER_GIT_VERSION ${DRIVER_GIT_VERSION_OUT})
+    string(REGEX REPLACE "\n" "" DRIVER_GIT_VERSION "${DRIVER_GIT_VERSION}")
+    message(STATUS "Microstrain Driver Version: ${DRIVER_GIT_VERSION}")
+  endif()
 endif()
 
+# Include the MIP SDK source
+add_subdirectory("${MIP_SDK_DIR}" EXCLUDE_FROM_ALL)
+include_directories("${MIP_SDK_SRC_DIR}")
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-implicit-function-declaration -Wno-incompatible-pointer-types -fno-builtin-memcpy")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-builtin-memcpy")
@@ -102,23 +141,37 @@ set(NODE_SRC_FILES
 ament_package()
 
 set(LIB_SRC_FILES
-  ${COMMON_SRC_DIR}/microstrain_subscribers.cpp
-  ${COMMON_SRC_DIR}/microstrain_publishers.cpp
-  ${COMMON_SRC_DIR}/microstrain_node_base.cpp
-  ${COMMON_SRC_DIR}/microstrain_services.cpp
-  ${COMMON_SRC_DIR}/microstrain_parser.cpp
-  ${COMMON_SRC_DIR}/microstrain_config.cpp
+  ${COMMON_SRC_DIR}/subscribers.cpp
+  ${COMMON_SRC_DIR}/publishers.cpp
+  ${COMMON_SRC_DIR}/node_common.cpp
+  ${COMMON_SRC_DIR}/services.cpp
+  ${COMMON_SRC_DIR}/config.cpp
+
+  ${COMMON_SRC_DIR}/utils/mappings/mip_publisher_mapping.cpp
+
+  ${COMMON_SRC_DIR}/utils/mip/ros_mip_device.cpp
+  ${COMMON_SRC_DIR}/utils/mip/ros_mip_device_main.cpp
+  ${COMMON_SRC_DIR}/utils/mip/ros_mip_device_aux.cpp
+  ${COMMON_SRC_DIR}/utils/mip/ros_connection.cpp
+
   src/microstrain_inertial_driver.cpp
 )
 set(LIB_INC_FILES
-  ${COMMON_INC_DIR}/microstrain_subscribers.h
-  ${COMMON_INC_DIR}/microstrain_publishers.h
-  ${COMMON_INC_DIR}/microstrain_node_base.h
-  ${COMMON_INC_DIR}/microstrain_services.h
-  ${COMMON_INC_DIR}/microstrain_parser.h
-  ${COMMON_INC_DIR}/microstrain_config.h
-  ${COMMON_INC_DIR}/microstrain_defs.h
-  ${COMMON_INC_DIR}/microstrain_ros_funcs.h
+  ${COMMON_INC_DIR}/subscribers.h
+  ${COMMON_INC_DIR}/publishers.h
+  ${COMMON_INC_DIR}/node_common.h
+  ${COMMON_INC_DIR}/services.h
+  ${COMMON_INC_DIR}/config.h
+  ${COMMON_INC_DIR}/utils/ros_compat.h
+
+  ${COMMON_INC_DIR}/utils/mappings/mip_mapping.h
+  ${COMMON_INC_DIR}/utils/mappings/mip_publisher_mapping.h
+
+  ${COMMON_INC_DIR}/utils/mip/ros_mip_device.h
+  ${COMMON_INC_DIR}/utils/mip/ros_mip_device_main.h
+  ${COMMON_INC_DIR}/utils/mip/ros_mip_device_aux.h
+  ${COMMON_INC_DIR}/utils/mip/ros_connection.h
+
   include/${PROJECT_NAME}/microstrain_inertial_driver.h
 )
 add_library(${PROJECT_NAME} ${LIB_SRC_FILES} ${LIB_INC_FILES})
@@ -142,6 +195,12 @@ ament_target_dependencies(${PROJECT_NAME}
 # Executables
 add_executable(${PROJECT_NAME}_node ${NODE_SRC_FILES} ${NODE_INC_FILES})
 
+# Annoying, but the ROS types don't match up with the MIP types for floats and doubles, so ignore those warnings for now
+set_source_files_properties(${COMMON_SRC_DIR}/services.cpp PROPERTIES COMPILE_OPTIONS "-Wno-narrowing")
+
+# Tell the code the version of the driver that is being build
+add_definitions(-DMICROSTRAIN_DRIVER_VERSION="${DRIVER_GIT_VERSION}")
+
 # Let the code know if it is being compiled with ROS1 or ROS2
 if(DEFINED ENV{ROS_VERSION})
   add_definitions(-DMICROSTRAIN_ROS_VERSION=$ENV{ROS_VERSION})
@@ -164,12 +223,9 @@ if(DEFINED ENV{ROS_DISTRO})
   endif()
 endif()
 
-# Allow the MSCL include directory to be accessed
-include_directories(${MSCL_INC_PATH} ${BOOST_INC_PATH})
-
 # Linking
 target_link_libraries(${PROJECT_NAME}
-  ${MSCL_LIB_PATH}
+  mip
   ${catkin_LIBRARIES}
 )
 target_link_libraries(${PROJECT_NAME}_node
@@ -181,7 +237,7 @@ target_link_libraries(${PROJECT_NAME}_node
 ## Install ##
 #############
 
-install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+install(TARGETS ${PROJECT_NAME}_node
   DESTINATION share/${PROJECT_NAME}
   LIBRARY DESTINATION lib/${PROJECT_NAME}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
@@ -196,8 +252,7 @@ install(DIRECTORY ${COMMON_DIR}/config
   DESTINATION share/${PROJECT_NAME}/${COMMON_NAME}
 )
 
-# We also have to install MSCL and it's dependencies so it can be found at runtime
-file(GLOB BOOST_LIBS "${MSCL_DIR}/Boost/lib/*")
-install(FILES ${BOOST_LIBS} ${MSCL_DIR}/libmscl.so
-  DESTINATION lib
-)
+# Reset the forced options
+set(BUILD_SHARED_LIBS OFF CACHE BOOL "${BUILD_SHARED_LIBS_TEMP}" FORCE)
+set(BUILD_EXAMPLES OFF CACHE BOOL "${BUILD_EXAMPLES_TEMP}" FORCE)
+set(BUILD_TESTING OFF CACHE BOOL "${BUILD_TESTING_TEMP}" FORCE)

microstrain_inertial_driver/include/microstrain_inertial_driver/microstrain_inertial_driver.h

@@ -1,9 +1,9 @@
 /////////////////////////////////////////////////////////////////////////////////////////////////////
 //
-// Parker-Lord ROS2 Inertial Driver Definition File
+// Parker-Lord Driver Definition File
 // 
 // Copyright (c) 2017, Brian Bingham
-// Copyright (c)  2021, Parker Hannifin Corp
+// Copyright (c) 2021, Parker Hannifin Corp
 // 
 // This code is licensed under MIT license (see LICENSE file for details)
 // 
@@ -13,34 +13,22 @@
 #ifndef _MICROSTRAIN_3DM_H
 #define _MICROSTRAIN_3DM_H
 
-/////////////////////////////////////////////////////////////////////////////////////////////////////
-//
-// Include Files
-//
-/////////////////////////////////////////////////////////////////////////////////////////////////////
-
 #include <cstdio>
 #include <unistd.h>
 #include <time.h>
 #include <iostream>
 #include <fstream>
 #include <functional>
 
-#include "microstrain_inertial_driver_common/microstrain_node_base.h"
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////
-///
-/// \brief Contains functions for micostrain driver
-///
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+#include "microstrain_inertial_driver_common/node_common.h"
 
 namespace microstrain 
 {
 
 ///
 /// \brief Microstrain class
 ///
-class Microstrain : public rclcpp_lifecycle::LifecycleNode, public MicrostrainNodeBase
+class Microstrain : public rclcpp_lifecycle::LifecycleNode, public NodeCommon
 {
  public:
   Microstrain();
@@ -59,7 +47,6 @@ class Microstrain : public rclcpp_lifecycle::LifecycleNode, public MicrostrainNo
 
   void parse_and_publish_main_wrapper();
   void parse_and_publish_aux_wrapper();
-  void device_status_wrapper();
 
  private:
 
@@ -73,9 +60,9 @@ template<typename Object, void (Object::*Callback)()>
 RosTimerType Microstrain::create_timer_wrapper(double rate_hz)
 {
 #ifdef MICROSTRAIN_ROLLING
-  return create_timer(std::chrono::duration<double, std::milli>(1 / rate_hz), std::bind(Callback, this));
+  return createTimer(std::chrono::duration<double, std::milli>(1 / rate_hz), std::bind(Callback, this));
 #else
-  return create_timer<Microstrain>(node_, rate_hz, Callback, this);
+  return createTimer<Microstrain>(node_, rate_hz, Callback, this);
 #endif
 }
 

microstrain_inertial_driver/launch/microstrain_launch.py

@@ -39,21 +39,6 @@ def generate_launch_description():
   launch_description.append(DeclareLaunchArgument('debug',       default_value='false',            description='Whether or not to log debug information.'))
   launch_description.append(DeclareLaunchArgument('params_file', default_value=_EMPTY_PARAMS_FILE, description='Path to file that will load additional parameters'))
 
-  # Add some old launch parameters for backwards compatibility
-  # NOTE: These parameters are deprecated and will be removed in a future release. It is strongly recommended to use the new params_file option instead
-  launch_description.append(DeclareLaunchArgument('port',                  default_value='/dev/ttyACM0',        description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('aux_port',              default_value='/dev/ttyACM1',        description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('baudrate',              default_value='115200',              description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('imu_frame_id',          default_value='sensor',              description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('imu_data_rate',         default_value='100',                 description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('filter_data_rate',      default_value='10',                  description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('gnss1_frame_id',        default_value='gnss1_antenna_wgs84', description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('gnss2_frame_id',        default_value='gnss2_antenns_wgs84', description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('filter_frame_id',       default_value='sensor_wgs84',        description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('filter_child_frame_id', default_value='sensor',              description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('nmea_frame_id',         default_value='nmea',                description="DEPRECATED. Use params_file instead"))
-  launch_description.append(DeclareLaunchArgument('use_enu_frame',         default_value='False',               description="DEPRECATED. Use params_file instead"))
-
   # Pass an environment variable to the node to determine if it is in debug or not
   launch_description.append(SetEnvironmentVariable('MICROSTRAIN_INERTIAL_DEBUG', value=LaunchConfiguration('debug')))
 
@@ -69,22 +54,6 @@ def generate_launch_description():
       # Load the default params file manually, since this is a ROS params file, we will need to load the file manually
       yaml.safe_load(open(_DEFAULT_PARAMS_FILE, 'r')),
 
-      # NOTE: These parameters are deprecated and will be removed in a future release. It is strongly recommended to use the new params_file option instead
-      {
-        "port"                  : LaunchConfiguration('port'),
-        "aux_port"              : LaunchConfiguration('aux_port'),
-        "baudrate"              : LaunchConfiguration('baudrate'),
-        "imu_frame_id"          : LaunchConfiguration('imu_frame_id'),
-        "gnss1_frame_id"        : LaunchConfiguration('gnss1_frame_id'),
-        "gnss2_frame_id"        : LaunchConfiguration('gnss2_frame_id'),
-        "filter_frame_id"       : LaunchConfiguration('filter_frame_id'),
-        "filter_child_frame_id" : LaunchConfiguration('filter_child_frame_id'),
-        "nmea_frame_id"         : LaunchConfiguration('nmea_frame_id'),
-        "imu_data_rate"         : LaunchConfiguration('imu_data_rate'),
-        "filter_data_rate"      : LaunchConfiguration('filter_data_rate'),
-        "use_enu_frame"         : LaunchConfiguration('use_enu_frame'),
-      },
-
       # If you want to override any settings in the params.yml file, make a new yaml file, and set the value via the params_file arg
       LaunchConfiguration('params_file'),
 

microstrain_inertial_driver/package.xml

@@ -10,6 +10,7 @@
   <license>MIT</license>
   <url type="website">https://github.com/LORD-MicroStrain/microstrain_inertial</url>
 
+  <buildtool_depend>git</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <!-- Used to determine which ROS version is available at build time -->