diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index 93a6f6ec65..35157df2cf 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -62,8 +62,8 @@ class GripperActionController : public controller_interface::ControllerInterface struct Commands { double position_cmd_; // Commanded position - double max_velocity_; // Desired max gripper velocity - double max_effort_; // Desired max allowed effort + double velocity_cmd_; // Commanded velocity + double effort_cmd_; // Commanded effort }; GripperActionController(); @@ -112,11 +112,11 @@ class GripperActionController : public controller_interface::ControllerInterface std::string name_; ///< Controller name. std::optional> - joint_command_interface_; + joint_position_command_interface_; std::optional> - effort_interface_; + joint_effort_command_interface_; std::optional> - speed_interface_; + joint_speed_command_interface_; std::optional> joint_position_state_interface_; std::optional> diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 479c255a25..5cea6324c2 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -83,25 +83,25 @@ controller_interface::return_type GripperActionController::update( check_for_success( get_node()->now(), error_position, current_position_op.value(), current_velocity_op.value()); - if (!joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_)) + if (!joint_position_command_interface_->get().set_value(command_struct_rt_.position_cmd_)) { RCLCPP_WARN( logger, "Unable to set the joint position command to: %f", command_struct_rt_.position_cmd_); return controller_interface::return_type::OK; } if ( - speed_interface_.has_value() && - !speed_interface_->get().set_value(command_struct_rt_.max_velocity_)) + joint_speed_command_interface_.has_value() && + !joint_speed_command_interface_->get().set_value(command_struct_rt_.velocity_cmd_)) { - RCLCPP_WARN(logger, "Unable to set the speed command to: %f", command_struct_rt_.max_velocity_); + RCLCPP_WARN(logger, "Unable to set the speed command to: %f", command_struct_rt_.velocity_cmd_); return controller_interface::return_type::OK; } if ( - effort_interface_.has_value() && - !effort_interface_->get().set_value(command_struct_rt_.max_effort_)) + joint_effort_command_interface_.has_value() && + !joint_effort_command_interface_->get().set_value(command_struct_rt_.effort_cmd_)) { - RCLCPP_WARN(logger, "Unable to set the effort command to: %f", command_struct_rt_.max_effort_); + RCLCPP_WARN(logger, "Unable to set the effort command to: %f", command_struct_rt_.effort_cmd_); return controller_interface::return_type::OK; } @@ -140,19 +140,19 @@ void GripperActionController::accepted_callback( command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; if (!params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) { - command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; + command_struct_.velocity_cmd_ = goal_handle->get_goal()->command.velocity[0]; } else { - command_struct_.max_velocity_ = params_.max_velocity; + command_struct_.velocity_cmd_ = params_.max_velocity; } if (!params_.max_effort_interface.empty() && !goal_handle->get_goal()->command.effort.empty()) { - command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; + command_struct_.effort_cmd_ = goal_handle->get_goal()->command.effort[0]; } else { - command_struct_.max_effort_ = params_.max_effort; + command_struct_.effort_cmd_ = params_.max_effort; } command_.set(command_struct_); @@ -206,8 +206,8 @@ void GripperActionController::set_hold_position() RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position"); } command_struct_.position_cmd_ = position_op.value(); - command_struct_.max_effort_ = params_.max_effort; - command_struct_.max_velocity_ = params_.max_velocity; + command_struct_.effort_cmd_ = params_.max_effort; + command_struct_.velocity_cmd_ = params_.max_velocity; command_.set(command_struct_); } @@ -340,7 +340,7 @@ controller_interface::CallbackReturn GripperActionController::on_activate( return controller_interface::CallbackReturn::ERROR; } - joint_command_interface_ = *command_interface_it; + joint_position_command_interface_ = *command_interface_it; joint_position_state_interface_ = *position_state_interface_it; joint_velocity_state_interface_ = *velocity_state_interface_it; @@ -348,11 +348,11 @@ controller_interface::CallbackReturn GripperActionController::on_activate( { if (interface.get_interface_name() == "set_gripper_max_effort") { - effort_interface_ = interface; + joint_effort_command_interface_ = interface; } else if (interface.get_interface_name() == "set_gripper_max_velocity") { - speed_interface_ = interface; + joint_speed_command_interface_ = interface; } } @@ -366,8 +366,8 @@ controller_interface::CallbackReturn GripperActionController::on_activate( { command_struct_.position_cmd_ = position_op.value(); } - command_struct_.max_effort_ = params_.max_effort; - command_struct_.max_velocity_ = params_.max_velocity; + command_struct_.effort_cmd_ = params_.max_effort; + command_struct_.velocity_cmd_ = params_.max_velocity; command_.try_set(command_struct_); // Result @@ -392,7 +392,7 @@ controller_interface::CallbackReturn GripperActionController::on_activate( controller_interface::CallbackReturn GripperActionController::on_deactivate( const rclcpp_lifecycle::State &) { - joint_command_interface_ = std::nullopt; + joint_position_command_interface_ = std::nullopt; joint_position_state_interface_ = std::nullopt; joint_velocity_state_interface_ = std::nullopt; return controller_interface::CallbackReturn::SUCCESS;