Initial ROS2 port and refactor

Signed-off-by: Christopher Strøm <[email protected]>

CMakeLists.txt

@@ -1,53 +1,33 @@
-cmake_minimum_required(VERSION 3.5.1)
+cmake_minimum_required(VERSION 3.5)
 project(driver_stim300)
 
-set(CMAKE_CXX_STANDARD 14)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
 
-add_library(stim300_driver_lib
-        src/driver_stim300.cpp
-        src/datagram_parser.cpp
-        src/serial_unix.cpp)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 
-target_include_directories(stim300_driver_lib
-        PUBLIC
-        $<INSTALL_INTERFACE:include>
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        PRIVATE
-        ${CMAKE_CURRENT_SOURCE_DIR}/src
-        )
-target_link_libraries(stim300_driver_lib)
+add_subdirectory(src)
 
-find_package(catkin REQUIRED COMPONENTS
-        roscpp
-        sensor_msgs
-	std_srvs
-        )
-catkin_package(
-        CATKIN_DEPENDS sensor_msgs std_srvs
+install(TARGETS
+  stim300_driver_lib
+  ros_stim300_driver_node
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-add_executable(stim300_driver_node src/stim300_driver_node.cpp)
-target_include_directories(stim300_driver_node
-        PRIVATE
-        ${catkin_INCLUDE_DIRS})
-target_link_libraries(stim300_driver_node PRIVATE stim300_driver_lib ${catkin_LIBRARIES})
-
-
-if (CATKIN_ENABLE_TESTING)
-    catkin_add_gtest(check_datasheet_constanst
-            test/check_stim300_constants.cpp
-            src/stim300_constants.h)
-
-    target_link_libraries(check_datasheet_constanst ${catkin_LIBRARIES})
-endif()
-
-if (CATKIN_ENABLE_TESTING)
-    catkin_add_gmock(check_driver_stim300
-            test/check_driver_stim300.cpp
-            test/mock_serial_driver.h
-            src/serial_driver.h)
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
 
-    target_link_libraries(check_driver_stim300 stim300_driver_lib ${catkin_LIBRARIES})
+if(BUILD_TESTING)
+ add_subdirectory(test)
 endif()
 
-
+ament_package()

include/ros_stim300_driver/ros_stim300_driver.hpp

@@ -0,0 +1,66 @@
+#ifndef ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP
+#define ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP
+
+#include "stim300_driver/driver_stim300.hpp"
+#include "stim300_driver/serial_unix.hpp"
+#include "math.h"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "std_srvs/srv/trigger.hpp"
+#include "iostream"
+#include <vector>
+
+constexpr int NUMBER_OF_CALIBRATION_SAMPLES{100};
+constexpr double ACC_TOLERANCE{0.1};
+constexpr double MAX_DROPPED_ACC_X_MSG{5};
+constexpr double MAX_DROPPED_ACC_Y_MSG{5};
+constexpr double MAX_DROPPED_ACC_Z_MSG{5};
+constexpr double MAX_DROPPED_GYRO_X_MSG{5};
+constexpr double MAX_DROPPED_GYRO_Y_MSG{5};
+constexpr double MAX_DROPPED_GYRO_Z_MSG{5};
+constexpr double GYRO_X_PEAK_TO_PEAK_NOISE{0.0002};
+constexpr double GYRO_Y_PEAK_TO_PEAK_NOISE{0.0002};
+constexpr double GYRO_Z_PEAK_TO_PEAK_NOISE{0.0002};
+
+struct Quaternion
+{
+    double w, x, y, z;
+};
+
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
+
+Quaternion FromRPYToQuaternion(EulerAngles angles); // yaw (Z), pitch (Y), roll (X)
+
+
+class Stim300DriverNode : public rclcpp::Node
+{
+public:
+    Stim300DriverNode();
+
+private:
+    void timerCallback();
+
+    bool responseCalibrateIMU(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+                          std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr calibration_service_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    bool calibration_mode_ = false;
+
+    std::string device_name_;
+    double variance_gyro_;
+    double variance_acc_;
+    double gravity_;
+    double sample_rate_;
+
+    sensor_msgs::msg::Imu stim300msg_;
+
+    std::unique_ptr<SerialUnix> serial_driver_;
+    std::unique_ptr<DriverStim300> driver_stim300_;
+};
+
+#endif  // ROS_STIM300_DRIVER_ROS_STIM300_DRIVER_HPP

src/datagram_parser.h → include/stim300_driver/datagram_parser.hpp

@@ -2,12 +2,13 @@
 #ifndef DRIVER_STIM300_DATAGRAM_PARSER_H
 #define DRIVER_STIM300_DATAGRAM_PARSER_H
 
-#include "stim300_constants.h"
+#include "stim300_constants.hpp"
 #include <array>
 #include <cassert>
 #include <cstdint>
 #include <sstream>
 #include <vector>
+
 using namespace stim_const;
 
 namespace stim_300 {

include/driver_stim300.h → include/stim300_driver/driver_stim300.hpp

@@ -2,9 +2,9 @@
 #ifndef DRIVER_STIM300_DRIVER_STIM300_H
 #define DRIVER_STIM300_DRIVER_STIM300_H
 
-#include "../src/datagram_parser.h"
-#include "../src/serial_unix.h"
-#include "../src/stim300_constants.h"
+#include "datagram_parser.hpp"
+#include "serial_unix.hpp"
+#include "stim300_constants.hpp"
 #include <boost/crc.hpp>
 #include <vector>
 

src/serial_unix.h → include/stim300_driver/serial_unix.hpp

@@ -1,13 +1,13 @@
 #ifndef DRIVER_STIM300_SERIAL_UBUNTU_H
 #define DRIVER_STIM300_SERIAL_UBUNTU_H
 
-#include "serial_driver.h"
+#include "serial_driver.hpp"
 #include <fcntl.h>
 #include <string>
 #include <termios.h>
 #include <unistd.h>
 
-#include "stim300_constants.h"
+#include "stim300_constants.hpp"
 #include <cstring>
 #include <stdexcept>
 #include <stdint.h>

launch/stim300_driver.launch.py

@@ -0,0 +1,41 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'device_name',
+            default_value='/dev/ttyUSB0',
+            description='Device name'
+        ),
+        DeclareLaunchArgument(
+            'standard_deviation_of_gyro',
+            default_value='0.0004',
+            description='Standard deviation of gyro'
+        ),
+        DeclareLaunchArgument(
+            'standard_deviation_of_acc',
+            default_value='0.004',
+            description='Standard deviation of accelerometer'
+        ),
+        DeclareLaunchArgument(
+            'sample_rate',
+            default_value='125',
+            description='Sample rate'
+        ),
+        Node(
+            package='driver_stim300',
+            executable='stim300_driver_node',
+            name='stim300driver',
+            parameters=[{
+                'device_name': LaunchConfiguration('device_name'),
+                'standard_deviation_of_gyro': LaunchConfiguration('standard_deviation_of_gyro'),
+                'standard_deviation_of_acc': LaunchConfiguration('standard_deviation_of_acc'),
+                'sample_rate': LaunchConfiguration('sample_rate')
+            }]
+        )
+    ])
+

package.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
     <name>driver_stim300</name>
     <version>0.0.0</version>
     <description>The drivers-imu_stim300 package</description>
@@ -9,18 +10,7 @@
 
     <license>MIT</license>
 
-    <!-- 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>gmock</test_depend-->
-    <!--<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>
+    <buildtool_depend>colcon</buildtool_depend>
     <build_depend>roscpp</build_depend>
     <build_depend>sensor_msgs</build_depend>
     <build_depend>std_srvs</build_depend>
@@ -31,10 +21,4 @@
     <exec_depend>sensor_msgs</exec_depend>
     <exec_depend>std_srvs</exec_depend>
 
-
-    <!-- The export tag contains other, unspecified, tags -->
-    <export>
-        <!-- Other tools can request additional information be placed here -->
-
-    </export>
 </package>

src/CMakeLists.txt

@@ -0,0 +1,3 @@
+add_subdirectory(stim300_driver)
+add_subdirectory(ros_stim300_driver)
+

src/ros_stim300_driver/CMakeLists.txt

@@ -0,0 +1,63 @@
+set(ROS_DRIVER_LIB ros_stim300_driver_lib)
+
+add_library(
+    ${ROS_DRIVER_LIB}
+    SHARED    
+)
+
+target_sources(
+    ${ROS_DRIVER_LIB}
+    PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/ros_stim300_driver.cpp
+)
+
+ament_target_dependencies(
+    ${ROS_DRIVER_LIB}
+    PUBLIC
+    rclcpp
+    sensor_msgs
+    std_srvs
+)
+
+
+target_include_directories(
+    ${ROS_DRIVER_LIB}
+    PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
+)
+
+target_link_libraries(
+  ${ROS_DRIVER_LIB}
+  PUBLIC
+  stim300_driver_lib
+)
+
+install(TARGETS
+    ${ROS_DRIVER_LIB}
+    DESTINATION lib
+)
+
+
+set(STIM300_DRIVER_BINARY_NAME ros_stim300_driver_node)
+
+add_executable(
+  ${STIM300_DRIVER_BINARY_NAME}
+  ${CMAKE_CURRENT_SOURCE_DIR}/ros_stim300_driver_node.cpp
+)
+
+target_include_directories(
+  ${STIM300_DRIVER_BINARY_NAME}
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
+)
+
+target_link_libraries(
+    ${STIM300_DRIVER_BINARY_NAME}
+    ${ROS_DRIVER_LIB}
+)
+
+install(
+  TARGETS
+  ${STIM300_DRIVER_BINARY_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)