Skip to content

Commit

Permalink
Add merged odometry to firmware_message_converter (#11)
Browse files Browse the repository at this point in the history 
* Added topic with odometry using wheel odom and imu data

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

* added mecanum parameters and odom message header

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

* added reset_odometry service

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

* include imu bias in merged_odometry

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

* fix bias addition problems

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

* remove redundant parameters

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

* fix usage of non existing fields

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

* include firmware/wheel_odom_mecanum topic handling in message converter

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

* add parameters regarding firmware/wheel_odom_mecanum topic to config

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

* remove mecanum part of the merged odom

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

* remove usage of undeclared variables

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

* format code

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

* remove mecanum parameters from firmware message converter config file

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

* implement code review guidelines

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

* removed float literals in double variables

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

* remove some more float literals

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

* changed variables to doubles

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 2, 2023
1 parent fb06fce commit f6fc6ee
Showing 1 changed file with 87 additions and 2 deletions.
89 changes: 87 additions & 2 deletions leo_fw/src/firmware_message_converter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,13 +9,16 @@
#include "nav_msgs/Odometry.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/JointState.h"
#include "std_srvs/Trigger.h"

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

#include "leo_msgs/SetImuCalibration.h"

constexpr double PI = 3.141592653;

static ros::Subscriber wheel_states_sub;
static ros::Publisher joint_states_pub;
static bool joint_states_advertised = false;
Expand All @@ -28,6 +31,16 @@ static ros::Subscriber imu_sub;
static ros::Publisher imu_pub;
static bool imu_advertised = false;

static ros::Publisher odom_merged_pub;
static ros::Timer odom_merged_timer;
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_angular_z = 0;
static ros::ServiceClient odom_reset_client;
static ros::ServiceServer reset_odometry_service;

ros::ServiceServer set_imu_calibration_service;

std::string robot_frame_id = "base_link";
Expand All @@ -37,6 +50,7 @@ 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> imu_angular_velocity_covariance_diagonal = {
0.000001, 0.000001, 0.00001};
std::vector<double> imu_linear_acceleration_covariance_diagonal = {0.001, 0.001,
Expand Down Expand Up @@ -90,12 +104,15 @@ void wheel_odom_callback(const leo_msgs::WheelOdomPtr& msg) {
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];

wheel_odom_pub.publish(wheel_odom);
}

void imu_callback(const leo_msgs::ImuPtr& msg) {
sensor_msgs::Imu imu;
imu.header.frame_id = imu_frame_id;
Expand All @@ -107,6 +124,8 @@ void imu_callback(const leo_msgs::ImuPtr& msg) {
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 @@ -117,6 +136,35 @@ void imu_callback(const leo_msgs::ImuPtr& msg) {
imu_pub.publish(imu);
}

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.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);

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.5);
merged_odom.pose.pose.orientation.w = std::cos(odom_merged_yaw * 0.5);

odom_merged_pub.publish(merged_odom);
}

void load_yaml_bias() {
YAML::Node node;
try {
Expand Down Expand Up @@ -173,6 +221,20 @@ bool set_imu_calibration_callback(leo_msgs::SetImuCalibrationRequest& req,
return true;
}

bool reset_odometry_callback(std_srvs::TriggerRequest& req,
std_srvs::TriggerResponse& res) {
odom_merged_position.x = 0.0;
odom_merged_position.y = 0.0;
odom_merged_yaw = 0.0;
if (odom_reset_client.call(req, res)) {
res.success = true;
return true;
} else {
res.success = false;
return false;
}
}

int main(int argc, char** argv) {
ros::init(argc, argv, "firmware_message_converter");

Expand All @@ -190,10 +252,16 @@ int main(int argc, char** argv) {
ros::names::resolve("firmware/wheel_states");
const std::string wheel_odom_topic =
ros::names::resolve("firmware/wheel_odom");
const std::string wheel_odom_mecanum_topic =
ros::names::resolve("firmware/wheel_odom_mecanum");
const std::string imu_topic = ros::names::resolve("firmware/imu");

set_imu_calibration_service =
nh.advertiseService("set_imu_calibration", set_imu_calibration_callback);
odom_reset_client =
nh.serviceClient<std_srvs::Trigger>("firmware/reset_odometry");
reset_odometry_service =
nh.advertiseService("reset_odometry", &reset_odometry_callback);

ros::Rate rate(2);
while (ros::ok()) {
Expand All @@ -209,18 +277,25 @@ int main(int argc, char** argv) {
if (wheel_odom_advertised && wheel_odom_sub.getNumPublishers() == 0) {
ROS_INFO(
"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.shutdown();
wheel_odom_pub.shutdown();
odom_merged_pub.shutdown();
odom_merged_timer.stop();
wheel_odom_advertised = false;
odom_merged_advertised = false;
}
if (imu_advertised && imu_sub.getNumPublishers() == 0) {
ROS_INFO(
"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.shutdown();
imu_pub.shutdown();
odom_merged_pub.shutdown();
odom_merged_timer.stop();
imu_advertised = false;
odom_merged_advertised = false;
}

rate.sleep();
Expand Down Expand Up @@ -258,6 +333,16 @@ 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) {
ROS_INFO(
"Both firmware/imu and firmware/wheel_odom topics are advertised. "
"Advertising odometry_merged topic.");
odom_merged_pub =
nh.advertise<nav_msgs::Odometry>("odometry_merged", 10);
odom_merged_timer =
nh.createTimer(ros::Duration(0.01), merge_odometry_callback);
odom_merged_advertised = true;
}
}

rate.sleep();
Expand Down

0 comments on commit f6fc6ee

Please sign in to comment.