Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down Expand Up @@ -112,11 +112,11 @@ class GripperActionController : public controller_interface::ControllerInterface

std::string name_; ///< Controller name.
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_command_interface_;
joint_position_command_interface_;
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
effort_interface_;
joint_effort_command_interface_;
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
speed_interface_;
joint_speed_command_interface_;
std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
joint_position_state_interface_;
std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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_);

Expand Down Expand Up @@ -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_);
}

Expand Down Expand Up @@ -347,19 +347,19 @@ 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;

for (auto & interface : command_interfaces_)
{
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;
}
}

Expand All @@ -373,8 +373,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
Expand All @@ -401,7 +401,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;
Expand Down
Loading