From 8cafdfae54d68610e0e9421ae683bc58e8ad4a5d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Mon, 18 Sep 2023 16:10:58 +0200 Subject: [PATCH 1/8] handling wheel odometry from mecanum wheels in firmware 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 | 69 ++++++++++++++++++++++- 1 file changed, 67 insertions(+), 2 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index ca493aa..d41f2be 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -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" @@ -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( @@ -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)); @@ -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; @@ -219,15 +246,16 @@ 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."); @@ -235,6 +263,35 @@ class FirmwareMessageConverter : public rclcpp::Node wheel_odom_sub_ = create_subscription( 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( + "wheel_odom_with_covariance", 10); + wheel_odom_mecanum_sub_ = + create_subscription( + 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_); @@ -266,6 +323,8 @@ class FirmwareMessageConverter : public rclcpp::Node "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 wheel_odom_mecanum_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, @@ -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 @@ -290,10 +350,15 @@ class FirmwareMessageConverter : public rclcpp::Node // Subscriptions rclcpp::Subscription::SharedPtr wheel_states_sub_; rclcpp::Subscription::SharedPtr wheel_odom_sub_; + rclcpp::Subscription::SharedPtr wheel_odom_mecanum_sub_; rclcpp::Subscription::SharedPtr imu_sub_; // Service rclcpp::Service::SharedPtr set_imu_calibration_service; + + // Flags + bool wheel_odom_advertised; + bool wheel_odom_mecanum_advertised; }; } // namespace leo_fw From b203aedd4377d8d38ffcdd0412bac03b4458392d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Tue, 19 Sep 2023 11:13:22 +0200 Subject: [PATCH 2/8] add mecanum parameters to converter 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 d2fccbb..962c07f 100644 --- a/leo_bringup/config/firmware_message_converter.yaml +++ b/leo_bringup/config/firmware_message_converter.yaml @@ -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] From 8c1ca436d460a0bad0512f0dc9f7fe9f77d19adb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Tue, 19 Sep 2023 11:13:51 +0200 Subject: [PATCH 3/8] fix typo 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 | 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 d41f2be..7d50a07 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -252,7 +252,7 @@ class FirmwareMessageConverter : public rclcpp::Node "Shutting down wheel_odom_with_covariance publisher."); wheel_odom_sub_.reset(); wheel_odom_pub_.reset(); - wheek_odom_advertised = false; + wheel_odom_advertised = false; } if (!wheel_odom_advertised && wheel_odom_publishers > 0) { From 053f48346e005ed37e7520aa4c15dbbfefeffcff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Mon, 28 Aug 2023 15:33:14 +0200 Subject: [PATCH 4/8] cherry-pick firmware.yaml file from other branch 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.yaml | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 leo_bringup/config/firmware.yaml diff --git a/leo_bringup/config/firmware.yaml b/leo_bringup/config/firmware.yaml new file mode 100644 index 0000000..998d4b3 --- /dev/null +++ b/leo_bringup/config/firmware.yaml @@ -0,0 +1,25 @@ +wheels: + encoder_resolution: 878.4 + torque_constant: 1.17647 + pid: + p: 0.0 + i: 0.005 + d: 0.0 + pwm_duty_limit: 100.0 + +# mecanum_wheels: false + +diff_drive: + wheel_radius: 0.0625 + wheel_separation: 0.358 + angular_velocity_multiplier: 1.76 + input_timeout: 500 + +# mecanum_drive: +# wheel_radius: 0.0635 +# wheel_separation: 0.37 +# wheel_base: 0.3052 +# angular_velocity_multiplier: 1.0 +# input_timeout: 500 + +battery_min_voltage: 10.0 \ No newline at end of file From 9f614fcbca43277d39f905a47ea884ffd8a831ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Tue, 19 Sep 2023 14:00:05 +0200 Subject: [PATCH 5/8] add mecanum_wheels param to firmware.yaml 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.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/leo_bringup/config/firmware.yaml b/leo_bringup/config/firmware.yaml index 998d4b3..adc793e 100644 --- a/leo_bringup/config/firmware.yaml +++ b/leo_bringup/config/firmware.yaml @@ -7,7 +7,7 @@ wheels: d: 0.0 pwm_duty_limit: 100.0 -# mecanum_wheels: false +mecanum_wheels: false diff_drive: wheel_radius: 0.0625 From 3d2ad63394d0f89416ced238a2594e98412239ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Thu, 21 Sep 2023 12:54:24 +0200 Subject: [PATCH 6/8] added separate publisher for mecanum 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 | 21 +++++++-------------- 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 7d50a07..5066cf9 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -246,16 +246,15 @@ class FirmwareMessageConverter : public rclcpp::Node size_t wheel_odom_publishers = count_publishers(wheel_odom_topic_); - if (wheel_odom_advertised && wheel_odom_publishers == 0) { + 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."); wheel_odom_sub_.reset(); wheel_odom_pub_.reset(); - wheel_odom_advertised = false; } - if (!wheel_odom_advertised && wheel_odom_publishers > 0) { + if (!wheel_odom_pub_ && wheel_odom_publishers > 0) { RCLCPP_INFO( get_logger(), "Detected a publisher on firmware/wheel_odom topic. " "Starting publishing on wheel_odom_with_covariance topic."); @@ -263,27 +262,25 @@ class FirmwareMessageConverter : public rclcpp::Node wheel_odom_sub_ = create_subscription( 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) { + 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_pub_.reset(); - wheel_odom_mecanum_advertised = false; + wheel_odom_mecanum_pub_.reset(); } - if (!wheel_odom_mecanum_advertised && wheel_odom_mecanum_publishers > 0) { + 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_pub_ = create_publisher( + wheel_odom_mecanum_pub_ = create_publisher( "wheel_odom_with_covariance", 10); wheel_odom_mecanum_sub_ = create_subscription( @@ -291,7 +288,6 @@ class FirmwareMessageConverter : public rclcpp::Node std::bind( &FirmwareMessageConverter::mecanum_odom_callback, this, _1)); - wheel_odom_mecanum_advertised = true; } size_t imu_publishers = count_publishers(imu_topic_); @@ -345,6 +341,7 @@ class FirmwareMessageConverter : public rclcpp::Node // Publishers rclcpp::Publisher::SharedPtr joint_states_pub_; rclcpp::Publisher::SharedPtr wheel_odom_pub_; + rclcpp::Publisher::SharedPtr wheel_odom_mecanum_pub_; rclcpp::Publisher::SharedPtr imu_pub_; // Subscriptions @@ -355,10 +352,6 @@ class FirmwareMessageConverter : public rclcpp::Node // Service rclcpp::Service::SharedPtr set_imu_calibration_service; - - // Flags - bool wheel_odom_advertised; - bool wheel_odom_mecanum_advertised; }; } // namespace leo_fw From 8a33df7beb2c5d1105230fec05f7f054e40e32b2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Thu, 21 Sep 2023 15:24:19 +0200 Subject: [PATCH 7/8] fixed usage of bad publisher 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 | 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 5066cf9..db37bef 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -134,7 +134,7 @@ class FirmwareMessageConverter : public rclcpp::Node wheel_odom_mecanum_twist_covariance_diagonal_[i]; } - wheel_odom_pub_->publish(wheel_odom); + wheel_odom_mecanum_pub_->publish(wheel_odom); } void imu_callback(const leo_msgs::msg::Imu::SharedPtr msg) const From 5caa6c98e9205a7f9d6a4ba27ed906b3047237ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 27 Sep 2023 18:28:02 +0200 Subject: [PATCH 8/8] removed unnecesarry added files 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.yaml | 25 ------------------------- 1 file changed, 25 deletions(-) delete mode 100644 leo_bringup/config/firmware.yaml diff --git a/leo_bringup/config/firmware.yaml b/leo_bringup/config/firmware.yaml deleted file mode 100644 index adc793e..0000000 --- a/leo_bringup/config/firmware.yaml +++ /dev/null @@ -1,25 +0,0 @@ -wheels: - encoder_resolution: 878.4 - torque_constant: 1.17647 - pid: - p: 0.0 - i: 0.005 - d: 0.0 - pwm_duty_limit: 100.0 - -mecanum_wheels: false - -diff_drive: - wheel_radius: 0.0625 - wheel_separation: 0.358 - angular_velocity_multiplier: 1.76 - input_timeout: 500 - -# mecanum_drive: -# wheel_radius: 0.0635 -# wheel_separation: 0.37 -# wheel_base: 0.3052 -# angular_velocity_multiplier: 1.0 -# input_timeout: 500 - -battery_min_voltage: 10.0 \ No newline at end of file