From 6dba146a6bc6e23656c9284a38e1ec14c692f00c Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 18:07:00 +0000 Subject: [PATCH] control mode only available in commanding active (#236) (cherry picked from commit 0e6f07683a17508d3333de0484835c4b07d1886e) --- lbr_fri_ros2/src/async_client.cpp | 24 +++++++++++------------ lbr_ros2_control/src/system_interface.cpp | 6 ------ 2 files changed, 12 insertions(+), 18 deletions(-) diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 0a2f4ea2..e4d84d10 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -67,7 +67,19 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, // initialize command state_interface_ptr_->set_state(robotState()); command_interface_ptr_->init_command(state_interface_ptr_->get_state()); +} + +void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; + +void AsyncClient::waitForCommand() { + KUKA::FRI::LBRClient::waitForCommand(); + state_interface_ptr_->set_state(robotState()); + command_interface_ptr_->init_command(state_interface_ptr_->get_state()); + command_interface_ptr_->buffered_command_to_fri(robotCommand(), + state_interface_ptr_->get_state()); +} +void AsyncClient::command() { // if robot is in impedance or Cartesian impedance control mode, override open_loop_ to false // also refer to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/226 auto control_mode = robotState().getControlMode(); @@ -84,19 +96,7 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, open_loop_ = false; } } -} - -void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; - -void AsyncClient::waitForCommand() { - KUKA::FRI::LBRClient::waitForCommand(); - state_interface_ptr_->set_state(robotState()); - command_interface_ptr_->init_command(state_interface_ptr_->get_state()); - command_interface_ptr_->buffered_command_to_fri(robotCommand(), - state_interface_ptr_->get_state()); -} -void AsyncClient::command() { if (open_loop_) { state_interface_ptr_->set_state_open_loop(robotState(), command_interface_ptr_->get_command().joint_position); diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 21adfe2b..26e1e141 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -228,12 +228,6 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::OKGREEN << "Robot connected" << lbr_fri_ros2::ColorScheme::ENDC); - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), - "Control mode '" - << lbr_fri_ros2::EnumMaps::control_mode_map( - async_client_ptr_->get_state_interface()->get_state().control_mode) - .c_str() - << "'"); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time %.3f s / %.1f Hz", async_client_ptr_->get_state_interface()->get_state().sample_time, 1. / async_client_ptr_->get_state_interface()->get_state().sample_time);