From 6bdacf3382f3311c5962081cbcaf6c5019db320b Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 6 Feb 2024 15:47:07 -0700 Subject: [PATCH] fix use of commmand struct Signed-off-by: Paul Gesel --- antipodal_gripper_controller/doc/userdoc.rst | 2 +- .../antipodal_gripper_action_controller.hpp | 4 ++-- ...tipodal_gripper_action_controller_impl.hpp | 22 +++++++++---------- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/antipodal_gripper_controller/doc/userdoc.rst b/antipodal_gripper_controller/doc/userdoc.rst index a000e2c863..dbbb1d13b7 100644 --- a/antipodal_gripper_controller/doc/userdoc.rst +++ b/antipodal_gripper_controller/doc/userdoc.rst @@ -5,7 +5,7 @@ Antipodal Gripper Action Controller -------------------------------- -Controller for executing a `AntipodalGripperCommand` action for simple antipodal grippers. +Controller for executing a ``AntipodalGripperCommand`` action for simple antipodal grippers. Parameters ^^^^^^^^^^^ diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp index c38b9189e1..45b7697528 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller.hpp @@ -62,9 +62,9 @@ class GripperActionController : public controller_interface::ControllerInterface */ struct Commands { - double position_cmd; // Commanded position + double position_cmd_; // Commanded position double max_velocity_; // Desired max gripper velocity - double max_effort_; // Desired max allowed effort + double max_effort_; // Desired max allowed effort }; GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); diff --git a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp index 66af53bab8..97fe6c5300 100644 --- a/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp +++ b/antipodal_gripper_controller/include/antipodal_gripper_controller/antipodal_gripper_action_controller_impl.hpp @@ -61,14 +61,14 @@ controller_interface::return_type GripperActionController::update( const double current_position = joint_position_state_interface_->get().get_value(); const double current_velocity = joint_velocity_state_interface_->get().get_value(); - const double error_position = command_struct_rt_.position_ - current_position; + const double error_position = command_struct_rt_.position_cmd_ - current_position; check_for_success(get_node()->now(), error_position, current_position, current_velocity); - joint_command_interface_->get().set_value(command_struct_rt_.position_); + joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); if (speed_interface_.has_value()) { - speed_interface_->get().set_value(command_struct_rt_.target_velocity_); + speed_interface_->get().set_value(command_struct_rt_.max_velocity_); } if (effort_interface_.has_value()) { @@ -107,14 +107,14 @@ void GripperActionController::accepted_callback( // This is the non-realtime command_struct // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position[0]; + command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; if (params_.use_velocity_interface && !goal_handle->get_goal()->command.velocity.empty()) { - command_struct_.target_velocity_ = goal_handle->get_goal()->command.velocity[0]; + command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; } else { - command_struct_.target_velocity_ = 0.0; + command_struct_.max_velocity_ = 0.0; } if (params_.use_effort_interface && !goal_handle->get_goal()->command.effort.empty()) { @@ -168,8 +168,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( void GripperActionController::set_hold_position() { - command_struct_.position_ = joint_position_state_interface_->get().get_value(); - command_struct_.target_velocity_ = 0.0; + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_velocity_ = 0.0; command_struct_.max_effort_ = params_.max_effort; command_.writeFromNonRT(command_struct_); } @@ -326,16 +326,16 @@ controller_interface::CallbackReturn GripperActionController::on_activate( // get_node()); // Command - non RT version - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; - command_struct_.target_velocity_ = 0.0; + command_struct_.max_velocity_ = 0.0; command_.initRT(command_struct_); // Result pre_alloc_result_ = std::make_shared(); pre_alloc_result_->state.position.resize(1); pre_alloc_result_->state.effort.resize(1); - pre_alloc_result_->state.position[0] = command_struct_.position_; + pre_alloc_result_->state.position[0] = command_struct_.position_cmd_; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = false;