Skip to content

Commit

Permalink
Add merged odometry to firmware message converter (#9)
Browse files Browse the repository at this point in the history 
* added system of merged odometry to message converter

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

* added required dependencies to package.xml and CMakeLists files

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

* changed service request toi shared ptr

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

* remove usage of spin until future complete from callback method

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

* increase publishing frequency for merged odometry

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

* code format

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

* remove const tag from some of the callback methods

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

* make PI a global variable

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

* fix filling success status in service

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

* add callbacks and multihthreaded executor

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

* working solution

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

* fix missing semicolon

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

---------

Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>
  • Loading branch information
@Bitterisland6
Bitterisland6 authored Apr 23, 2024
1 parent fbdacdd commit f89da43
Show file tree
Hide file tree
Showing 3 changed files with 146 additions and 8 deletions.
3 changes: 3 additions & 0 deletions leo_fw/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(leo_msgs REQUIRED)
find_package(yaml-cpp REQUIRED)

Expand All @@ -26,10 +27,12 @@ ament_target_dependencies(leo_fw
sensor_msgs
leo_msgs
nav_msgs
std_srvs
)
rclcpp_components_register_node(leo_fw
PLUGIN "leo_fw::FirmwareMessageConverter"
EXECUTABLE "firmware_message_converter"
EXECUTOR "MultiThreadedExecutor"
)

ament_python_install_package(${PROJECT_NAME})
Expand Down
2 changes: 2 additions & 0 deletions leo_fw/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,13 +36,15 @@
<build_depend>rclcpp</build_depend>
<build_depend>rclcpp_components</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>yaml-cpp</build_depend>

<exec_depend>leo_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>rclcpp_components</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_srvs</exec_depend>

<!-- Test -->
<test_depend>ament_lint_auto</test_depend>
Expand Down
149 changes: 141 additions & 8 deletions leo_fw/src/firmware_message_converter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include "nav_msgs/msg/odometry.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "std_srvs/srv/trigger.hpp"

#include "leo_msgs/msg/imu.hpp"
#include "leo_msgs/msg/wheel_odom.hpp"
Expand All @@ -39,6 +40,9 @@

using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;

constexpr double PI = 3.141592653;

namespace leo_fw
{
Expand All @@ -57,6 +61,17 @@ class FirmwareMessageConverter : public rclcpp::Node
&FirmwareMessageConverter::set_imu_calibration_callback, this,
std::placeholders::_1, std::placeholders::_2));

client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
reset_odometry_client_ = create_client<std_srvs::srv::Trigger>(
"firmware/reset_odometry",
rmw_qos_profile_services_default,
client_cb_group_);

reset_odometry_service_ = create_service<std_srvs::srv::Trigger>(
"reset_odometry",
std::bind(
&FirmwareMessageConverter::reset_odometry_callback, this, std::placeholders::_1,
std::placeholders::_2));

robot_frame_id_ = declare_parameter("robot_frame_id", robot_frame_id_);
odom_frame_id_ = declare_parameter("odom_frame_id", odom_frame_id_);
Expand Down Expand Up @@ -94,7 +109,7 @@ class FirmwareMessageConverter : public rclcpp::Node
joint_states_pub_->publish(joint_states);
}

void wheel_odom_callback(const leo_msgs::msg::WheelOdom::SharedPtr msg) const
void wheel_odom_callback(const leo_msgs::msg::WheelOdom::SharedPtr msg)
{
nav_msgs::msg::Odometry wheel_odom;
wheel_odom.header.frame_id = odom_frame_id_;
Expand All @@ -107,6 +122,8 @@ class FirmwareMessageConverter : public rclcpp::Node
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;

for (int i = 0; i < 6; i++) {
wheel_odom.twist.covariance[i * 7] =
wheel_odom_twist_covariance_diagonal_[i];
Expand All @@ -115,7 +132,7 @@ class FirmwareMessageConverter : public rclcpp::Node
wheel_odom_pub_->publish(wheel_odom);
}

void mecanum_odom_callback(const leo_msgs::msg::WheelOdomMecanum::SharedPtr msg) const
void mecanum_odom_callback(const leo_msgs::msg::WheelOdomMecanum::SharedPtr msg)
{
nav_msgs::msg::Odometry wheel_odom;
wheel_odom.header.frame_id = odom_frame_id_;
Expand All @@ -129,6 +146,9 @@ class FirmwareMessageConverter : public rclcpp::Node
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];
Expand All @@ -137,7 +157,7 @@ class FirmwareMessageConverter : public rclcpp::Node
wheel_odom_mecanum_pub_->publish(wheel_odom);
}

void imu_callback(const leo_msgs::msg::Imu::SharedPtr msg) const
void imu_callback(const leo_msgs::msg::Imu::SharedPtr msg)
{
sensor_msgs::msg::Imu imu;
imu.header.frame_id = tf_frame_prefix_ + imu_frame_id_;
Expand All @@ -149,6 +169,8 @@ class FirmwareMessageConverter : public rclcpp::Node
imu.linear_acceleration.y = msg->accel_y;
imu.linear_acceleration.z = msg->accel_z;

velocity_angular_z = imu.angular_velocity.z;

for (int i = 0; i < 3; i++) {
imu.angular_velocity_covariance[i * 4] =
imu_angular_velocity_covariance_diagonal_[i];
Expand All @@ -159,6 +181,52 @@ class FirmwareMessageConverter : public rclcpp::Node
imu_pub_->publish(imu);
}

void merged_odometry_callback()
{
nav_msgs::msg::Odometry merged_odom;
merged_odom.header.frame_id = odom_frame_id_;
merged_odom.child_frame_id = tf_frame_prefix_ + robot_frame_id_;
merged_odom.header.stamp = 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;

double move_x = velocity_linear_x * std::cos(odom_merged_yaw) - velocity_linear_y * std::sin(
odom_merged_yaw);
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;

odom_merged_yaw += velocity_angular_z * 0.01;

if (odom_merged_yaw > 2.0 * PI) {
odom_merged_yaw -= 2.0 * PI;
} else if (odom_merged_yaw < 0.0) {
odom_merged_yaw += 2.0 * PI;
}

merged_odom.pose.pose.position.x = odom_merged_position.x;
merged_odom.pose.pose.position.y = odom_merged_position.y;
merged_odom.pose.pose.orientation.z = std::sin(odom_merged_yaw * 0.5F);
merged_odom.pose.pose.orientation.w = std::cos(odom_merged_yaw * 0.5F);

const std::vector<double> * merged_covariance;
if (wheel_odom_mecanum_pub_) {
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);
}

void set_imu_calibration_callback(
const std::shared_ptr<leo_msgs::srv::SetImuCalibration::Request> request,
std::shared_ptr<leo_msgs::srv::SetImuCalibration::Response> response)
Expand All @@ -177,6 +245,37 @@ class FirmwareMessageConverter : public rclcpp::Node
response->success = true;
}

void reset_odometry_callback(
const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
std::shared_ptr<std_srvs::srv::Trigger::Response> res)
{
constexpr std::chrono::seconds callback_timeout = std::chrono::seconds(3);

odom_merged_position.x = 0.0;
odom_merged_position.y = 0.0;
odom_merged_yaw = 0.0;

auto reset_request = std::make_shared<std_srvs::srv::Trigger_Request>();
auto future = reset_odometry_client_->async_send_request(reset_request);
auto result_status = future.wait_for(callback_timeout);

if (result_status == std::future_status::ready) {
if (future.get()->success) {
res->success = true;
res->message = "Odometry reset successful.";
} else {
res->success = false;
res->message = "Failed to reset odometry.";
}
} else if (result_status == std::future_status::timeout) {
res->success = false;
res->message = "Firmware service timeout.";
} else {
res->success = false;
res->message = "Firmware service call deffered.";
}
}

void load_yaml_bias()
{
YAML::Node node;
Expand Down Expand Up @@ -249,9 +348,11 @@ class FirmwareMessageConverter : public rclcpp::Node
if (wheel_odom_pub_ && wheel_odom_publishers == 0) {
RCLCPP_INFO(
get_logger(), "firmware/wheel_odom topic no longer has any publishers. "
"Shutting down wheel_odom_with_covariance publisher.");
"Shutting down wheel_odom_with_covariance and odometry_merged publishers.");
wheel_odom_sub_.reset();
wheel_odom_pub_.reset();
odom_merged_pub_.reset();
odom_merged_timer_->cancel();
}

if (!wheel_odom_pub_ && wheel_odom_publishers > 0) {
Expand All @@ -270,9 +371,11 @@ class FirmwareMessageConverter : public rclcpp::Node
RCLCPP_INFO(
get_logger(),
"firmware/wheel_odom_mecanum topic no longer has any publishers. "
"Shutting down wheel_odom_with_covariance publisher.");
"Shutting down wheel_odom_with_covariance and odometry_merged publishers.");
wheel_odom_mecanum_sub_.reset();
wheel_odom_mecanum_pub_.reset();
odom_merged_pub_.reset();
odom_merged_timer_->cancel();
}

if (!wheel_odom_mecanum_pub_ && wheel_odom_mecanum_publishers > 0) {
Expand All @@ -295,9 +398,11 @@ class FirmwareMessageConverter : public rclcpp::Node
if (imu_pub_ && imu_publishers == 0) {
RCLCPP_INFO(
get_logger(), "firmware/imu topic no longer has any publishers. "
"Shutting down imu/data_raw publisher.");
"Shutting down imu/data_raw and odometry_merged publishers.");
imu_sub_.reset();
imu_pub_.reset();
odom_merged_pub_.reset();
odom_merged_timer_->cancel();
}

if (!imu_pub_ && imu_publishers > 0) {
Expand All @@ -309,8 +414,27 @@ class FirmwareMessageConverter : public rclcpp::Node
imu_topic_, rclcpp::QoS(5).best_effort(),
std::bind(&FirmwareMessageConverter::imu_callback, this, _1));
}

if (imu_pub_ && (wheel_odom_mecanum_pub_ || wheel_odom_pub_) && !odom_merged_pub_) {
RCLCPP_INFO(
get_logger(), "Both firmware/imu and (firmware/wheel_odom or "
"firmware/wheel_odom_mecanum) topics are advertised. "
"Advertising odometry_merged topic.");
odom_merged_pub_ = create_publisher<nav_msgs::msg::Odometry>("odometry_merged", 10);
odom_merged_timer_ =
create_wall_timer(
10ms,
std::bind(&FirmwareMessageConverter::merged_odometry_callback, this));
}
}

// Merged Odom variables
double velocity_linear_x;
double velocity_linear_y;
double velocity_angular_z;
double odom_merged_yaw;
geometry_msgs::msg::Point odom_merged_position;

// Parameters
std::string robot_frame_id_ = "base_link";
std::string odom_frame_id_ = "odom";
Expand All @@ -335,13 +459,15 @@ class FirmwareMessageConverter : public rclcpp::Node
std::string wheel_odom_mecanum_topic_;
std::string imu_topic_;

// Timer
// Timers
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::TimerBase::SharedPtr odom_merged_timer_;

// 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<nav_msgs::msg::Odometry>::SharedPtr odom_merged_pub_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;

// Subscriptions
Expand All @@ -350,8 +476,15 @@ class FirmwareMessageConverter : public rclcpp::Node
rclcpp::Subscription<leo_msgs::msg::WheelOdomMecanum>::SharedPtr wheel_odom_mecanum_sub_;
rclcpp::Subscription<leo_msgs::msg::Imu>::SharedPtr imu_sub_;

// Service
// Services
rclcpp::Service<leo_msgs::srv::SetImuCalibration>::SharedPtr set_imu_calibration_service;
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_odometry_service_;

// Clients
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr reset_odometry_client_;

// Callback Groups
rclcpp::CallbackGroup::SharedPtr client_cb_group_;
};

} // namespace leo_fw
Expand Down

0 comments on commit f89da43

Please sign in to comment.