Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Mecanum Wheel Odometry in firmware_message_converter #5

Merged
merged 8 commits into from
Oct 16, 2023
Merged

Mecanum Wheel Odometry in firmware_message_converter #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
8cafdfa
handling wheel odometry from mecanum wheels in firmware message conv…
Bitterisland6 Sep 18, 2023
b203aed
add mecanum parameters to converter config
Bitterisland6 Sep 19, 2023
8c1ca43
fix typo
Bitterisland6 Sep 19, 2023
053f483
cherry-pick firmware.yaml file from other branch
Bitterisland6 Aug 28, 2023
9f614fc
add mecanum_wheels param to firmware.yaml file
Bitterisland6 Sep 19, 2023
3d2ad63
added separate publisher for mecanum odom
Bitterisland6 Sep 21, 2023
8a33df7
fixed usage of bad publisher
Bitterisland6 Sep 21, 2023
5caa6c9
removed unnecesarry added files
Bitterisland6 Sep 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions leo_bringup/config/firmware_message_converter.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,6 @@
[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]
58 changes: 58 additions & 0 deletions leo_fw/src/firmware_message_converter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@

#include "leo_msgs/msg/imu.hpp"
#include "leo_msgs/msg/wheel_odom.hpp"
#include "leo_msgs/msg/wheel_odom_mecanum.hpp"
#include "leo_msgs/msg/wheel_states.hpp"

#include "leo_msgs/srv/set_imu_calibration.hpp"
Expand Down Expand Up @@ -63,6 +64,9 @@ class FirmwareMessageConverter : public rclcpp::Node
wheel_joint_names_ = declare_parameter("wheel_joint_names", wheel_joint_names_);
wheel_odom_twist_covariance_diagonal_ = declare_parameter(
"wheel_odom_twist_covariance_diagonal", wheel_odom_twist_covariance_diagonal_);
wheel_odom_mecanum_twist_covariance_diagonal_ = declare_parameter(
"wheel_odom_mecanum_twist_covariance_diagonal",
wheel_odom_mecanum_twist_covariance_diagonal_);
imu_angular_velocity_covariance_diagonal_ = declare_parameter(
"imu_angular_velocity_covariance_diagonal", imu_angular_velocity_covariance_diagonal_);
imu_linear_acceleration_covariance_diagonal_ = declare_parameter(
Expand All @@ -72,6 +76,7 @@ class FirmwareMessageConverter : public rclcpp::Node
auto node_topics = get_node_topics_interface();
wheel_states_topic_ = node_topics->resolve_topic_name("firmware/wheel_states");
wheel_odom_topic_ = node_topics->resolve_topic_name("firmware/wheel_odom");
wheel_odom_mecanum_topic_ = node_topics->resolve_topic_name("firmware/wheel_odom_mecanum");
imu_topic_ = node_topics->resolve_topic_name("firmware/imu");

timer_ = create_wall_timer(500ms, std::bind(&FirmwareMessageConverter::timer_callback, this));
Expand Down Expand Up @@ -110,6 +115,28 @@ class FirmwareMessageConverter : public rclcpp::Node
wheel_odom_pub_->publish(wheel_odom);
}

void mecanum_odom_callback(const leo_msgs::msg::WheelOdomMecanum::SharedPtr msg) const
{
nav_msgs::msg::Odometry wheel_odom;
wheel_odom.header.frame_id = odom_frame_id_;
wheel_odom.child_frame_id = tf_frame_prefix_ + 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);

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

wheel_odom_mecanum_pub_->publish(wheel_odom);
}

void imu_callback(const leo_msgs::msg::Imu::SharedPtr msg) const
{
sensor_msgs::msg::Imu imu;
Expand Down Expand Up @@ -237,6 +264,32 @@ class FirmwareMessageConverter : public rclcpp::Node
std::bind(&FirmwareMessageConverter::wheel_odom_callback, this, _1));
}

size_t wheel_odom_mecanum_publishers = count_publishers(wheel_odom_mecanum_topic_);

if (wheel_odom_mecanum_pub_ && wheel_odom_mecanum_publishers == 0) {
RCLCPP_INFO(
get_logger(),
"firmware/wheel_odom_mecanum topic no longer has any publishers. "
"Shutting down wheel_odom_with_covariance publisher.");
wheel_odom_mecanum_sub_.reset();
wheel_odom_mecanum_pub_.reset();
}

if (!wheel_odom_mecanum_pub_ && wheel_odom_mecanum_publishers > 0) {
RCLCPP_INFO(
get_logger(),
"Detected a publisher on firmware/wheel_odom_mecanum topic. "
"Starting publishing on wheel_odom_with_covariance topic.");
wheel_odom_mecanum_pub_ = create_publisher<nav_msgs::msg::Odometry>(
"wheel_odom_with_covariance", 10);
wheel_odom_mecanum_sub_ =
create_subscription<leo_msgs::msg::WheelOdomMecanum>(
wheel_odom_mecanum_topic_, rclcpp::QoS(5).best_effort(),
std::bind(
&FirmwareMessageConverter::mecanum_odom_callback, this,
_1));
}

size_t imu_publishers = count_publishers(imu_topic_);

if (imu_pub_ && imu_publishers == 0) {
Expand Down Expand Up @@ -266,6 +319,8 @@ class FirmwareMessageConverter : public rclcpp::Node
"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};
std::vector<double> imu_linear_acceleration_covariance_diagonal_ = {0.001, 0.001,
Expand All @@ -277,6 +332,7 @@ class FirmwareMessageConverter : public rclcpp::Node
// Topic names
std::string wheel_states_topic_;
std::string wheel_odom_topic_;
std::string wheel_odom_mecanum_topic_;
std::string imu_topic_;

// Timer
Expand All @@ -285,11 +341,13 @@ class FirmwareMessageConverter : public rclcpp::Node
// Publishers
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_states_pub_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr wheel_odom_pub_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr wheel_odom_mecanum_pub_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;

// Subscriptions
rclcpp::Subscription<leo_msgs::msg::WheelStates>::SharedPtr wheel_states_sub_;
rclcpp::Subscription<leo_msgs::msg::WheelOdom>::SharedPtr wheel_odom_sub_;
rclcpp::Subscription<leo_msgs::msg::WheelOdomMecanum>::SharedPtr wheel_odom_mecanum_sub_;
rclcpp::Subscription<leo_msgs::msg::Imu>::SharedPtr imu_sub_;

// Service
Expand Down
Loading