Skip to content

Commit

Permalink
Add support for mecanum wheels, add boot state (#7)
Browse files Browse the repository at this point in the history 
* adapt firmware to newest version of diff_drive_lib library

Signed-off-by: Aleksander Szymański <[email protected]>

* specify max param name length

Signed-off-by: Aleksander Szymański <[email protected]>

* add more space for micro ROS heap

Signed-off-by: Aleksander Szymański <[email protected]>

* add mecanum parameters to header files

Signed-off-by: Aleksander Szymański <[email protected]>

* change default value of angular_velocity_multiplier param

Signed-off-by: Aleksander Szymański <[email protected]>

* add loading of mecanum params

Signed-off-by: Aleksander Szymański <[email protected]>

* allocate less memory for micro ros heap

Signed-off-by: Aleksander Szymański <[email protected]>

* adapt main file for mecanum wheels handling

Signed-off-by: Aleksander Szymański <[email protected]>

* format code

Signed-off-by: Aleksander Szymański <[email protected]>

* add return statement at the end of a bool method

Signed-off-by: Aleksander Szymański <[email protected]>

* fixed reset_board service race condition

Signed-off-by: Aleksander Szymański <[email protected]>

* Update leo_msgs package

* Update parameters

* Update max params number

* Increase heap size for micro-ros

* Allow dynamically changing controller type

* Fix default firmware params

---------

Signed-off-by: Aleksander Szymański <[email protected]>
Co-authored-by: Błażej Sowa <[email protected]>
  • Loading branch information
@Bitterisland6 @bjsowa
Bitterisland6 and bjsowa authored Nov 15, 2023
1 parent 5ff26f1 commit b2a0dda
Show file tree
Hide file tree
Showing 7 changed files with 187 additions and 94 deletions.
10 changes: 7 additions & 3 deletions Inc/firmware/configuration.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,13 @@
#include "mainf.h"
#include "usart.h"

#include "diff_drive_lib/diff_drive_controller.hpp"
#include "diff_drive_lib/robot_controller.hpp"

#include "firmware/hal_compat.hpp"
#include "firmware/motor_controller.hpp"

// Size of the heap memory used for micro-ROS entities
constexpr uint32_t UROS_HEAP_SIZE = 25000;
constexpr uint32_t UROS_HEAP_SIZE = 30000;

// UART used for micro-ROS communication
static constexpr UART_HandleTypeDef& UROS_UART = huart1;
Expand Down Expand Up @@ -119,25 +119,29 @@ extern MotorController MotB;
extern MotorController MotC;
extern MotorController MotD;

constexpr diff_drive_lib::DiffDriveConfiguration DD_CONFIG = {
constexpr diff_drive_lib::RobotConfiguration ROBOT_CONFIG = {
.wheel_FL_conf =
{
.motor = MotC,
.op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
.velocity_rolling_window_size = 10,
},
.wheel_RL_conf =
{
.motor = MotD,
.op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
.velocity_rolling_window_size = 10,
},
.wheel_FR_conf =
{
.motor = MotA,
.op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
.velocity_rolling_window_size = 10,
},
.wheel_RR_conf =
{
.motor = MotB,
.op_mode = diff_drive_lib::WheelOperationMode::VELOCITY,
.velocity_rolling_window_size = 10,
},
};
16 changes: 9 additions & 7 deletions Inc/firmware/parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@

#include <rclc_parameter/rclc_parameter.h>

#include <diff_drive_lib/diff_drive_controller.hpp>
#include <diff_drive_lib/robot_controller.hpp>

struct Parameters : diff_drive_lib::DiffDriveParams {
struct Parameters : diff_drive_lib::RobotParams {
// Override inherited parameters
Parameters() {
// Wheel
Expand All @@ -15,15 +15,17 @@ struct Parameters : diff_drive_lib::DiffDriveParams {
wheel_pid_d = 0.0F;
wheel_pwm_duty_limit = 100.0F;

// Differential drive
dd_wheel_radius = 0.0625F;
dd_wheel_separation = 0.33F;
dd_angular_velocity_multiplier = 1.91F;
dd_input_timeout = 500;
robot_wheel_radius = 0.0625F;
robot_wheel_separation = 0.358F;
robot_wheel_base = 0.3052F;
robot_angular_velocity_multiplier = 1.76F;
robot_input_timeout = 500;
}

float battery_min_voltage = 10.0;

bool mecanum_wheels = false;

bool init(rclc_parameter_server_t* param_server);
void update(rclc_parameter_server_t* param_server);
};
148 changes: 102 additions & 46 deletions Src/firmware/mainf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,14 @@
#include <geometry_msgs/msg/twist.h>
#include <leo_msgs/msg/imu.h>
#include <leo_msgs/msg/wheel_odom.h>
#include <leo_msgs/msg/wheel_odom_mecanum.h>
#include <leo_msgs/msg/wheel_states.h>
#include <std_msgs/msg/empty.h>
#include <std_msgs/msg/float32.h>
#include <std_srvs/srv/trigger.h>

#include "diff_drive_lib/diff_drive_controller.hpp"
#include "diff_drive_lib/mecanum_controller.hpp"
#include "diff_drive_lib/wheel_controller.hpp"

#include "mainf.h"
Expand Down Expand Up @@ -47,6 +50,8 @@ static std::atomic_bool publish_battery(false);

static leo_msgs__msg__WheelOdom wheel_odom;
static rcl_publisher_t wheel_odom_pub;
static leo_msgs__msg__WheelOdomMecanum wheel_odom_mecanum;
static rcl_publisher_t wheel_odom_mecanum_pub;
static std::atomic_bool publish_wheel_odom(false);

static leo_msgs__msg__WheelStates wheel_states;
Expand All @@ -64,6 +69,9 @@ static std_msgs__msg__Empty param_trigger;
static rcl_publisher_t param_trigger_pub;
static std::atomic_bool publish_param_trigger(true);

static bool mecanum_wheels = false;
static std::atomic_bool controller_replacement(false);

#define WHEEL_WRAPPER(NAME) \
constexpr const char* NAME##_cmd_pwm_topic = \
"~/wheel_" #NAME "/cmd_pwm_duty"; \
Expand Down Expand Up @@ -111,7 +119,10 @@ MotorController MotB(MOT_B_CONFIG);
MotorController MotC(MOT_C_CONFIG);
MotorController MotD(MOT_D_CONFIG);

static diff_drive_lib::DiffDriveController dc(DD_CONFIG);
static uint8_t
controller_buffer[std::max(sizeof(diff_drive_lib::DiffDriveController),
sizeof(diff_drive_lib::MecanumController))];
static diff_drive_lib::RobotController* controller;
static ImuReceiver imu_receiver(&IMU_I2C);

static Parameters params;
Expand All @@ -120,13 +131,13 @@ static std::atomic_bool reload_parameters(false);
static void cmdVelCallback(const void* msgin) {
const geometry_msgs__msg__Twist* msg =
(const geometry_msgs__msg__Twist*)msgin;
dc.setSpeed(msg->linear.x, msg->angular.z);
controller->setSpeed(msg->linear.x, msg->linear.y, msg->angular.z);
}

static void resetOdometryCallback(const void* reqin, void* resin) {
std_srvs__srv__Trigger_Response* res =
(std_srvs__srv__Trigger_Response*)resin;
dc.resetOdom();
controller->resetOdom();
res->success = true;
}

Expand Down Expand Up @@ -196,6 +207,7 @@ static void initMsgs() {
std_msgs__msg__Float32__init(&battery);
std_msgs__msg__Float32__init(&battery_averaged);
leo_msgs__msg__WheelOdom__init(&wheel_odom);
leo_msgs__msg__WheelOdomMecanum__init(&wheel_odom_mecanum);
leo_msgs__msg__WheelStates__init(&wheel_states);
leo_msgs__msg__Imu__init(&imu);
std_msgs__msg__Empty__init(&param_trigger);
Expand Down Expand Up @@ -230,9 +242,6 @@ static bool initROS() {
&battery_averaged_pub, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
"~/battery_averaged"))
RCCHECK(rclc_publisher_init_best_effort(
&wheel_odom_pub, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(leo_msgs, msg, WheelOdom), "~/wheel_odom"))
RCCHECK(rclc_publisher_init_best_effort(
&wheel_states_pub, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(leo_msgs, msg, WheelStates),
Expand All @@ -251,21 +260,21 @@ static bool initROS() {
RCCHECK(rclc_executor_add_subscription(&executor, &twist_sub, &twist_msg,
cmdVelCallback, ON_NEW_DATA))

#define WHEEL_INIT_ROS(NAME) \
RCCHECK(rclc_subscription_init_default( \
&NAME##_cmd_pwm_sub, &node, \
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), \
NAME##_cmd_pwm_topic)) \
RCCHECK(rclc_executor_add_subscription_with_context( \
&executor, &NAME##_cmd_pwm_sub, &NAME##_cmd_pwm_msg, \
wheelCmdPWMDutyCallback, &dc.wheel_##NAME, ON_NEW_DATA)) \
RCCHECK(rclc_subscription_init_default( \
&NAME##_cmd_vel_sub, &node, \
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), \
NAME##_cmd_vel_topic)) \
RCCHECK(rclc_executor_add_subscription_with_context( \
&executor, &NAME##_cmd_vel_sub, &NAME##_cmd_vel_msg, \
wheelCmdVelCallback, &dc.wheel_##NAME, ON_NEW_DATA))
#define WHEEL_INIT_ROS(NAME) \
RCCHECK(rclc_subscription_init_default( \
&NAME##_cmd_pwm_sub, &node, \
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), \
NAME##_cmd_pwm_topic)) \
RCCHECK(rclc_executor_add_subscription_with_context( \
&executor, &NAME##_cmd_pwm_sub, &NAME##_cmd_pwm_msg, \
wheelCmdPWMDutyCallback, &controller->wheel_##NAME, ON_NEW_DATA)) \
RCCHECK(rclc_subscription_init_default( \
&NAME##_cmd_vel_sub, &node, \
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), \
NAME##_cmd_vel_topic)) \
RCCHECK(rclc_executor_add_subscription_with_context( \
&executor, &NAME##_cmd_vel_sub, &NAME##_cmd_vel_msg, \
wheelCmdVelCallback, &controller->wheel_##NAME, ON_NEW_DATA))

WHEEL_INIT_ROS(FL)
WHEEL_INIT_ROS(RL)
Expand Down Expand Up @@ -306,7 +315,7 @@ static bool initROS() {

// Parameter Server
static rclc_parameter_options_t param_options;
param_options.max_params = 11;
param_options.max_params = 13;
param_options.notify_changed_over_dds = true;
RCCHECK(rclc_parameter_server_init_with_option(&param_server, &node,
&param_options))
Expand Down Expand Up @@ -350,6 +359,7 @@ static void finiROS() {
(void)!rcl_publisher_fini(&imu_pub, &node);
(void)!rcl_publisher_fini(&wheel_states_pub, &node);
(void)!rcl_publisher_fini(&wheel_odom_pub, &node);
(void)!rcl_publisher_fini(&wheel_odom_mecanum_pub, &node);
(void)!rcl_publisher_fini(&battery_averaged_pub, &node);
(void)!rcl_publisher_fini(&battery_pub, &node);
(void)!rcl_publisher_fini(&param_trigger_pub, &node);
Expand Down Expand Up @@ -385,13 +395,37 @@ void setup() {

imu_receiver.init();

// Initialize Diff Drive Controller
dc.init(params);

status = AgentStatus::CONNECTING_TO_AGENT;
battery_led_status = BatteryLedStatus::NOT_CONNECTED;
}

void initController() {
mecanum_wheels = params.mecanum_wheels;
if (mecanum_wheels) {
rclc_publisher_init_best_effort(
&wheel_odom_mecanum_pub, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(leo_msgs, msg, WheelOdomMecanum),
"~/wheel_odom_mecanum");
controller =
new (controller_buffer) diff_drive_lib::MecanumController(ROBOT_CONFIG);
} else {
rclc_publisher_init_best_effort(
&wheel_odom_pub, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(leo_msgs, msg, WheelOdom), "~/wheel_odom");
controller = new (controller_buffer)
diff_drive_lib::DiffDriveController(ROBOT_CONFIG);
}
controller->init(params);
}

void finiController() {
if (mecanum_wheels) {
(void)!rcl_publisher_fini(&wheel_odom_mecanum_pub, &node);
} else {
(void)!rcl_publisher_fini(&wheel_odom_pub, &node);
}
}

void loop() {
static uint32_t boot_enter_time;
switch (status) {
Expand All @@ -411,7 +445,6 @@ void loop() {

if (reload_parameters.exchange(false)) {
params.update(&param_server);
dc.updateParams(params);
}

if (publish_param_trigger) {
Expand All @@ -422,20 +455,28 @@ void loop() {
// this uncomented breaks whole ROS communication
// (void)!rclc_executor_remove_service(&executor, &boot_firmware_srv);
// (void)!rcl_service_fini(&boot_firmware_srv, &node);
initController();
status = AgentStatus::AGENT_CONNECTED;
}
break;
case AgentStatus::AGENT_CONNECTED:
rclc_executor_spin_some(&executor, 0);

if (reset_request) reset();

if (publish_battery) {
(void)!rcl_publish(&battery_pub, &battery, NULL);
(void)!rcl_publish(&battery_averaged_pub, &battery_averaged, NULL);
publish_battery = false;
}

if (publish_wheel_odom) {
(void)!rcl_publish(&wheel_odom_pub, &wheel_odom, NULL);
if (params.mecanum_wheels) {
(void)!rcl_publish(&wheel_odom_mecanum_pub, &wheel_odom_mecanum,
NULL);
} else {
(void)!rcl_publish(&wheel_odom_pub, &wheel_odom, NULL);
}
publish_wheel_odom = false;
}

Expand All @@ -451,11 +492,18 @@ void loop() {

if (reload_parameters.exchange(false)) {
params.update(&param_server);
dc.updateParams(params);
if (params.mecanum_wheels != mecanum_wheels) {
controller_replacement = true;
finiController();
initController();
controller_replacement = false;
} else {
controller->updateParams(params);
}
}
break;
case AgentStatus::AGENT_LOST:
dc.disable();
controller->disable();
finiROS();
status = AgentStatus::CONNECTING_TO_AGENT;
break;
Expand Down Expand Up @@ -541,11 +589,10 @@ void update() {
publish_param_trigger = true;
}
}
if (reset_request) reset();

if (status != AgentStatus::AGENT_CONNECTED) return;
if (status != AgentStatus::AGENT_CONNECTED || controller_replacement) return;

dc.update(UPDATE_PERIOD);
controller->update(UPDATE_PERIOD);

if (cnt % BATTERY_PUB_PERIOD == 0 && !publish_battery) {
battery.data = static_cast<float>(BATTERY_ADC) * BATTERY_ADC_TO_VOLTAGE;
Expand All @@ -555,29 +602,38 @@ void update() {
}

if (cnt % JOINTS_PUB_PERIOD == 0 && !publish_wheel_states) {
auto dd_wheel_states = dc.getWheelStates();
auto robot_wheel_states = controller->getWheelStates();

wheel_states.stamp = now();
for (size_t i = 0; i < 4; i++) {
wheel_states.position[i] = dd_wheel_states.position[i];
wheel_states.velocity[i] = dd_wheel_states.velocity[i];
wheel_states.torque[i] = dd_wheel_states.torque[i];
wheel_states.pwm_duty_cycle[i] = dd_wheel_states.pwm_duty_cycle[i];
wheel_states.position[i] = robot_wheel_states.position[i];
wheel_states.velocity[i] = robot_wheel_states.velocity[i];
wheel_states.torque[i] = robot_wheel_states.torque[i];
wheel_states.pwm_duty_cycle[i] = robot_wheel_states.pwm_duty_cycle[i];
}

publish_wheel_states = true;
}

if (cnt % ODOM_PUB_PERIOD == 0 && !publish_wheel_odom) {
auto dd_odom = dc.getOdom();

wheel_odom.stamp = now();
wheel_odom.velocity_lin = dd_odom.velocity_lin;
wheel_odom.velocity_ang = dd_odom.velocity_ang;
wheel_odom.pose_x = dd_odom.pose_x;
wheel_odom.pose_y = dd_odom.pose_y;
wheel_odom.pose_yaw = dd_odom.pose_yaw;

auto robot_odom = controller->getOdom();

if (params.mecanum_wheels) {
wheel_odom_mecanum.stamp = now();
wheel_odom_mecanum.velocity_lin_x = robot_odom.velocity_lin_x;
wheel_odom_mecanum.velocity_lin_y = robot_odom.velocity_lin_y;
wheel_odom_mecanum.velocity_ang = robot_odom.velocity_ang;
wheel_odom_mecanum.pose_x = robot_odom.pose_x;
wheel_odom_mecanum.pose_y = robot_odom.pose_y;
wheel_odom_mecanum.pose_yaw = robot_odom.pose_yaw;
} else {
wheel_odom.stamp = now();
wheel_odom.velocity_lin = robot_odom.velocity_lin_x;
wheel_odom.velocity_ang = robot_odom.velocity_ang;
wheel_odom.pose_x = robot_odom.pose_x;
wheel_odom.pose_y = robot_odom.pose_y;
wheel_odom.pose_yaw = robot_odom.pose_yaw;
}
publish_wheel_odom = true;
}

Expand Down
Loading

0 comments on commit b2a0dda

Please sign in to comment.