// Close end effector to setpoint
bool setEndEffector(double setpoint)
{
// Error check - servos are alive and we've been recieving messages
if( !endEffectorResponding() )
{
return false;
}
// Check that there is a valid end effector setpoint set
if( setpoint >= END_EFFECTOR_CLOSE_VALUE_MAX &&
setpoint <= END_EFFECTOR_OPEN_VALUE_MAX )
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Unable to set end effector: out of range setpoint of " <<
setpoint << ". Valid range is " << END_EFFECTOR_CLOSE_VALUE_MAX << " to "
<< END_EFFECTOR_OPEN_VALUE_MAX );
return false;
}
// Check if end effector is already close and arm is still
if( ee_status_.target_position == setpoint &&
ee_status_.moving == false &&
ee_status_.position > setpoint + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < setpoint - END_EFFECTOR_POSITION_TOLERANCE )
{
// Consider the ee to already be in the corret position
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector close: already in position");
return true;
}
// Publish command to servos
std_msgs::Float64 joint_value;
joint_value.data = setpoint;
end_effector_pub_.publish(joint_value);
// Wait until end effector is done moving
int timeout = 0;
while( ee_status_.moving == true &&
ee_status_.position > setpoint + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < setpoint - END_EFFECTOR_POSITION_TOLERANCE &&
ros::ok() )
{
// Feedback
feedback_.position = ee_status_.position;
//TODO: fill in more of the feedback
action_server_->publishFeedback(feedback_);
ros::Duration(0.25).sleep();
++timeout;
if( timeout > 16 ) // wait 4 seconds
{
ROS_ERROR_NAMED("clam_gripper_controller","Unable to close end effector: timeout on goal position");
return false;
}
}
return true;
}
// Close end effector
bool closeEndEffector()
{
// Error check - servos are alive and we've been recieving messages
if( !endEffectorResponding() )
{
return false;
}
// Check if end effector is already close and arm is stil
if( ee_status_.target_position == END_EFFECTOR_CLOSE_VALUE_MAX &&
ee_status_.moving == false &&
ee_status_.position > END_EFFECTOR_CLOSE_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_CLOSE_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE )
{
// Consider the ee to already be in the correct position
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector already closed within tolerance, unable to close further");
return true;
}
// Set the velocity for the end effector to a low value
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity low");
velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME);
if( !velocity_client_.waitForExistence(ros::Duration(5.0)) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Timed out waiting for velocity client existance");
return false;
}
dynamixel_hardware_interface::SetVelocity set_velocity_srv;
set_velocity_srv.request.velocity = END_EFFECTOR_MEDIUM_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
std_msgs::Float64 joint_value;
double timeout_sec = 10.0; // time before we declare an error
const double CHECK_INTERVAL = 0.1; // how often we check the load
const double FIRST_BACKOUT_AMOUNT = -0.25;
const double SECOND_BACKOUT_AMOUNT = -0.0075;
const double BACKOUT_AMOUNT[2] = {FIRST_BACKOUT_AMOUNT, SECOND_BACKOUT_AMOUNT};
// Grasp twice - to reduce amount of slips and ensure better grip
for(int i = 0; i < 2; ++i)
{
timeout_sec = 10; // reset timeout;
ROS_DEBUG_STREAM("Grasping with end effector - grasp number " << i + 1);
// Tell servos to start closing slowly to max amount
joint_value.data = END_EFFECTOR_CLOSE_VALUE_MAX;
end_effector_pub_.publish(joint_value);
// Wait until end effector is done moving
while( ee_status_.position < joint_value.data - END_EFFECTOR_POSITION_TOLERANCE ||
ee_status_.position > joint_value.data + END_EFFECTOR_POSITION_TOLERANCE )
{
ros::spinOnce(); // Allows ros to get the latest servo message - we need the load
// Check if load has peaked
if( ee_status_.load < END_EFFECTOR_LOAD_SETPOINT ) // we have touched object!
{
// Open the gripper back up a little to reduce the amount of load.
// the first time open it a lot to help with grasp quality
joint_value.data = ee_status_.position + BACKOUT_AMOUNT[i];
// Check that we haven't passed the open limit
if( joint_value.data < END_EFFECTOR_OPEN_VALUE_MAX )
joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;
ROS_DEBUG_NAMED("clam_gripper_controller","Setting end effector setpoint to %f when it was %f", joint_value.data, ee_status_.position);
end_effector_pub_.publish(joint_value);
if( i == 0 ) // give lots of time to pull out the first time
{
ros::Duration(1.00).sleep();
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Sleeping as we publish joint value " << joint_value.data);
set_velocity_srv.request.velocity = END_EFFECTOR_SLOW_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
ros::Duration(1.0).sleep();
}
break;
}
// Debug output
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","" << joint_value.data - END_EFFECTOR_POSITION_TOLERANCE << " < " <<
ee_status_.position << " < " << joint_value.data + END_EFFECTOR_POSITION_TOLERANCE
<< " -- LOAD: " << ee_status_.load );
// Feedback
feedback_.position = ee_status_.position;
//TODO: fill in more of the feedback
action_server_->publishFeedback(feedback_);
// Wait an interval before checking again
ros::Duration(CHECK_INTERVAL).sleep();
// Check if timeout has occured
timeout_sec -= CHECK_INTERVAL;
if( timeout_sec <= 0 )
{
ROS_ERROR_NAMED("clam_gripper_controller","Timeout: Unable to close end effector");
return false;
}
}
}
// DONE
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished closing end effector action");
return true;
}
// Open end effector
bool openEndEffector()
{
// Error check - servos are alive and we've been recieving messages
if( !endEffectorResponding() )
{
return false;
}
// Check if end effector is already open and arm is still
if( ee_status_.target_position == END_EFFECTOR_OPEN_VALUE_MAX &&
ee_status_.moving == false &&
ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE )
{
// Consider the ee to already be in the corret position
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector open: already in position");
return true;
}
// Set the velocity for the end effector servo
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity");
velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME);
while(!velocity_client_.waitForExistence(ros::Duration(10.0)))
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
dynamixel_hardware_interface::SetVelocity set_velocity_srv;
set_velocity_srv.request.velocity = END_EFFECTOR_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
// Publish command to servos
std_msgs::Float64 joint_value;
joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;
end_effector_pub_.publish(joint_value);
// Wait until end effector is done moving
int timeout = 0;
while( ee_status_.moving == true &&
ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE &&
ros::ok() )
{
// Feedback
feedback_.position = ee_status_.position;
//TODO: fill in more of the feedback
action_server_->publishFeedback(feedback_);
// Looping
ros::Duration(0.25).sleep();
++timeout;
if( timeout > 16 ) // wait 4 seconds
{
ROS_ERROR_NAMED("clam_gripper_controller","Unable to open end effector: timeout on goal position");
return false;
}
}
// It worked!
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished end effector action");
return true;
}
