From 64eaea4eed0ec6c42278c05d3055682e4a68e192 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 16 Aug 2024 07:57:00 +0200 Subject: [PATCH 1/2] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- admittance_controller/src/admittance_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 1e51b993ae..97ed503fac 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -289,10 +289,10 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( "~/wrench_reference", rclcpp::SystemDefaultsQoS(), [&](const geometry_msgs::msg::WrenchStamped & msg) { - if (msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id) + if (msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && !msg.header.frame_id.empty()) { RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame"); + get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " << msg.header.frame_id << ". Expected reference frame: << admittance_->parameters_.ft_sensor.frame_id); return; } input_wrench_command_.writeFromNonRT(msg); From 9bb9435fd45b22391c50101634f3eeed144b5e5c Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Thu, 22 Aug 2024 09:32:46 +0200 Subject: [PATCH 2/2] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota Co-authored-by: Bence Magyar --- admittance_controller/src/admittance_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 97ed503fac..dbe4c0b1d2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -28,7 +28,7 @@ namespace admittance_controller { -static geometry_msgs::msg::Wrench add_wrenches( +geometry_msgs::msg::Wrench add_wrenches( const geometry_msgs::msg::Wrench & a, const geometry_msgs::msg::Wrench & b) { geometry_msgs::msg::Wrench res; @@ -292,7 +292,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( if (msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && !msg.header.frame_id.empty()) { RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " << msg.header.frame_id << ". Expected reference frame: << admittance_->parameters_.ft_sensor.frame_id); + get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " << msg.header.frame_id << ". Expected reference frame: " << admittance_->parameters_.ft_sensor.frame_id); return; } input_wrench_command_.writeFromNonRT(msg);