|
| 1 | +/********************************************************************* |
| 2 | + * Software License Agreement (BSD License) |
| 3 | + * |
| 4 | + * Copyright (c) 2025, Marq Rasmussen |
| 5 | + * All rights reserved. |
| 6 | + * |
| 7 | + * Redistribution and use in source and binary forms, with or without |
| 8 | + * modification, are permitted provided that the following conditions |
| 9 | + * are met: |
| 10 | + * |
| 11 | + * * Redistributions of source code must retain the above copyright |
| 12 | + * notice, this list of conditions and the following disclaimer. |
| 13 | + * * Redistributions in binary form must reproduce the above |
| 14 | + * copyright notice, this list of conditions and the following |
| 15 | + * disclaimer in the documentation and/or other materials provided |
| 16 | + * with the distribution. |
| 17 | + * * Neither the name of the Marq Rasmussen nor the names of its |
| 18 | + * contributors may be used to endorse or promote products derived |
| 19 | + * from this software without specific prior written permission. |
| 20 | + * |
| 21 | + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 22 | + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 23 | + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 24 | + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 25 | + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 26 | + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 27 | + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 28 | + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 29 | + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 30 | + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 31 | + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 32 | + * POSSIBILITY OF SUCH DAMAGE. |
| 33 | + *********************************************************************/ |
| 34 | +/* Design inspired by gripper_command_controller_handle.hpp */ |
| 35 | +/* Author: Marq Rasmussen */ |
| 36 | + |
| 37 | +#pragma once |
| 38 | + |
| 39 | +#include <moveit_simple_controller_manager/action_based_controller_handle.hpp> |
| 40 | +#include <control_msgs/action/parallel_gripper_command.hpp> |
| 41 | +#include <set> |
| 42 | + |
| 43 | +namespace moveit_simple_controller_manager |
| 44 | +{ |
| 45 | +/* |
| 46 | + * This is an interface for a gripper using the control_msgs/ParallelGripperCommand action interface. |
| 47 | + */ |
| 48 | +class ParallelGripperCommandControllerHandle |
| 49 | + : public ActionBasedControllerHandle<control_msgs::action::ParallelGripperCommand> |
| 50 | +{ |
| 51 | +public: |
| 52 | + /* Topics will map to name/ns/goal, name/ns/result, etc */ |
| 53 | + ParallelGripperCommandControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, |
| 54 | + const std::string& ns, const double max_effort = 0.0, |
| 55 | + const double max_velocity = 0.0) |
| 56 | + : ActionBasedControllerHandle<control_msgs::action::ParallelGripperCommand>( |
| 57 | + node, name, ns, "moveit.simple_controller_manager.parallel_gripper_controller_handle") |
| 58 | + , max_effort_(max_effort) |
| 59 | + , max_velocity_(max_velocity) |
| 60 | + { |
| 61 | + } |
| 62 | + |
| 63 | + bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) override |
| 64 | + { |
| 65 | + RCLCPP_DEBUG_STREAM(logger_, "Received new trajectory for " << name_); |
| 66 | + |
| 67 | + if (!controller_action_client_) |
| 68 | + return false; |
| 69 | + |
| 70 | + if (!isConnected()) |
| 71 | + { |
| 72 | + RCLCPP_ERROR_STREAM(logger_, "Action client not connected to action server: " << getActionName()); |
| 73 | + return false; |
| 74 | + } |
| 75 | + |
| 76 | + if (!trajectory.multi_dof_joint_trajectory.points.empty()) |
| 77 | + { |
| 78 | + RCLCPP_ERROR(logger_, "%s cannot execute multi-dof trajectories.", name_.c_str()); |
| 79 | + return false; |
| 80 | + } |
| 81 | + |
| 82 | + if (trajectory.joint_trajectory.points.empty()) |
| 83 | + { |
| 84 | + RCLCPP_ERROR(logger_, "%s requires at least one joint trajectory point, but none received.", name_.c_str()); |
| 85 | + return false; |
| 86 | + } |
| 87 | + |
| 88 | + if (trajectory.joint_trajectory.joint_names.empty()) |
| 89 | + { |
| 90 | + RCLCPP_ERROR(logger_, "%s received a trajectory with no joint names specified.", name_.c_str()); |
| 91 | + return false; |
| 92 | + } |
| 93 | + |
| 94 | + // goal to be sent |
| 95 | + control_msgs::action::ParallelGripperCommand::Goal goal; |
| 96 | + auto& cmd_state = goal.command; |
| 97 | + |
| 98 | + auto& joint_names = trajectory.joint_trajectory.joint_names; |
| 99 | + auto it = std::find(joint_names.begin(), joint_names.end(), command_joint_); |
| 100 | + if (it != joint_names.end()) |
| 101 | + { |
| 102 | + cmd_state.name.push_back(command_joint_); |
| 103 | + std::size_t gripper_joint_index = std::distance(joint_names.begin(), it); |
| 104 | + // send last trajectory point |
| 105 | + if (trajectory.joint_trajectory.points.back().positions.size() <= gripper_joint_index) |
| 106 | + { |
| 107 | + RCLCPP_ERROR(logger_, "ParallelGripperCommand expects a joint trajectory with one \ |
| 108 | + point that specifies at least the position of joint \ |
| 109 | + '%s', but insufficient positions provided.", |
| 110 | + trajectory.joint_trajectory.joint_names[gripper_joint_index].c_str()); |
| 111 | + return false; |
| 112 | + } |
| 113 | + cmd_state.position.push_back(trajectory.joint_trajectory.points.back().positions[gripper_joint_index]); |
| 114 | + // only set the velocity or effort if the user has specified a positive non-zero max value |
| 115 | + if (max_velocity_ > 0.0) |
| 116 | + { |
| 117 | + cmd_state.velocity.push_back(max_velocity_); |
| 118 | + } |
| 119 | + if (max_effort_ > 0.0) |
| 120 | + { |
| 121 | + cmd_state.effort.push_back(max_effort_); |
| 122 | + } |
| 123 | + } |
| 124 | + else |
| 125 | + { |
| 126 | + RCLCPP_ERROR(logger_, "Received trajectory does not include a command for joint name %s.", command_joint_.c_str()); |
| 127 | + return false; |
| 128 | + } |
| 129 | + |
| 130 | + rclcpp_action::Client<control_msgs::action::ParallelGripperCommand>::SendGoalOptions send_goal_options; |
| 131 | + // Active callback |
| 132 | + send_goal_options.goal_response_callback = |
| 133 | + [this](const rclcpp_action::Client<control_msgs::action::ParallelGripperCommand>::GoalHandle::SharedPtr& |
| 134 | + /* unused-arg */) { RCLCPP_DEBUG_STREAM(logger_, name_ << " started execution."); }; |
| 135 | + // Send goal |
| 136 | + auto current_goal_future = controller_action_client_->async_send_goal(goal, send_goal_options); |
| 137 | + current_goal_ = current_goal_future.get(); |
| 138 | + if (!current_goal_) |
| 139 | + { |
| 140 | + RCLCPP_ERROR(logger_, "%s goal was rejected by server.", name_.c_str()); |
| 141 | + return false; |
| 142 | + } |
| 143 | + |
| 144 | + done_ = false; |
| 145 | + last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING; |
| 146 | + return true; |
| 147 | + } |
| 148 | + |
| 149 | + void setCommandJoint(const std::string& name) |
| 150 | + { |
| 151 | + command_joint_ = name; |
| 152 | + } |
| 153 | + |
| 154 | +private: |
| 155 | + void controllerDoneCallback( |
| 156 | + const rclcpp_action::ClientGoalHandle<control_msgs::action::ParallelGripperCommand>::WrappedResult& wrapped_result) |
| 157 | + override |
| 158 | + { |
| 159 | + finishControllerExecution(wrapped_result.code); |
| 160 | + } |
| 161 | + |
| 162 | + /* |
| 163 | + * The ``max_effort`` used in the ParallelGripperCommand message. |
| 164 | + */ |
| 165 | + double max_effort_ = 0.0; |
| 166 | + |
| 167 | + /* |
| 168 | + * The ``max_velocity_`` used in the ParallelGripperCommand message. |
| 169 | + */ |
| 170 | + double max_velocity_ = 0.0; |
| 171 | + |
| 172 | + /* |
| 173 | + * The joint to command in the ParallelGripperCommand message |
| 174 | + */ |
| 175 | + std::string command_joint_; |
| 176 | +}; // namespace moveit_simple_controller_manager |
| 177 | + |
| 178 | +} // end namespace moveit_simple_controller_manager |
0 commit comments