Code refactor v2 (#22)

moved all functionalities to modules

---------

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

.devcontainer/Dockerfile

@@ -2,6 +2,7 @@ FROM python:3.9-slim AS stm32flash_builder
 
 
 ARG USERNAME=runner
+ENV USER=runner
 ARG USER_UID=1001
 ARG USER_GID=$USER_UID
 
@@ -30,4 +31,6 @@ RUN git clone https://github.com/husarion/flasher && \
 
 # Build stm32 flasher
 RUN git clone https://github.com/husarion/stm32loader && \
-    cd stm32loader && sudo python setup.py install
+    cd stm32loader && sudo python setup.py install
+
+SHELL ["/bin/bash", "-c"]

.gitmodules

@@ -1,7 +1,3 @@
-[submodule "ros_lib"]
-	path = lib/ros_lib
-	url = https://github.com/byq77/rosserial-mbed.git
-	branch = master
 [submodule "encoder-mbed"]
 	path = lib/encoder-mbed
 	url = https://github.com/byq77/encoder-mbed.git
@@ -10,11 +6,10 @@
 	path = lib/motor-driver-mbed
 	url = https://github.com/byq77/drv88xx-driver-mbed.git
 	branch = master
-[submodule "imu-driver"]
-	path = lib/imu-driver
-	url = https://github.com/byq77/core2-imu-driver.git
-	branch = master
 [submodule "vl53l0x-mbed"]
 	path = lib/vl53l0x-mbed
 	url = https://github.com/byq77/vl53l0x-mbed.git
 	branch = master
+[submodule "lib/imu-driver"]
+	path = lib/imu-driver
+	url = https://github.com/delihus/core2-imu-driver-mbed

ignore_packages.py

@@ -1,5 +1,8 @@
 import os
 user_name = os.getenv('USER')
+if user_name == None:
+    print("There is no global variable 'USER'!!!")
+    exit(-1)
 try:
     os.makedirs(f'/home/{user_name}/.platformio/packages/framework-mbed/features/')
 except FileExistsError:
@@ -8,4 +11,4 @@
 with open(f'/home/{user_name}/.platformio/packages/framework-mbed/features/.mbedignore', 'w') as f:
     f.write('cellular/*\n')
     f.write('netsocket/cellular/*\n')
-    f.write('nanostack/*\n')
+    f.write('nanostack/*\n')

include/battery.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <mbed.h>
+
+#include <sensor_msgs/msg/battery_state.h>
+#include <rmw_microros/rmw_microros.h>
+#include <micro_ros_utilities/string_utilities.h>
+
+constexpr int MEASUREMENT_SERIES = 10;
+constexpr float BATTERY_VOLTAGE_LOW = 10.8;
+
+enum
+{
+  BATTERY_LOW = 1,
+  BATTERY_OK = 0
+};
+
+struct BatteryData
+{
+  float voltage;
+  float threshold;
+  uint8_t status;
+};
+
+static BatteryData battery_data = { 0.0, BATTERY_VOLTAGE_LOW, BATTERY_OK };
+static DigitalOut battery_led(LED1, 1);
+static Ticker battery_led_flipper;
+static AnalogIn battery_adc(BAT_MEAS);
+
+extern sensor_msgs__msg__BatteryState battery_msg;
+static rtos::Thread read_battery_voltage_and_show_status_thead;
+
+void init_battery();
+float read_battery_voltage_and_show_status();
+void read_battery_voltage_and_show_status_task();
+void fill_battery_msg(sensor_msgs__msg__BatteryState *msg);

include/buttons.hpp

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <mbed.h>
+
+#include <std_msgs/msg/bool.h>
+
+enum Buttons
+{
+  button_left,
+  button_right,
+  BUTTONS_COUNT
+};
+
+static volatile bool buttons_publish_flag[BUTTONS_COUNT] = { false, false };
+static std_msgs__msg__Bool button_msgs[BUTTONS_COUNT];
+void init_button_and_attach_to_callbacks(mbed::InterruptIn *interrupt, void (*rise)(), void (*fall)());

include/imu.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <mbed.h>
+#include <ImuDriver.h>
+
+#include <rmw_microros/rmw_microros.h>
+#include <micro_ros_utilities/string_utilities.h>
+#include <sensor_msgs/msg/imu.h>
+
+
+static ImuDriver *imu_driver_ptr;
+static sensor_msgs__msg__Imu imu_msg;
+extern Mail<ImuDriver::ImuMeasurement, 10> imu_sensor_mail_box;
+
+
+static const char* imu_sensor_type_string[] = { "BNO055_ADDR_A", "BNO055_ADDR_B", "MPU9250", "MPU9255", "BHI260", "UNKNOWN" };
+static char imu_description_string[64] = "";
+
+void init_imu(mbed::I2C *i2c_ptr);
+void fill_imu_msg(sensor_msgs__msg__Imu *msg);
+void fill_imu_msg_with_measurements(sensor_msgs__msg__Imu* msg, ImuDriver::ImuMeasurement* measurements);

include/leds.hpp

@@ -0,0 +1,16 @@
+#pragma once
+#include <mbed.h>
+#include <std_msgs/msg/bool.h>
+
+enum LEDs
+{
+  led_left,
+  led_right,
+  LED_COUNT
+};
+
+static DigitalOut led2(LED2, 0);
+static DigitalOut led3(LED3, 0);
+
+void led1_callback(const void* message);
+void led2_callback(const void* message);

include/main.hpp

@@ -1,70 +1,7 @@
 #pragma once
-#include <ImuDriver.h>
-#include <RosbotDrive.h>
-
-#include <memory_debug_message_info.hpp>
 #include <microros.hpp>
-#include <map>
-
-#ifndef KINEMATIC_TYPE
-#define KINEMATIC_TYPE 0
-#endif
-
-#ifndef JOINT_STATES_ENABLE
-#define JOINT_STATES_ENABLE 0
-#endif
 
-#if JOINT_STATES_ENABLE
-static volatile bool joint_states_enabled = true;
-#else
-static volatile bool joint_states_enabled = false;
-#endif
 static UARTSerial microros_serial(RPI_SERIAL_TX, RPI_SERIAL_RX);
 
 static volatile bool distance_sensors_enabled = false;
-
-static volatile bool buttons_publish_flag[BUTTONS_COUNT] = {false, false};
-static volatile float button_release_time = 0.1;
-
-static volatile float battery_voltage = 0.0;
-
-static volatile bool is_speed_watchdog_enabled = true;
-static volatile bool is_speed_watchdog_active = false;
-static uint64_t speed_watchdog_interval = 1000;  // ms
-
-static Timer odom_watchdog_timer;
-static volatile uint32_t last_speed_command_time = 0;
-
 static DigitalOut sens_power(SENS_POWER_ON, 0);
-
-static InterruptIn button1(BUTTON1);
-static InterruptIn button2(BUTTON2);
-static ImuDriver *imu_driver_ptr;
-static rosbot_sensors::ServoManger servo_manager;
-
-std::map<double, uint8_t> servo_voltage_configuration{
-    {5.0, 0},
-    {6.0, 1},
-    {7.4, 2},
-    {8.6, 3}
-};
-
-// Motors setup
-#define MOTOR_FR MOTOR1
-#define MOTOR_FL MOTOR4
-#define MOTOR_RR MOTOR2
-#define MOTOR_RL MOTOR3
-
-constexpr uint8_t POLARITY = 0b00111100;
-constexpr float ROBOT_LENGTH = 0.197;
-constexpr uint8_t ENCODER_CPR = 48;
-constexpr float ROBOT_LENGTH_HALF = ROBOT_LENGTH / 2.0;
-constexpr float DISTANCE_FRONT_TO_REAR_WHEEL = 0.11;
-constexpr float WHEEL_SEPARATION_LENGTH = DISTANCE_FRONT_TO_REAR_WHEEL / 2;
-constexpr float ROBOT_WIDTH = 0.215;  // 0.22 0.195
-constexpr float ROBOT_WIDTH_HALF = ROBOT_WIDTH / 2.0;
-constexpr float DIAMETER_MODIFICATOR = 1.106;  // 1.24, 1.09, 1.164
-constexpr float TYRE_DEFLATION = 1.042;        // theoretical distance / real distance
-constexpr float GEAR_RATIO = 34.014;
-constexpr float WHEEL_DIAMETER = 0.085;
-constexpr float WHEEL_RADIUS = WHEEL_DIAMETER / 2.0;

include/microros.hpp

@@ -7,83 +7,33 @@
 #include <rclc/executor.h>
 #include <rclc/rclc.h>
 #include <rmw_microros/rmw_microros.h>
-#include <sensor_msgs/msg/battery_state.h>
-#include <sensor_msgs/msg/imu.h>
-#include <sensor_msgs/msg/joint_state.h>
-#include <sensor_msgs/msg/range.h>
-#include <std_msgs/msg/float32_multi_array.h>
-#include <std_msgs/msg/u_int32_multi_array.h>
-#include <std_msgs/msg/bool.h>
 #include <microros_transport/mbed_serial_transport.hpp>
 #include <rclc_parameter/rclc_parameter.h>
 
+#include <leds.hpp>
+#include <wheels.hpp>
+#include <buttons.hpp>
+#include <battery.hpp>
+#include <servos.hpp>
+#include <imu.hpp>
+#include <ranges.hpp>
+
 constexpr const uint8_t UXR_CLIENT_DOMAIN_ID_TO_OVERRIDE_WITH_ENV = 255;
 
 constexpr const char *NODE_NAME = "rosbot_ros2_firmware";
 constexpr const char *IMU_TOPIC_NAME = "_imu/data_raw";
 constexpr const char *WHEELS_STATE_TOPIC_NAME = "_motors_response";
-constexpr const char *BATTERY_TOPIC_NAME = "battery";
 constexpr const char *WHEELS_COMMAND_TOPIC_NAME = "_motors_cmd";
+constexpr const char *BATTERY_TOPIC_NAME = "battery";
 constexpr const char *SERVOS_COMMAND_TOPIC_NAME = "cmd_ser";
 
-constexpr const char *FRONT_LEFT_MOTOR_NAME = "fl_wheel_joint";
-constexpr const char *FRONT_RIGHT_MOTOR_NAME = "fr_wheel_joint";
-constexpr const char *REAR_LEFT_MOTOR_NAME = "rl_wheel_joint";
-constexpr const char *REAR_RIGHT_MOTOR_NAME = "rr_wheel_joint";
-enum Buttons{
-    button_left,
-    button_right,
-    BUTTONS_COUNT
-};
-
-enum Ranges {
-    range_right_front,
-    range_left_front,
-    range_right_rear,
-    range_left_rear,
-    RANGES_COUNT
-};
-
-enum LEDs{
-    led_left,
-    led_right,
-    LED_COUNT
-};
-
-enum Motors {
-    motor_right_rear,
-    motor_left_rear,
-    motor_right_front,
-    motor_left_front,
-    MOTORS_COUNT
-};
-
-enum Servos{
-    servo0,
-    servo1,
-    servo2,
-    servo3,
-    servo4,
-    servo5,
-    SERVOS_COUNT
-};
-
-enum MotorsState {
-    motor_state_position,
-    motor_state_velocity,
-    motor_state_effort,
-    MOTORS_STATE_COUNT
-};
-
 enum AgentStates {
     WAITING_AGENT,
     AGENT_AVAILABLE,
     AGENT_CONNECTED,
     AGENT_DISCONNECTED
 };
 
-static DigitalOut led2(LED2, 0);
-static DigitalOut led3(LED3, 0);
 bool microros_deinit();
 
 #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){return false;}}
@@ -118,13 +68,6 @@ bool init_led_subscribers();
 bool init_param_server();
 bool init_parameters();
 
-void fill_wheels_state_msg(sensor_msgs__msg__JointState *msg);
-void fill_imu_msg(sensor_msgs__msg__Imu *msg);
-void fill_battery_msg(sensor_msgs__msg__BatteryState *msg);
-void fill_wheels_command_msg(std_msgs__msg__Float32MultiArray *msg);
-void fill_servos_command_msg(std_msgs__msg__UInt32MultiArray *msg);
-void fill_range_msg(sensor_msgs__msg__Range *msg, uint8_t id);
-
 bool publish_imu_msg(sensor_msgs__msg__Imu *imu_msg);
 bool publish_wheels_state_msg(sensor_msgs__msg__JointState *msg);
 bool publish_battery_msg(sensor_msgs__msg__BatteryState *msg);

include/ranges.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <mbed.h>
+#include <MultiDistanceSensor.h>
+
+#include <rmw_microros/rmw_microros.h>
+#include <micro_ros_utilities/string_utilities.h>
+#include <sensor_msgs/msg/range.h>
+
+enum Ranges {
+    range_right_front,
+    range_left_front,
+    range_right_rear,
+    range_left_rear,
+    RANGES_COUNT
+};
+
+static const char *range_frame_names[] = {"fr_range", "fl_range", "rr_range", "rl_range"};
+
+static sensor_msgs__msg__Range range_msgs[RANGES_COUNT];
+
+void init_ranges();
+void fill_range_msg(sensor_msgs__msg__Range *msg, uint8_t id);
+void fill_range_msg_with_measurements(sensor_msgs__msg__Range *msg, float range);