control mode only available in commanding active (#236)

(cherry picked from commit 0e6f076)
lbr-stack · Dec 14, 2024 · 6dba146 · 6dba146
1 parent 3be3559
commit 6dba146
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 18 deletions.
diff --git 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
@@ -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);