55 Controller::init(nh, manager);
59 std::string l_name, r_name;
60 nh.
param<std::string>(
"l_gripper_joint", l_name,
"l_gripper_finger_joint");
61 nh.
param<std::string>(
"r_gripper_joint", r_name,
"r_gripper_finger_joint");
63 left_ = manager_->getJointHandle(l_name);
64 right_ = manager_->getJointHandle(r_name);
70 "Unable to retreive joint (%s), Namespace: %s/l_gripper_joint",
78 "Unable to retreive joint (%s), Namespace: %s/r_gripper_joint",
85 boost::bind(&ParallelGripperController::executeCb,
this, _1),
90 nh.
param<
double>(
"max_position", max_position_, 0.1);
91 nh.
param<
double>(
"max_effort", max_effort_, 10.0);
96 use_centering_controller_ =
true;
100 goal_ = max_position_;
101 effort_ = max_effort_;
107 bool ParallelGripperController::start()
112 "Unable to start, not initialized.");
116 if (!server_->isActive())
119 "Unable to start, action server is not active.");
126 bool ParallelGripperController::stop(
bool force)
131 if (server_->isActive())
136 server_->setPreempted();
148 bool ParallelGripperController::reset()
159 if (use_centering_controller_)
161 double position = left_->getPosition() + right_->getPosition();
162 double effort = fabs(effort_);
163 if (goal_ < position)
168 double offset = centering_pid_.update(left_->getPosition() - right_->getPosition(), dt.
toSec());
170 left_->setEffort(effort - offset);
171 right_->setEffort(effort + offset);
175 left_->setPosition(goal_/2.0, 0, effort_);
176 right_->setPosition(goal_/2.0, 0, effort_);
180 void ParallelGripperController::executeCb(
const control_msgs::GripperCommandGoalConstPtr& goal)
182 control_msgs::GripperCommandFeedback feedback;
183 control_msgs::GripperCommandResult
result;
187 server_->setAborted(result,
"Controller is not initialized.");
191 if (manager_->requestStart(
getName()) != 0)
193 server_->setAborted(result,
"Cannot execute, unable to start controller.");
195 "Cannot execute, unable to start controller.");
200 if (goal->command.max_effort <= 0.0 || goal->command.max_effort > max_effort_)
202 effort_ = max_effort_;
206 effort_ = goal->command.max_effort;
210 if (goal->command.position > max_position_)
212 goal_ = max_position_;
214 else if (goal->command.position < 0.0)
220 goal_ = goal->command.position;
224 float last_position = left_->getPosition() + right_->getPosition();
231 if (server_->isPreemptRequested() || !
ros::ok())
234 server_->setPreempted();
239 feedback.position = left_->getPosition() + right_->getPosition();
240 feedback.effort = left_->getEffort() + right_->getEffort();
241 feedback.reached_goal =
false;
242 feedback.stalled =
false;
243 server_->publishFeedback(feedback);
246 if (fabs(feedback.position - goal->command.position) < 0.002)
248 result.position = feedback.position;
249 result.effort = feedback.effort;
250 result.reached_goal =
true;
251 result.stalled =
false;
253 server_->setSucceeded(result);
258 if (fabs(feedback.position - last_position) > 0.005)
260 last_position = feedback.position;
267 result.position = feedback.position;
268 result.effort = feedback.effort;
269 result.reached_goal =
false;
270 result.stalled =
true;
271 ROS_DEBUG_NAMED(
"ParallelGripperController",
"Gripper stalled, but succeeding.");
272 server_->setSucceeded(result);
281 std::vector<std::string> ParallelGripperController::getCommandedNames()
283 std::vector<std::string> names;
284 names.push_back(left_->getName());
285 names.push_back(right_->getName());
289 std::vector<std::string> ParallelGripperController::getClaimedNames()
292 return getCommandedNames();
std::string getName(void *handle)
#define ROS_DEBUG_NAMED(name,...)
Controller for a parallel jaw gripper, is really only intended for use in simulation.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
const std::string & getNamespace() const
#define ROS_ERROR_NAMED(name,...)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)