Skip to content

Commit

Permalink
Mecanum wheels handling in merged odometry (#10)
Browse files Browse the repository at this point in the history 
* added parameters required for mecanum wheels

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>

* upgrade of firmware message converter to use linear valocity y

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>

* Revert "upgrade of firmware message converter to use linear valocity y"

This reverts commit f51b6a2.

* add mecanum parameters to firmware message converter config file

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* Added handling of odometry from mecanum wheels in merged odom

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* Change parameter name

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* updated variable names

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* advertising odometry_merged topic for mecanum odom

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* include covariance matrices in merged_odometry

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* update parameter values in leo_bringup package

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

* format code

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>

---------

Signed-off-by: Bitterisland6 <bitterisland6@gmail.com>
Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>
  • Loading branch information
@Bitterisland6
Bitterisland6 authored Aug 7, 2023
1 parent 4989c1c commit 6111162
Show file tree
Hide file tree
Showing 3 changed files with 89 additions and 6 deletions.
13 changes: 11 additions & 2 deletions leo_bringup/config/firmware.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,19 @@ firmware:
pid: { p: 0.0, i: 0.005, d: 0.0 }
pwm_duty_limit: 100.0

mecanum_wheels: false

diff_drive:
wheel_radius: 0.0625
wheel_separation: 0.33
angular_velocity_multiplier: 1.91
wheel_separation: 0.358
angular_velocity_multiplier: 1.76
input_timeout: 500

mecanum_drive:
wheel_radius: 0.0635
wheel_separation: 0.37
wheel_base: 0.3052
angular_velocity_multiplier: 1.0
input_timeout: 500

battery_min_voltage: 10.0
Expand Down
1 change: 1 addition & 0 deletions leo_bringup/config/firmware_message_converter.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,5 +8,6 @@ wheel_joint_names:
[wheel_FL_joint, wheel_RL_joint, wheel_FR_joint, wheel_RR_joint]

wheel_odom_twist_covariance_diagonal: [0.0001, 0.0, 0.0, 0.0, 0.0, 0.001]
wheel_odom_mecanum_twist_covariance_diagonal: [0.0001, 0.0001, 0.0, 0.0, 0.0, 0.001]
imu_angular_velocity_covariance_diagonal: [0.000001, 0.000001, 0.00001]
imu_linear_acceleration_covariance_diagonal: [0.001, 0.001, 0.001]
81 changes: 77 additions & 4 deletions leo_fw/src/firmware_message_converter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@

#include "leo_msgs/Imu.h"
#include "leo_msgs/WheelOdom.h"
#include "leo_msgs/WheelOdomMecanum.h"
#include "leo_msgs/WheelStates.h"

#include "leo_msgs/SetImuCalibration.h"
Expand All @@ -27,6 +28,9 @@ static ros::Subscriber wheel_odom_sub;
static ros::Publisher wheel_odom_pub;
static bool wheel_odom_advertised = false;

static bool wheel_odom_mecanum_advertised = false;
static ros::Subscriber wheel_odom_mecanum_sub;

static ros::Subscriber imu_sub;
static ros::Publisher imu_pub;
static bool imu_advertised = false;
Expand All @@ -37,6 +41,7 @@ static geometry_msgs::Point odom_merged_position;
static double odom_merged_yaw;
static bool odom_merged_advertised = false;
static double velocity_linear_x = 0;
static double velocity_linear_y = 0;
static double velocity_angular_z = 0;
static ros::ServiceClient odom_reset_client;
static ros::ServiceServer reset_odometry_service;
Expand All @@ -50,6 +55,8 @@ std::vector<std::string> wheel_joint_names = {
"wheel_FL_joint", "wheel_RL_joint", "wheel_FR_joint", "wheel_RR_joint"};
std::vector<double> wheel_odom_twist_covariance_diagonal = {0.0001, 0.0, 0.0,
0.0, 0.0, 0.001};
std::vector<double> wheel_odom_mecanum_twist_covariance_diagonal = {
0.0001, 0.0001, 0.0, 0.0, 0.0, 0.001};

std::vector<double> imu_angular_velocity_covariance_diagonal = {
0.000001, 0.000001, 0.00001};
Expand All @@ -67,6 +74,8 @@ void load_parameters(ros::NodeHandle& pnh) {
pnh.getParam("wheel_joint_names", wheel_joint_names);
pnh.getParam("wheel_odom_twist_covariance_diagonal",
wheel_odom_twist_covariance_diagonal);
pnh.getParam("wheel_odom_mecanum_twist_covariance_diagonal",
wheel_odom_mecanum_twist_covariance_diagonal);
pnh.getParam("imu_angular_velocity_covariance_diagonal",
imu_angular_velocity_covariance_diagonal);
pnh.getParam("imu_linear_acceleration_covariance_diagonal",
Expand Down Expand Up @@ -136,16 +145,42 @@ void imu_callback(const leo_msgs::ImuPtr& msg) {
imu_pub.publish(imu);
}

void mecanum_odom_callback(const leo_msgs::WheelOdomMecanumPtr& msg) {
nav_msgs::Odometry wheel_odom;
wheel_odom.header.frame_id = odom_frame_id;
wheel_odom.child_frame_id = robot_frame_id;
wheel_odom.header.stamp = msg->stamp;
wheel_odom.twist.twist.linear.x = msg->velocity_lin_x;
wheel_odom.twist.twist.linear.y = msg->velocity_lin_y;
wheel_odom.twist.twist.angular.z = msg->velocity_ang;
wheel_odom.pose.pose.position.x = msg->pose_x;
wheel_odom.pose.pose.position.y = msg->pose_y;
wheel_odom.pose.pose.orientation.z = std::sin(msg->pose_yaw * 0.5F);
wheel_odom.pose.pose.orientation.w = std::cos(msg->pose_yaw * 0.5F);

velocity_linear_x = msg->velocity_lin_x;
velocity_linear_y = msg->velocity_lin_y;

for (int i = 0; i < 6; i++)
wheel_odom.twist.covariance[i * 7] =
wheel_odom_mecanum_twist_covariance_diagonal[i];

wheel_odom_pub.publish(wheel_odom);
}

void merge_odometry_callback(const ros::TimerEvent& events) {
nav_msgs::Odometry merged_odom;
merged_odom.header.frame_id = odom_frame_id;
merged_odom.child_frame_id = robot_frame_id;
merged_odom.header.stamp = ros::Time::now();
merged_odom.twist.twist.linear.x = velocity_linear_x;
merged_odom.twist.twist.linear.y = velocity_linear_y;
merged_odom.twist.twist.angular.z = velocity_angular_z;

const double move_x = velocity_linear_x * std::cos(odom_merged_yaw);
const double move_y = velocity_linear_x * std::sin(odom_merged_yaw);
const double move_x = velocity_linear_x * std::cos(odom_merged_yaw) -
velocity_linear_y * std::sin(odom_merged_yaw);
const double move_y = velocity_linear_x * std::sin(odom_merged_yaw) +
velocity_linear_y * std::cos(odom_merged_yaw);

odom_merged_position.x += move_x * 0.01;
odom_merged_position.y += move_y * 0.01;
Expand All @@ -162,6 +197,18 @@ void merge_odometry_callback(const ros::TimerEvent& events) {
merged_odom.pose.pose.orientation.z = std::sin(odom_merged_yaw * 0.5);
merged_odom.pose.pose.orientation.w = std::cos(odom_merged_yaw * 0.5);

std::vector<double>* merged_covariance;

if (wheel_odom_mecanum_advertised)
merged_covariance = &wheel_odom_mecanum_twist_covariance_diagonal;
else
merged_covariance = &wheel_odom_twist_covariance_diagonal;

for (int i = 0; i < 5; i++)
merged_odom.twist.covariance[i * 7] = (*merged_covariance)[i];
merged_odom.twist.covariance[35] =
imu_angular_velocity_covariance_diagonal[2];

odom_merged_pub.publish(merged_odom);
}

Expand Down Expand Up @@ -286,6 +333,19 @@ int main(int argc, char** argv) {
wheel_odom_advertised = false;
odom_merged_advertised = false;
}
if (wheel_odom_mecanum_advertised &&
wheel_odom_mecanum_sub.getNumPublishers() == 0) {
ROS_INFO(
"firmware/wheel_odom_mecanum topic no longer has any publishers. "
"Shutting down wheel_odom_with_covariance and odometry_merged "
"publishers.");
wheel_odom_mecanum_sub.shutdown();
wheel_odom_pub.shutdown();
odom_merged_pub.shutdown();
odom_merged_timer.stop();
wheel_odom_mecanum_advertised = false;
odom_merged_advertised = false;
}
if (imu_advertised && imu_sub.getNumPublishers() == 0) {
ROS_INFO(
"firmware/imu topic no longer has any publishers. "
Expand Down Expand Up @@ -325,6 +385,17 @@ int main(int argc, char** argv) {
nh.subscribe("firmware/wheel_odom", 5, wheel_odom_callback);
wheel_odom_advertised = true;
}
if (!wheel_odom_mecanum_advertised &&
topic.name == wheel_odom_mecanum_topic) {
ROS_INFO(
"Detected firmware/wheel_odom_mecanum topic advertised. "
"Advertising wheel_odom_with_covariance topic.");
wheel_odom_pub =
nh.advertise<nav_msgs::Odometry>("wheel_odom_with_covariance", 10);
wheel_odom_mecanum_sub = nh.subscribe("firmware/wheel_odom_mecanum", 5,
mecanum_odom_callback);
wheel_odom_mecanum_advertised = true;
}
if (!imu_advertised && topic.name == imu_topic) {
ROS_INFO(
"Detected firmware/imu topic advertised. "
Expand All @@ -333,9 +404,11 @@ int main(int argc, char** argv) {
imu_sub = nh.subscribe("firmware/imu", 5, imu_callback);
imu_advertised = true;
}
if (!odom_merged_advertised && imu_advertised && wheel_odom_advertised) {
if (!odom_merged_advertised && imu_advertised &&
(wheel_odom_advertised || wheel_odom_mecanum_advertised)) {
ROS_INFO(
"Both firmware/imu and firmware/wheel_odom topics are advertised. "
"Both firmware/imu and (firmware/wheel_odom or "
"firmware/wheel_odom_mecanum) topics are advertised. "
"Advertising odometry_merged topic.");
odom_merged_pub =
nh.advertise<nav_msgs::Odometry>("odometry_merged", 10);
Expand Down

0 comments on commit 6111162

Please sign in to comment.