Skip to content

Commit

Permalink
handling wheel odometry from mecanum wheels in firmware message conv…
Browse files Browse the repository at this point in the history 
…erter

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>
  • Loading branch information
@Bitterisland6
Bitterisland6 committed Sep 18, 2023
1 parent a4c19a3 commit 8cafdfa
Showing 1 changed file with 67 additions and 2 deletions.
69 changes: 67 additions & 2 deletions leo_fw/src/firmware_message_converter.cpp
View file
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_pub_->publish(wheel_odom);
}

void imu_callback(const leo_msgs::msg::Imu::SharedPtr msg) const
{
sensor_msgs::msg::Imu imu;
Expand Down Expand Up @@ -219,22 +246,52 @@ class FirmwareMessageConverter : public rclcpp::Node

size_t wheel_odom_publishers = count_publishers(wheel_odom_topic_);

if (wheel_odom_pub_ && wheel_odom_publishers == 0) {
if (wheel_odom_advertised && wheel_odom_publishers == 0) {
RCLCPP_INFO(
get_logger(), "firmware/wheel_odom topic no longer has any publishers. "
"Shutting down wheel_odom_with_covariance publisher.");
wheel_odom_sub_.reset();
wheel_odom_pub_.reset();
wheek_odom_advertised = false;
}

if (!wheel_odom_pub_ && wheel_odom_publishers > 0) {
if (!wheel_odom_advertised && wheel_odom_publishers > 0) {
RCLCPP_INFO(
get_logger(), "Detected a publisher on firmware/wheel_odom topic. "
"Starting publishing on wheel_odom_with_covariance topic.");
wheel_odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("wheel_odom_with_covariance", 10);
wheel_odom_sub_ = create_subscription<leo_msgs::msg::WheelOdom>(
wheel_odom_topic_, rclcpp::QoS(5).best_effort(),
std::bind(&FirmwareMessageConverter::wheel_odom_callback, this, _1));
wheel_odom_advertised = true;
}

size_t wheel_odom_mecanum_publishers = count_publishers(wheel_odom_mecanum_topic_);

if (wheel_odom_mecanum_advertised && 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_pub_.reset();
wheel_odom_mecanum_advertised = false;
}

if (!wheel_odom_mecanum_advertised && 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_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));
wheel_odom_mecanum_advertised = true;
}

size_t imu_publishers = count_publishers(imu_topic_);
Expand Down Expand Up @@ -266,6 +323,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 +336,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 @@ -290,10 +350,15 @@ class FirmwareMessageConverter : public rclcpp::Node
// 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
rclcpp::Service<leo_msgs::srv::SetImuCalibration>::SharedPtr set_imu_calibration_service;

// Flags
bool wheel_odom_advertised;
bool wheel_odom_mecanum_advertised;
};

} // namespace leo_fw
Expand Down

0 comments on commit 8cafdfa

Please sign in to comment.