Remove messy wpf code and fix headers folder structure

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

CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(driver_stim300)
 
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD 20)
 endif()
 
 find_package(ament_cmake REQUIRED)
@@ -19,7 +19,7 @@ install(TARGETS
 )
 
 install(
-  DIRECTORY include/
+  DIRECTORY include/${PROJECT_NAME}
   DESTINATION include/${PROJECT_NAME}
 )
 install(

include/ros_stim300_driver/ros_stim300_driver.hpp → include/driver_stim300/ros_stim300_driver/ros_stim300_driver.hpp

@@ -11,16 +11,6 @@
 #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;
@@ -30,14 +20,26 @@ struct EulerAngles {
   double roll, pitch, yaw;
 };
 
+struct CalibrationData
+{
+  double inclination_x_sum{};
+  double inclination_y_sum{};
+  double inclination_z_sum{};
+  int n_samples{};
+};
+
+
 Quaternion
-FromRPYToQuaternion(EulerAngles angles); // yaw (Z), pitch (Y), roll (X)
+fromRPYToQuaternion(EulerAngles angles); // yaw (Z), pitch (Y), roll (X)
 
 class Stim300DriverNode : public rclcpp::Node {
 public:
   Stim300DriverNode();
 
 private:
+  void calibrateSensor(const int inc_x, const int inc_y, const int inc_z);
+  void processNewMeasurement();
+  void setupTimedCallback();
   void timerCallback();
 
   bool responseCalibrateIMU(
@@ -48,13 +50,15 @@ class Stim300DriverNode : public rclcpp::Node {
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr calibration_service_;
   rclcpp::TimerBase::SharedPtr timer_;
 
-  bool calibration_mode_ = false;
+  bool calibration_mode_{false};
+  CalibrationData calibrationData_{};
+
 
-  std::string device_name_;
-  double variance_gyro_;
-  double variance_acc_;
-  double gravity_;
-  double sample_rate_;
+  std::string device_name_{};
+  double variance_gyro_{};
+  double variance_acc_{};
+  double gravity_{};
+  int sample_rate_{};
 
   sensor_msgs::msg::Imu stim300msg_;
 

include/stim300_driver/datagram_parser.hpp → include/driver_stim300/stim300_driver/datagram_parser.hpp

@@ -91,6 +91,8 @@ struct DatagramParser {
     } else if (size == 2) {
       return ((*it++ << 24) | (*it++ << 16)) >> 16;
     }
+
+    return 0;
   }
 };
 } // end namespace stim_300

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

@@ -79,7 +79,7 @@ class DriverStim300 {
   uint8_t sensor_status_{0};
 
   Stim300Status readDataStream();
-  bool setDatagramFormat(DatagramIdentifier id);
+  void setDatagramFormat(DatagramIdentifier id);
   static bool verifyChecksum(const std::vector<uint8_t>::const_iterator &begin,
                              const std::vector<uint8_t>::const_iterator &end,
                              const uint8_t &crc_dummy_bytes);

include/stim300_driver/stim300_constants.hpp → include/driver_stim300/stim300_driver/stim300_constants.hpp

@@ -82,6 +82,8 @@ static constexpr uint16_t sampleFreq2int(const SampleFreq &sample_freq) {
     return 2000;
   case SampleFreq::TRG:
     return 0;
+  default:
+    return 0;
   }
 }
 
@@ -169,49 +171,53 @@ static constexpr std::array<DatagramInfo, 18>
     }};
 
 constexpr uint8_t datagramIdentifierToRaw(DatagramIdentifier d_id) {
-  for (int i = 0; i < datagram_info_map.size(); ++i) {
-    if (datagram_info_map[i].id == d_id) {
-      return datagram_info_map[i].raw_id;
+  for (size_t i = 0; i < datagram_info_map.size(); ++i) {
+    if (datagram_info_map.at(i).id == d_id) {
+      return datagram_info_map.at(i).raw_id;
     }
   }
+  return 0; // TODO: Error handling
+
 }
 
 constexpr DatagramIdentifier rawToDatagramIdentifier(uint8_t datagram_id) {
-  for (int i = 0; i < datagram_info_map.size(); ++i) {
-    if (datagram_info_map[i].raw_id == datagram_id) {
-      return datagram_info_map[i].id;
+  for (size_t i = 0; i < datagram_info_map.size(); ++i) {
+    if (datagram_info_map.at(i).raw_id == datagram_id) {
+      return datagram_info_map.at(i).id;
     }
   }
-  return DatagramIdentifier::CONFIGURATION_CRLF; // Todo: implement error
-                                                 // handeling
+  return DatagramIdentifier::CONFIGURATION_CRLF; // TODO: Error handling
 };
 
 constexpr uint8_t numberOfPaddingBytes(DatagramIdentifier datagram_identifier) {
-  for (int i = 0; i < datagram_info_map.size(); ++i) {
-    if (datagram_info_map[i].id == datagram_identifier) {
-      return datagram_info_map[i].number_of_padding_bytes;
+  for (size_t i = 0; i < datagram_info_map.size(); ++i) {
+    if (datagram_info_map.at(i).id == datagram_identifier) {
+      return datagram_info_map.at(i).number_of_padding_bytes;
     }
   }
+  return 0; // TODO: Error handling
 }
 
 constexpr std::array<bool, 5>
 isIncluded(DatagramIdentifier datagram_identifier) {
-  for (int i = 0; i < datagram_info_map.size(); ++i) {
-    if (datagram_info_map[i].id == datagram_identifier) {
-      return datagram_info_map[i].included_sensors;
+  for (size_t i = 0; i < datagram_info_map.size(); ++i) {
+    if (datagram_info_map.at(i).id == datagram_identifier) {
+      return datagram_info_map.at(i).included_sensors;
     }
   }
+  return {}; // TODO: Error handling
 }
 
 static DatagramIdentifier toDatagramID(std::array<bool, 5> isIncluded) {
-  for (int i = 0; i < datagram_info_map.size(); ++i) {
-    if (datagram_info_map[i].included_sensors == isIncluded) {
-      return datagram_info_map[i].id;
+  for (size_t i = 0; i < datagram_info_map.size(); ++i) {
+    if (datagram_info_map.at(i).included_sensors == isIncluded) {
+      return datagram_info_map.at(i).id;
     }
   }
+  return DatagramIdentifier::CONFIGURATION_CRLF; // TODO: Error handling
 }
 
-static const uint8_t
+static uint8_t
 calculateDatagramSize(DatagramIdentifier datagram_identifier) {
   if (datagram_identifier == DatagramIdentifier::CONFIGURATION or
       datagram_identifier == DatagramIdentifier::CONFIGURATION_CRLF) {
@@ -252,6 +258,8 @@ static constexpr double accScale(AccRange acc_range) {
     return 1.0 / powerOf2(18);
   case AccRange::G80:
     return 1.0 / powerOf2(16);
+  default:
+    return 0;
   }
 }
 
@@ -267,6 +275,8 @@ static constexpr double accIncrScale(AccRange acc_range) {
     return 1.0 / powerOf2(21);
   case AccRange::G80:
     return 1.0 / powerOf2(19);
+  default:
+    return 0;
   }
 }
 

package.xml

@@ -2,23 +2,18 @@
 <?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>
+    <version>0.0.1</version>
     <description>The drivers-imu_stim300 package</description>
-
-
     <maintainer email="[email protected]">vortex</maintainer>
 
     <license>MIT</license>
 
-    <buildtool_depend>colcon</buildtool_depend>
-    <build_depend>roscpp</build_depend>
-    <build_depend>sensor_msgs</build_depend>
-    <build_depend>std_srvs</build_depend>
-    <build_export_depend>roscpp</build_export_depend>
-    <build_export_depend>sensor_msgs</build_export_depend>
-    <build_export_depend>std_srvs</build_export_depend>	
-    <exec_depend>roscpp</exec_depend>
-    <exec_depend>sensor_msgs</exec_depend>
-    <exec_depend>std_srvs</exec_depend>
+    <buildtool_depend>ament_cmake</buildtool_depend>
+    <depend>roscpp</depend>
+    <depend>sensor_msgs</depend>
+    <depend>std_srvs</depend>
+    <export>
+    <build_type>ament_cmake</build_type>
+    </export>
 
 </package>

src/ros_stim300_driver/CMakeLists.txt

@@ -19,18 +19,18 @@ ament_target_dependencies(
     std_srvs
 )
 
+target_link_libraries(
+  ${ROS_DRIVER_LIB}
+  PUBLIC
+  stim300_driver_lib
+)
 
 target_include_directories(
     ${ROS_DRIVER_LIB}
     PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}>
 )
 
-target_link_libraries(
-  ${ROS_DRIVER_LIB}
-  PUBLIC
-  stim300_driver_lib
-)
 
 install(TARGETS
     ${ROS_DRIVER_LIB}
@@ -48,7 +48,7 @@ add_executable(
 target_include_directories(
   ${STIM300_DRIVER_BINARY_NAME}
   PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}>
 )
 
 target_link_libraries(