From d57b5b7d8e133905abef9c38365a5489be4066f5 Mon Sep 17 00:00:00 2001 From: Bitterisland6 Date: Mon, 3 Jul 2023 13:59:16 +0200 Subject: [PATCH 01/17] Added topic with odometry using wheel odom and imu data Signed-off-by: Bitterisland6 --- leo_fw/src/firmware_message_converter.cpp | 65 +++++++++++++++++++++-- 1 file changed, 62 insertions(+), 3 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index d016a73..339a547 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -28,6 +28,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 float odom_merged_yaw; +static bool odom_merged_advertised = false; +static float velocity_linear_x = 0; +static float velocity_linear_y = 0; +static float velocity_angular_z = 0; +static constexpr float PI = 3.141592653F; + ros::ServiceServer set_imu_calibration_service; std::string robot_frame_id = "base_link"; @@ -83,13 +93,17 @@ void wheel_odom_callback(const leo_msgs::WheelOdomPtr& msg) { 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; + 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_twist_covariance_diagonal[i]; @@ -107,6 +121,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]; @@ -117,6 +133,34 @@ 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.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 float move_x = velocity_linear_x * std::cos(odom_merged_yaw) - velocity_linear_y * std::sin(odom_merged_yaw); + const float 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.0F * PI) + odom_merged_yaw -= 2.0F * PI; + else if(odom_merged_yaw < 0.0F) + odom_merged_yaw += 2.0F * 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); + + odom_merged_pub.publish(merged_odom); +} + void load_yaml_bias() { YAML::Node node; try { @@ -209,18 +253,24 @@ 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(); @@ -258,6 +308,15 @@ 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("odometry_merged", 10); + odom_merged_timer = nh.createTimer(ros::Duration(0.01), merge_odometry_callback); + odom_merged_advertised = true; + } } rate.sleep(); From 44d83411589aaa546cae46d918de6ebbd839c83b Mon Sep 17 00:00:00 2001 From: Bitterisland6 Date: Mon, 3 Jul 2023 16:11:41 +0200 Subject: [PATCH 02/17] added mecanum parameters and odom message header Signed-off-by: Bitterisland6 --- leo_bringup/config/firmware.yaml | 3 +++ leo_fw/src/firmware_message_converter.cpp | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/leo_bringup/config/firmware.yaml b/leo_bringup/config/firmware.yaml index 4cc125b..d792c7e 100644 --- a/leo_bringup/config/firmware.yaml +++ b/leo_bringup/config/firmware.yaml @@ -10,8 +10,11 @@ firmware: diff_drive: wheel_radius: 0.0625 wheel_separation: 0.33 + wheel_base: 0.3052 angular_velocity_multiplier: 1.91 input_timeout: 500 + + mecanum_wheels: true battery_min_voltage: 10.0 diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 339a547..c1780c6 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -135,7 +135,9 @@ void imu_callback(const leo_msgs::ImuPtr& msg) { 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; From 0a0f2f6d6459618b776f0e54502dd7ffadfd5cca Mon Sep 17 00:00:00 2001 From: Bitterisland6 Date: Tue, 4 Jul 2023 15:13:15 +0200 Subject: [PATCH 03/17] added reset_odometry service Signed-off-by: Bitterisland6 --- leo_fw/src/firmware_message_converter.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index c1780c6..97c7ee5 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -9,6 +9,7 @@ #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" @@ -36,6 +37,8 @@ static bool odom_merged_advertised = false; static float velocity_linear_x = 0; static float velocity_linear_y = 0; static float velocity_angular_z = 0; +ros::ServiceClient odom_reset_client; +ros::ServiceServer reset_odometry_service; static constexpr float PI = 3.141592653F; ros::ServiceServer set_imu_calibration_service; @@ -219,6 +222,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.0F; + odom_merged_position.y = 0.0F; + odom_merged_yaw = 0.0F; + 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"); @@ -240,6 +257,8 @@ int main(int argc, char** argv) { set_imu_calibration_service = nh.advertiseService("set_imu_calibration", set_imu_calibration_callback); + odom_reset_client = nh.serviceClient("firmware/reset_odometry"); + reset_odometry_service = nh.advertiseService("reset_odometry", &reset_odometry_callback); ros::Rate rate(2); while (ros::ok()) { From d5c309e5853b0f0078d1bebbdfbd423c6f8eee47 Mon Sep 17 00:00:00 2001 From: Bitterisland6 Date: Tue, 4 Jul 2023 15:25:43 +0200 Subject: [PATCH 04/17] include imu bias in merged_odometry Signed-off-by: Bitterisland6 --- leo_fw/src/firmware_message_converter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 97c7ee5..79bf4c0 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -124,7 +124,7 @@ 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; + velocity_angular_z = imu.angular_velocity.z + imu_calibration_bias[2]; for (int i = 0; i < 3; i++) { imu.angular_velocity_covariance[i * 4] = From bde8c34a76fd9cc93c4a8075c3a25d290694b1ca Mon Sep 17 00:00:00 2001 From: Bitterisland6 Date: Tue, 4 Jul 2023 16:13:38 +0200 Subject: [PATCH 05/17] fix bias addition problems Signed-off-by: Bitterisland6 --- leo_fw/src/firmware_message_converter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 79bf4c0..97c7ee5 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -124,7 +124,7 @@ 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 + imu_calibration_bias[2]; + velocity_angular_z = imu.angular_velocity.z; for (int i = 0; i < 3; i++) { imu.angular_velocity_covariance[i * 4] = From 9533d17b4b94ea053eefc08a05101c769b1fc3db Mon Sep 17 00:00:00 2001 From: Bitterisland6 Date: Wed, 5 Jul 2023 16:17:30 +0200 Subject: [PATCH 06/17] remove redundant parameters Signed-off-by: Bitterisland6 --- leo_bringup/config/firmware.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/leo_bringup/config/firmware.yaml b/leo_bringup/config/firmware.yaml index d792c7e..4cc125b 100644 --- a/leo_bringup/config/firmware.yaml +++ b/leo_bringup/config/firmware.yaml @@ -10,11 +10,8 @@ firmware: diff_drive: wheel_radius: 0.0625 wheel_separation: 0.33 - wheel_base: 0.3052 angular_velocity_multiplier: 1.91 input_timeout: 500 - - mecanum_wheels: true battery_min_voltage: 10.0 From 7a8a053c0a503143db41654696101ab22300708a Mon Sep 17 00:00:00 2001 From: Bitterisland6 Date: Wed, 5 Jul 2023 18:09:20 +0200 Subject: [PATCH 07/17] fix usage of non existing fields Signed-off-by: Bitterisland6 --- leo_fw/src/firmware_message_converter.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 97c7ee5..1e0f34e 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -96,8 +96,7 @@ void wheel_odom_callback(const leo_msgs::WheelOdomPtr& msg) { 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.linear.x = msg->velocity_lin; 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; From da7486259c4e95558e438bdbed764c4ff3f4323a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Fri, 7 Jul 2023 18:11:04 +0200 Subject: [PATCH 08/17] include firmware/wheel_odom_mecanum topic handling in message converter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/src/firmware_message_converter.cpp | 59 +++++++++++++++++++++-- 1 file changed, 56 insertions(+), 3 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 1e0f34e..68c1f19 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -14,6 +14,7 @@ #include "leo_msgs/Imu.h" #include "leo_msgs/WheelOdom.h" #include "leo_msgs/WheelStates.h" +#include "leo_msgs/MecanumOdom.h" #include "leo_msgs/SetImuCalibration.h" @@ -24,6 +25,8 @@ static bool joint_states_advertised = false; 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; @@ -50,6 +53,9 @@ std::vector wheel_joint_names = { "wheel_FL_joint", "wheel_RL_joint", "wheel_FR_joint", "wheel_RR_joint"}; std::vector wheel_odom_twist_covariance_diagonal = {0.0001, 0.0, 0.0, 0.0, 0.0, 0.001}; +std::vector mecanum_wheel_odom_twist_covariance_diagonal = {0.0001, 0.0001, 0.0, + 0.0, 0.0, 0.001}; + std::vector imu_angular_velocity_covariance_diagonal = { 0.000001, 0.000001, 0.00001}; std::vector imu_linear_acceleration_covariance_diagonal = {0.001, 0.001, @@ -66,6 +72,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("mecanum_wheel_odom_twist_covariance_diagonal", + mecanum_wheel_odom_twist_covariance_diagonal); pnh.getParam("imu_angular_velocity_covariance_diagonal", imu_angular_velocity_covariance_diagonal); pnh.getParam("imu_linear_acceleration_covariance_diagonal", @@ -103,8 +111,7 @@ 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_x; - velocity_linear_y = msg->velocity_lin_y; + velocity_linear_x = msg->velocity_lin; for (int i = 0; i < 6; i++) wheel_odom.twist.covariance[i * 7] = @@ -112,6 +119,7 @@ void wheel_odom_callback(const leo_msgs::WheelOdomPtr& msg) { 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; @@ -135,6 +143,29 @@ void imu_callback(const leo_msgs::ImuPtr& msg) { imu_pub.publish(imu); } +void mecanum_odom_callback(const leo_msgs::MecanumOdomPtr& 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] = + mecanum_wheel_odom_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; @@ -252,6 +283,8 @@ 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 = @@ -281,6 +314,17 @@ 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. " @@ -320,6 +364,16 @@ 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("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. " @@ -328,7 +382,6 @@ 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. " From 7fb70e7014c4c85a62974f1bd74983ddca962756 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Fri, 7 Jul 2023 18:12:01 +0200 Subject: [PATCH 09/17] add parameters regarding firmware/wheel_odom_mecanum topic to config MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_bringup/config/firmware_message_converter.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/leo_bringup/config/firmware_message_converter.yaml b/leo_bringup/config/firmware_message_converter.yaml index 49a6271..6f10733 100644 --- a/leo_bringup/config/firmware_message_converter.yaml +++ b/leo_bringup/config/firmware_message_converter.yaml @@ -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] +mecanum_wheel_odom_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] From 9d22814e32ddd12fdcbd1c7bfe500ddad81e1a41 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 19 Jul 2023 12:16:09 +0200 Subject: [PATCH 10/17] remove mecanum part of the merged odom MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/src/firmware_message_converter.cpp | 53 ----------------------- 1 file changed, 53 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 68c1f19..47991b9 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -14,7 +14,6 @@ #include "leo_msgs/Imu.h" #include "leo_msgs/WheelOdom.h" #include "leo_msgs/WheelStates.h" -#include "leo_msgs/MecanumOdom.h" #include "leo_msgs/SetImuCalibration.h" @@ -25,8 +24,6 @@ static bool joint_states_advertised = false; 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; @@ -38,7 +35,6 @@ static geometry_msgs::Point odom_merged_position; static float odom_merged_yaw; static bool odom_merged_advertised = false; static float velocity_linear_x = 0; -static float velocity_linear_y = 0; static float velocity_angular_z = 0; ros::ServiceClient odom_reset_client; ros::ServiceServer reset_odometry_service; @@ -53,8 +49,6 @@ std::vector wheel_joint_names = { "wheel_FL_joint", "wheel_RL_joint", "wheel_FR_joint", "wheel_RR_joint"}; std::vector wheel_odom_twist_covariance_diagonal = {0.0001, 0.0, 0.0, 0.0, 0.0, 0.001}; -std::vector mecanum_wheel_odom_twist_covariance_diagonal = {0.0001, 0.0001, 0.0, - 0.0, 0.0, 0.001}; std::vector imu_angular_velocity_covariance_diagonal = { 0.000001, 0.000001, 0.00001}; @@ -72,8 +66,6 @@ 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("mecanum_wheel_odom_twist_covariance_diagonal", - mecanum_wheel_odom_twist_covariance_diagonal); pnh.getParam("imu_angular_velocity_covariance_diagonal", imu_angular_velocity_covariance_diagonal); pnh.getParam("imu_linear_acceleration_covariance_diagonal", @@ -143,36 +135,12 @@ void imu_callback(const leo_msgs::ImuPtr& msg) { imu_pub.publish(imu); } -void mecanum_odom_callback(const leo_msgs::MecanumOdomPtr& 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] = - mecanum_wheel_odom_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 float move_x = velocity_linear_x * std::cos(odom_merged_yaw) - velocity_linear_y * std::sin(odom_merged_yaw); @@ -314,17 +282,6 @@ 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. " @@ -364,16 +321,6 @@ 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("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. " From eaa90a75c1194ab2b774a905e3e6ef1686dd8373 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 19 Jul 2023 12:22:45 +0200 Subject: [PATCH 11/17] remove usage of undeclared variables MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/src/firmware_message_converter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 47991b9..b40dbf3 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -143,8 +143,8 @@ void merge_odometry_callback(const ros::TimerEvent& events){ merged_odom.twist.twist.linear.x = velocity_linear_x; merged_odom.twist.twist.angular.z = velocity_angular_z; - const float move_x = velocity_linear_x * std::cos(odom_merged_yaw) - velocity_linear_y * std::sin(odom_merged_yaw); - const float move_y = velocity_linear_x * std::sin(odom_merged_yaw) + velocity_linear_y * std::cos(odom_merged_yaw); + const float move_x = velocity_linear_x * std::cos(odom_merged_yaw); + const float 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; From 46e386d0853288344658c44d4586505a71ed5f8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 19 Jul 2023 12:37:31 +0200 Subject: [PATCH 12/17] format code MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/src/firmware_message_converter.cpp | 29 +++++++++++++---------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index b40dbf3..827c717 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -135,7 +135,7 @@ void imu_callback(const leo_msgs::ImuPtr& msg) { imu_pub.publish(imu); } -void merge_odometry_callback(const ros::TimerEvent& events){ +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; @@ -153,14 +153,14 @@ void merge_odometry_callback(const ros::TimerEvent& events){ if (odom_merged_yaw > 2.0F * PI) odom_merged_yaw -= 2.0F * PI; - else if(odom_merged_yaw < 0.0F) + else if (odom_merged_yaw < 0.0F) odom_merged_yaw += 2.0F * 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); - + odom_merged_pub.publish(merged_odom); } @@ -220,12 +220,12 @@ bool set_imu_calibration_callback(leo_msgs::SetImuCalibrationRequest& req, return true; } -bool reset_odometry_callback(std_srvs::TriggerRequest &req, - std_srvs::TriggerResponse &res) { +bool reset_odometry_callback(std_srvs::TriggerRequest& req, + std_srvs::TriggerResponse& res) { odom_merged_position.x = 0.0F; odom_merged_position.y = 0.0F; odom_merged_yaw = 0.0F; - if(odom_reset_client.call(req, res)) { + if (odom_reset_client.call(req, res)) { res.success = true; return true; } else { @@ -257,8 +257,10 @@ int main(int argc, char** argv) { set_imu_calibration_service = nh.advertiseService("set_imu_calibration", set_imu_calibration_callback); - odom_reset_client = nh.serviceClient("firmware/reset_odometry"); - reset_odometry_service = nh.advertiseService("reset_odometry", &reset_odometry_callback); + odom_reset_client = + nh.serviceClient("firmware/reset_odometry"); + reset_odometry_service = + nh.advertiseService("reset_odometry", &reset_odometry_callback); ros::Rate rate(2); while (ros::ok()) { @@ -274,7 +276,8 @@ 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 and odometry_merged publishers."); + "Shutting down wheel_odom_with_covariance and odometry_merged " + "publishers."); wheel_odom_sub.shutdown(); wheel_odom_pub.shutdown(); odom_merged_pub.shutdown(); @@ -329,12 +332,14 @@ 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) { ROS_INFO( "Both firmware/imu and firmware/wheel_odom topics are advertised. " "Advertising odometry_merged topic."); - odom_merged_pub = nh.advertise("odometry_merged", 10); - odom_merged_timer = nh.createTimer(ros::Duration(0.01), merge_odometry_callback); + odom_merged_pub = + nh.advertise("odometry_merged", 10); + odom_merged_timer = + nh.createTimer(ros::Duration(0.01), merge_odometry_callback); odom_merged_advertised = true; } } From a0689d28ded3f91d110b8ee2ec1578a7a5cefb12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 19 Jul 2023 14:56:06 +0200 Subject: [PATCH 13/17] remove mecanum parameters from firmware message converter config file MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_bringup/config/firmware_message_converter.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/leo_bringup/config/firmware_message_converter.yaml b/leo_bringup/config/firmware_message_converter.yaml index 6f10733..49a6271 100644 --- a/leo_bringup/config/firmware_message_converter.yaml +++ b/leo_bringup/config/firmware_message_converter.yaml @@ -8,6 +8,5 @@ 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] -mecanum_wheel_odom_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] From 2295653bc9aefb7b96825bf51f885610fe64a74c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 2 Aug 2023 17:29:33 +0200 Subject: [PATCH 14/17] implement code review guidelines MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/src/firmware_message_converter.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 827c717..b9ad21b 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -17,6 +17,8 @@ #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; @@ -32,13 +34,12 @@ 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 float odom_merged_yaw; +static double odom_merged_yaw; static bool odom_merged_advertised = false; -static float velocity_linear_x = 0; -static float velocity_angular_z = 0; -ros::ServiceClient odom_reset_client; -ros::ServiceServer reset_odometry_service; -static constexpr float PI = 3.141592653F; +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; @@ -151,10 +152,10 @@ void merge_odometry_callback(const ros::TimerEvent& events) { odom_merged_yaw += velocity_angular_z * 0.01; - if (odom_merged_yaw > 2.0F * PI) - odom_merged_yaw -= 2.0F * PI; - else if (odom_merged_yaw < 0.0F) - odom_merged_yaw += 2.0F * PI; + 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; From ad00f57bb662b6ba1c24b2def581d6954aea3b67 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 2 Aug 2023 19:10:09 +0200 Subject: [PATCH 15/17] removed float literals in double variables MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/src/firmware_message_converter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index b9ad21b..5ea718a 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -223,9 +223,9 @@ bool set_imu_calibration_callback(leo_msgs::SetImuCalibrationRequest& req, bool reset_odometry_callback(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) { - odom_merged_position.x = 0.0F; - odom_merged_position.y = 0.0F; - odom_merged_yaw = 0.0F; + 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; From b57fbb7d586f6cb5a530b7100682a65b70cf462a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 2 Aug 2023 19:11:48 +0200 Subject: [PATCH 16/17] remove some more float literals MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/src/firmware_message_converter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 5ea718a..78a4783 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -159,8 +159,8 @@ void merge_odometry_callback(const ros::TimerEvent& events) { 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); + 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); } From 281fa26fb5e18c5098d07b1312571a7405e48820 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 2 Aug 2023 19:15:28 +0200 Subject: [PATCH 17/17] changed variables to doubles MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/src/firmware_message_converter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 78a4783..cd980f3 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -144,8 +144,8 @@ void merge_odometry_callback(const ros::TimerEvent& events) { merged_odom.twist.twist.linear.x = velocity_linear_x; merged_odom.twist.twist.angular.z = velocity_angular_z; - const float move_x = velocity_linear_x * std::cos(odom_merged_yaw); - const float move_y = velocity_linear_x * std::sin(odom_merged_yaw); + 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;