From d3cb4b3709f927fc845763a151d502edceffa496 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Mon, 27 Nov 2023 16:18:37 +0100 Subject: [PATCH 01/12] added system of merged odometry to 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 | 126 +++++++++++++++++++++- 1 file changed, 121 insertions(+), 5 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index f67aeb7..0ba0678 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -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" @@ -39,6 +40,7 @@ using namespace std::chrono_literals; using std::placeholders::_1; +using std::placeholders::_2; namespace leo_fw { @@ -57,6 +59,13 @@ class FirmwareMessageConverter : public rclcpp::Node &FirmwareMessageConverter::set_imu_calibration_callback, this, std::placeholders::_1, std::placeholders::_2)); + reset_odometry_client = create_client("firmware/reset_odometry"); + + reset_odometry_service = create_service( + "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_); @@ -107,6 +116,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]; @@ -129,6 +140,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]; @@ -149,6 +163,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]; @@ -159,6 +175,52 @@ class FirmwareMessageConverter : public rclcpp::Node imu_pub_->publish(imu); } + void merged_odometry_callback() const + { + 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 * 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 request, std::shared_ptr response) @@ -177,6 +239,28 @@ class FirmwareMessageConverter : public rclcpp::Node response->success = true; } + void reset_odometry_callback( + const std::shared_ptr req, + std::shared_ptr res) + { + odom_merged_position.x = 0.0; + odom_merged_position.y = 0.0; + odom_merged_yaw = 0.0; + + auto reset_request = std_srvs::srv::Trigger_Request::SharedPtr(); + + auto result = reset_odometry_client->async_send_request(reset_request); + + if (rclcpp::spin_until_future_complete( + this->get_node_base_interface(), + result) != rclcpp::FutureReturnCode::SUCCESS) + { + res->success = false; + } else { + res->success = true; + } + } + void load_yaml_bias() { YAML::Node node; @@ -249,9 +333,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) { @@ -270,9 +356,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) { @@ -295,9 +383,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) { @@ -309,8 +399,28 @@ 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("odometry_merged", 10); + odom_merged_timer_ = + create_wall_timer( + 100ms, + std::bind(&FirmwareMessageConverter::merged_odometry_callback, this)); + } } + // Merged Odom variables + static constexpr double PI = 3.141592653; + mutable double velocity_linear_x; + mutable double velocity_linear_y; + mutable double velocity_angular_z; + mutable double odom_merged_yaw; + mutable geometry_msgs::msg::Point odom_merged_position; + // Parameters std::string robot_frame_id_ = "base_link"; std::string odom_frame_id_ = "odom"; @@ -335,13 +445,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::SharedPtr joint_states_pub_; rclcpp::Publisher::SharedPtr wheel_odom_pub_; rclcpp::Publisher::SharedPtr wheel_odom_mecanum_pub_; + rclcpp::Publisher::SharedPtr odom_merged_pub_; rclcpp::Publisher::SharedPtr imu_pub_; // Subscriptions @@ -350,8 +462,12 @@ class FirmwareMessageConverter : public rclcpp::Node rclcpp::Subscription::SharedPtr wheel_odom_mecanum_sub_; rclcpp::Subscription::SharedPtr imu_sub_; - // Service + // Services rclcpp::Service::SharedPtr set_imu_calibration_service; + rclcpp::Service::SharedPtr reset_odometry_service; + + // Clients + rclcpp::Client::SharedPtr reset_odometry_client; }; } // namespace leo_fw From d2006fe9c90db8091673c2e48ca55673690dfe62 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Mon, 27 Nov 2023 16:28:31 +0100 Subject: [PATCH 02/12] added required dependencies to package.xml and CMakeLists files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/CMakeLists.txt | 2 ++ leo_fw/package.xml | 2 ++ 2 files changed, 4 insertions(+) diff --git a/leo_fw/CMakeLists.txt b/leo_fw/CMakeLists.txt index ab654b4..98e57a1 100644 --- a/leo_fw/CMakeLists.txt +++ b/leo_fw/CMakeLists.txt @@ -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) @@ -26,6 +27,7 @@ ament_target_dependencies(leo_fw sensor_msgs leo_msgs nav_msgs + std_srvs ) rclcpp_components_register_node(leo_fw PLUGIN "leo_fw::FirmwareMessageConverter" diff --git a/leo_fw/package.xml b/leo_fw/package.xml index 0355cc1..0aa127d 100644 --- a/leo_fw/package.xml +++ b/leo_fw/package.xml @@ -36,6 +36,7 @@ rclcpp rclcpp_components sensor_msgs + std_srvs yaml-cpp leo_msgs @@ -43,6 +44,7 @@ rclcpp rclcpp_components sensor_msgs + std_srvs ament_lint_auto From 858c04509ee34f79c9c61f19e176e18366701184 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Mon, 18 Dec 2023 15:36:02 +0100 Subject: [PATCH 03/12] changed service request toi shared ptr 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 0ba0678..de507f7 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -247,7 +247,7 @@ class FirmwareMessageConverter : public rclcpp::Node odom_merged_position.y = 0.0; odom_merged_yaw = 0.0; - auto reset_request = std_srvs::srv::Trigger_Request::SharedPtr(); + auto reset_request = std::make_shared(); auto result = reset_odometry_client->async_send_request(reset_request); From 47f08dbb53b0255495d6ae9d567243e201181a04 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Mon, 18 Dec 2023 17:27:13 +0100 Subject: [PATCH 04/12] remove usage of spin until future complete from callback method 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 | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index de507f7..4cd0e5b 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -243,6 +243,7 @@ class FirmwareMessageConverter : public rclcpp::Node const std::shared_ptr req, std::shared_ptr 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; @@ -251,9 +252,9 @@ class FirmwareMessageConverter : public rclcpp::Node auto result = reset_odometry_client->async_send_request(reset_request); - if (rclcpp::spin_until_future_complete( - this->get_node_base_interface(), - result) != rclcpp::FutureReturnCode::SUCCESS) + auto result_status = result.wait_for(callback_timeout); + + if (result_status == std::future_status::ready) { res->success = false; } else { From 76223ee40aa949fdba1f1be33ee6e7d1e5ae9662 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 20 Dec 2023 16:25:48 +0100 Subject: [PATCH 05/12] increase publishing frequency for merged odometry 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 4cd0e5b..5f77f6e 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -409,7 +409,7 @@ class FirmwareMessageConverter : public rclcpp::Node odom_merged_pub_ = create_publisher("odometry_merged", 10); odom_merged_timer_ = create_wall_timer( - 100ms, + 10ms, std::bind(&FirmwareMessageConverter::merged_odometry_callback, this)); } } From 05cd60a10576f42040653da6b57c1d6403d5bcd5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 20 Dec 2023 17:31:06 +0100 Subject: [PATCH 06/12] code format 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 | 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 5f77f6e..4191de5 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -254,8 +254,7 @@ class FirmwareMessageConverter : public rclcpp::Node auto result_status = result.wait_for(callback_timeout); - if (result_status == std::future_status::ready) - { + if (result_status == std::future_status::ready) { res->success = false; } else { res->success = true; From 8611dd4c88ddf809738cc8d9ae2ae1a0311b9bec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 20 Dec 2023 18:27:42 +0100 Subject: [PATCH 07/12] remove const tag from some of the callback methods 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 | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 4191de5..f1d109c 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -103,7 +103,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_; @@ -126,7 +126,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_; @@ -151,7 +151,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_; @@ -175,7 +175,7 @@ class FirmwareMessageConverter : public rclcpp::Node imu_pub_->publish(imu); } - void merged_odometry_callback() const + void merged_odometry_callback() { nav_msgs::msg::Odometry merged_odom; merged_odom.header.frame_id = odom_frame_id_; @@ -415,11 +415,11 @@ class FirmwareMessageConverter : public rclcpp::Node // Merged Odom variables static constexpr double PI = 3.141592653; - mutable double velocity_linear_x; - mutable double velocity_linear_y; - mutable double velocity_angular_z; - mutable double odom_merged_yaw; - mutable geometry_msgs::msg::Point odom_merged_position; + 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"; From 2a1eaf6e501ff6e2c1f1ea059c53b578abfab650 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 20 Dec 2023 18:30:59 +0100 Subject: [PATCH 08/12] make PI a global variable 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 | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index f1d109c..a013752 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -42,6 +42,8 @@ using namespace std::chrono_literals; using std::placeholders::_1; using std::placeholders::_2; +constexpr double PI = 3.141592653; + namespace leo_fw { @@ -414,7 +416,6 @@ class FirmwareMessageConverter : public rclcpp::Node } // Merged Odom variables - static constexpr double PI = 3.141592653; double velocity_linear_x; double velocity_linear_y; double velocity_angular_z; From 6c1be617985f9acf255b1b12bdb6fcf6b0f8732a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Wed, 20 Dec 2023 18:48:16 +0100 Subject: [PATCH 09/12] fix filling success status in service 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 a013752..e872b06 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -257,9 +257,9 @@ class FirmwareMessageConverter : public rclcpp::Node auto result_status = result.wait_for(callback_timeout); if (result_status == std::future_status::ready) { - res->success = false; - } else { res->success = true; + } else { + res->success = false; } } From 9dfa6de798b42ba45667616d78303cfe0dead349 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Thu, 28 Dec 2023 20:20:37 +0100 Subject: [PATCH 10/12] add callbacks and multihthreaded executor MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- leo_fw/CMakeLists.txt | 1 + leo_fw/src/firmware_message_converter.cpp | 13 ++++++++----- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/leo_fw/CMakeLists.txt b/leo_fw/CMakeLists.txt index 98e57a1..594066e 100644 --- a/leo_fw/CMakeLists.txt +++ b/leo_fw/CMakeLists.txt @@ -32,6 +32,7 @@ ament_target_dependencies(leo_fw rclcpp_components_register_node(leo_fw PLUGIN "leo_fw::FirmwareMessageConverter" EXECUTABLE "firmware_message_converter" + EXECUTOR "MultiThreadedExecutor" ) ament_python_install_package(${PROJECT_NAME}) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index e872b06..04541c3 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -61,7 +61,8 @@ class FirmwareMessageConverter : public rclcpp::Node &FirmwareMessageConverter::set_imu_calibration_callback, this, std::placeholders::_1, std::placeholders::_2)); - reset_odometry_client = create_client("firmware/reset_odometry"); + auto client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + reset_odometry_client = create_client("firmware/reset_odometry", rmw_qos_profile_services_default, client_cb_group_); reset_odometry_service = create_service( "reset_odometry", @@ -252,13 +253,15 @@ class FirmwareMessageConverter : public rclcpp::Node auto reset_request = std::make_shared(); - auto result = reset_odometry_client->async_send_request(reset_request); + auto future = reset_odometry_client->async_send_request(reset_request); - auto result_status = result.wait_for(callback_timeout); + auto result_status = future.wait_for(callback_timeout); - if (result_status == std::future_status::ready) { - res->success = true; + if (future.valid()) { + res->success = future.get()->success; } else { + if(result_status == std::future_status::timeout) + res->message = "Firmware service timeout"; res->success = false; } } From 3d92ba480c953dff1486496149f4ad7221198ac1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Thu, 4 Jan 2024 18:52:26 +0100 Subject: [PATCH 11/12] working solution 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 | 37 +++++++++++++++-------- 1 file changed, 25 insertions(+), 12 deletions(-) diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp index 04541c3..eba3905 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -61,10 +61,13 @@ class FirmwareMessageConverter : public rclcpp::Node &FirmwareMessageConverter::set_imu_calibration_callback, this, std::placeholders::_1, std::placeholders::_2)); - auto client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - reset_odometry_client = create_client("firmware/reset_odometry", rmw_qos_profile_services_default, client_cb_group_); + client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + reset_odometry_client_ = create_client( + "firmware/reset_odometry", + rmw_qos_profile_services_default, + client_cb_group_); - reset_odometry_service = create_service( + reset_odometry_service_ = create_service( "reset_odometry", std::bind( &FirmwareMessageConverter::reset_odometry_callback, this, std::placeholders::_1, @@ -247,22 +250,29 @@ class FirmwareMessageConverter : public rclcpp::Node std::shared_ptr 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(); - - auto future = reset_odometry_client->async_send_request(reset_request); - + auto future = reset_odometry_client_->async_send_request(reset_request); auto result_status = future.wait_for(callback_timeout); - if (future.valid()) { - res->success = future.get()->success; + 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 { - if(result_status == std::future_status::timeout) - res->message = "Firmware service timeout"; res->success = false; + res->message = "Firmware service call deffered."; } } @@ -468,10 +478,13 @@ class FirmwareMessageConverter : public rclcpp::Node // Services rclcpp::Service::SharedPtr set_imu_calibration_service; - rclcpp::Service::SharedPtr reset_odometry_service; + rclcpp::Service::SharedPtr reset_odometry_service_; // Clients - rclcpp::Client::SharedPtr reset_odometry_client; + rclcpp::Client::SharedPtr reset_odometry_client_; + + // Callback Groups + rclcpp::CallbackGroup::SharedPtr client_cb_group_ }; } // namespace leo_fw From fe439a6cfc12025cb9ebd2f10a707fa43569f8bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Thu, 4 Jan 2024 19:04:52 +0100 Subject: [PATCH 12/12] fix missing semicolon 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 eba3905..6ade152 100644 --- a/leo_fw/src/firmware_message_converter.cpp +++ b/leo_fw/src/firmware_message_converter.cpp @@ -484,7 +484,7 @@ class FirmwareMessageConverter : public rclcpp::Node rclcpp::Client::SharedPtr reset_odometry_client_; // Callback Groups - rclcpp::CallbackGroup::SharedPtr client_cb_group_ + rclcpp::CallbackGroup::SharedPtr client_cb_group_; }; } // namespace leo_fw