/**
* Opens or closes the gripper
*/
void UltrasoneController::commandCB(const std_msgs::Bool& msg)
{
std_msgs::Bool bool_msg;
bool_msg.data = !msg.data;
mPingCommand_pub.publish(bool_msg);
if (msg.data)
setGripper(msg.data);
}
/// Open and close the grippers of the legs to match the grasping actions
/// in the given set of trajectories. It is assumed that the legs alternate
/// between open an close. leftLegFixed=true indicates that the left leg
/// gripper should be locked during the first trajectory.
void openAndCloseGrippers(std::vector<moveit_msgs::RobotTrajectory>& trajectories, bool leftLegFixed)
{
// Open and close the appropriate grippers between each trajectory
std::vector<moveit_msgs::RobotTrajectory> newTrajectories;
for(size_t i = 0; i < trajectories.size(); ++i)
{
// open the gripper for the leg that is moving during this trajectory
moveit_msgs::RobotTrajectory openGripperTrajectory;
setGripper(openGripperTrajectory, leftLegFixed, true);
newTrajectories.push_back(openGripperTrajectory);
newTrajectories.push_back(trajectories[i]);
// close the gripper for the leg that moved in trajectories[i]
moveit_msgs::RobotTrajectory closeGripperTrajectory;
setGripper(closeGripperTrajectory, leftLegFixed, false);
newTrajectories.push_back(closeGripperTrajectory);
leftLegFixed = !leftLegFixed;
}
trajectories.swap(newTrajectories);
}
/**
* Release gripper and deliver the object
*/
uint8_t Controller::release()
{
mBusy = true;
//open gripper
setGripper(true);
usleep(1000000);
//Close gripper and move arm back
std_msgs::Bool bool_msg;
bool_msg.data = true;
mGripperClosePublisher.publish(bool_msg);
moveArm(MIN_ARM_X_VALUE, MIN_ARM_Z_VALUE);
//Reset arousal and listen to feedback
mArousal = NEUTRAL_AROUSAL;
double currentTime = ros::Time::now().toSec();
int sleep_rate;
mNodeHandle.param<int>("node_sleep_rate", sleep_rate, 50);
ros::Rate sleep(sleep_rate);
//wait for feedback
while(ros::ok() && ros::Time::now().toSec() - currentTime < 30 && mArousal == NEUTRAL_AROUSAL)
{
sleep.sleep();
ros::spinOnce();
}
if(mArousal > NEUTRAL_AROUSAL)
{
mBusy = false;
return nero_msgs::Emotion::HAPPY;
}
else if(mArousal < NEUTRAL_AROUSAL)
{
mBusy = false;
return nero_msgs::Emotion::SAD;
}
else
{
setFocusFace(false);
mBusy = false;
return nero_msgs::Emotion::NEUTRAL;
}
}
/**
* Called when new sensor data is made available.
*/
void UltrasoneController::readSensorDataCB(const std_msgs::UInt16& msg)
{
ROS_INFO("I heard: [%d]", msg.data);
// default message, open the gripper
nero_msgs::GripperControl mc;
mc.mode = CM_TORQUE_MODE;
mc.value = AUTOMATIC_GRIPPER_TORQUE;
mc.waitTime = GRIPPER_WAIT_TIME;
// Is the distance smaller than 10cm and are we not grabbing the object yet? Then close the gripper
if (msg.data < CLOSE_GRIPPER_DISTANCE && mGripperState != GS_CLOSED)
{
setGripper(false);
// and stop the ping sensor
std_msgs::Bool bool_msg;
bool_msg.data = false;
mPingCommand_pub.publish(bool_msg);
}
}
uint8_t Controller::get(int object)
{
mBusy = true;
float min_y = 0.f;
mLock = LOCK_ARM;
moveArm(MIN_ARM_X_VALUE, MIN_ARM_Z_VALUE);
waitForLock();
if (mPathingEnabled)
{
//Go to the table
updateRobotPosition();
moveHead(0.0, 0.0);
mLock = LOCK_PATH;
moveBase(mGoal);
waitForLock();
ROS_INFO("Reached Goal");
}
//Aim Kinect to the table
setFocusFace(false);
mLock = LOCK_HEAD;
moveHead(VIEW_OBJECTS_ANGLE, 0.0);
waitForLock();
//Start object recognition process
geometry_msgs::PoseStamped objectPose;
double drive_time = 0.0;
uint8_t attempts = 0;
for (attempts = 0; attempts < MAX_GRAB_ATTEMPTS; attempts++)
{
// start depth for object grabbing (takes a second to start up, so we put it here)
setDepthForced(true);
uint8_t i = 0;
for (i = 0; i < 3; i++)
{
mLock = LOCK_HEAD;
switch (i)
{
case 0: moveHead(VIEW_OBJECTS_ANGLE, 0.0); break;
case 1: moveHead(VIEW_OBJECTS_ANGLE, MIN_VIEW_ANGLE); break;
case 2: moveHead(VIEW_OBJECTS_ANGLE, MAX_VIEW_ANGLE); break;
}
waitForLock();
if (findObject(object, objectPose, min_y) && objectPose.pose.position.y != 0.0)
{
// object found with turned head? rotate base
if (i != 0)
{
mLock = LOCK_BASE;
moveHead(VIEW_OBJECTS_ANGLE, 0.0);
switch (i)
{
case 1: rotateBase(MIN_VIEW_ANGLE); break;
case 2: rotateBase(MAX_VIEW_ANGLE); break;
}
waitForLock();
if (findObject(object, objectPose, min_y) == false || objectPose.pose.position.y == 0.0)
{
ROS_ERROR("Failed to find object, quitting script.");
if (mPathingEnabled)
returnToOriginalPosition();
setDepthForced(false);
return nero_msgs::Emotion::SAD;
}
}
break;
}
}
// found an object, now rotate base to face the object directly and
// create enough space between the robot and the table for the arm to move up
double yaw = -atan(objectPose.pose.position.x / objectPose.pose.position.y);
while (fabs(yaw) > TARGET_YAW_THRESHOLD || fabs(TABLE_DISTANCE - min_y) > TABLE_DISTANCE_THRESHOLD)
{
if (fabs(TABLE_DISTANCE - min_y) > TABLE_DISTANCE_THRESHOLD)
{
ROS_INFO("Moving by %lf. Distance: %lf", min_y - TABLE_DISTANCE, min_y);
mLock = LOCK_BASE;
positionBase(min_y - TABLE_DISTANCE);
waitForLock();
}
else
{
ROS_INFO("Rotating by %lf.", yaw);
mLock = LOCK_BASE;
rotateBase(yaw);
waitForLock();
}
if (findObject(object, objectPose, min_y) == false || objectPose.pose.position.y == 0.0)
{
ROS_ERROR("Failed to find object, quitting script.");
if (mPathingEnabled)
returnToOriginalPosition();
return nero_msgs::Emotion::SAD;
}
yaw = -atan(objectPose.pose.position.x / objectPose.pose.position.y);
}
setDepthForced(false); // we are done with object finding
ROS_INFO("yaw: %lf", yaw);
ROS_INFO("distance: %lf", fabs(TABLE_DISTANCE - min_y));
//Move arm to object and grab it
objectPose.pose.position.x = 0.0; // we are straight ahead of the object, so this should be 0.0 (it is most likely already close to 0.0)
mLock = LOCK_ARM;
moveArm(objectPose);
waitForLock();
//Be ready to grab object
setGripper(true);
usleep(1000000);
setGripper(false);
//Move forward to grab the object
double expected_drive_time = (objectPose.pose.position.y - ARM_LENGTH) / GRAB_TARGET_SPEED;
drive_time = positionBaseSpeed(expected_drive_time + EXTRA_GRAB_TIME, GRAB_TARGET_SPEED, true);
if (drive_time >= expected_drive_time + EXTRA_GRAB_TIME)
{
ROS_ERROR("Been driving forward too long!");
// open gripper again
setGripper(true);
expressEmotion(nero_msgs::Emotion::SAD);
positionBaseSpeed(drive_time, -GRAB_TARGET_SPEED);
//Move object to body
mLock = LOCK_ARM;
moveArm(MIN_ARM_X_VALUE, MIN_ARM_Z_VALUE);
waitForLock();
expressEmotion(nero_msgs::Emotion::NEUTRAL);
}
else
break;
}
// no more attempts left, we are sad
if (attempts >= MAX_GRAB_ATTEMPTS)
{
ROS_ERROR("No more grab attempts left.");
if (mPathingEnabled)
returnToOriginalPosition();
return nero_msgs::Emotion::SAD;
}
// little hack to allow the gripper to close
usleep(500000);
//Lift object
mLock = LOCK_ARM;
moveArm(objectPose.pose.position.x, objectPose.pose.position.z + LIFT_OBJECT_DISTANCE);
waitForLock();
//Move away from table
positionBaseSpeed(drive_time, -GRAB_TARGET_SPEED);
//Move object to body
mLock = LOCK_ARM;
moveArm(MIN_ARM_X_VALUE, MIN_ARM_Z_VALUE);
waitForLock();
if (mPathingEnabled)
returnToOriginalPosition();
moveHead(FOCUS_FACE_ANGLE, 0.0);
//Deliver the juice
//mLock = LOCK_ARM;
//moveArm(DELIVER_ARM_X_VALUE, DELIVER_ARM_Z_VALUE);
//waitForLock();
mBusy = false;
return nero_msgs::Emotion::HAPPY;
}
