void Controller::returnToOriginalPosition()
{
moveHead(0.0, 0.0);
mLock = LOCK_PATH;
moveBase(mOriginalPosition.pose);
waitForLock();
}
task main()
{
waitForStart();
while(true){
getJoystickSettings(joystick);
moveBase(joystick.joy1_x1, joystick.joy1_y1);
lift();
para();
}
}
int HandymanTeleopKey::run(int argc, char **argv)
{
char c;
/////////////////////////////////////////////
// get the console in raw mode
int kfd = 0;
struct termios cooked;
struct termios raw;
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
/////////////////////////////////////////////
showHelp();
ros::init(argc, argv, "handyman_teleop_key");
ros::NodeHandle node_handle;
// Override the default ros sigint handler.
// This must be set after the first NodeHandle is created.
signal(SIGINT, rosSigintHandler);
ros::Rate loop_rate(40);
std::string sub_msg_to_robot_topic_name;
std::string pub_msg_to_moderator_topic_name;
std::string sub_joint_state_topic_name;
std::string pub_base_twist_topic_name;
std::string pub_arm_trajectory_topic_name;
std::string pub_gripper_trajectory_topic_name;
node_handle.param<std::string>("sub_msg_to_robot_topic_name", sub_msg_to_robot_topic_name, "/handyman/message/to_robot");
node_handle.param<std::string>("pub_msg_to_moderator_topic_name", pub_msg_to_moderator_topic_name, "/handyman/message/to_moderator");
node_handle.param<std::string>("sub_joint_state_topic_name", sub_joint_state_topic_name, "/hsrb/joint_states");
node_handle.param<std::string>("pub_base_twist_topic_name", pub_base_twist_topic_name, "/hsrb/opt_command_velocity");
node_handle.param<std::string>("pub_arm_trajectory_topic_name", pub_arm_trajectory_topic_name, "/hsrb/arm_trajectory_controller/command");
node_handle.param<std::string>("pub_gripper_trajectory_topic_name", pub_gripper_trajectory_topic_name, "/hsrb/gripper_trajectory_controller/command");
ros::Subscriber sub_msg = node_handle.subscribe<handyman::HandymanMsg>(sub_msg_to_robot_topic_name, 100, &HandymanTeleopKey::messageCallback, this);
ros::Publisher pub_msg = node_handle.advertise<handyman::HandymanMsg>(pub_msg_to_moderator_topic_name, 10);
ros::Subscriber sub_joint_state = node_handle.subscribe<sensor_msgs::JointState>(sub_joint_state_topic_name, 10, &HandymanTeleopKey::jointStateCallback, this);
ros::Publisher pub_base_twist = node_handle.advertise<geometry_msgs::Twist> (pub_base_twist_topic_name, 10);
ros::Publisher pub_arm_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_arm_trajectory_topic_name, 10);
ros::Publisher pub_gripper_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_gripper_trajectory_topic_name, 10);
const float linear_coef = 0.2f;
const float linear_oblique_coef = 0.141f;
const float angular_coef = 0.5f;
float move_speed = 1.0f;
bool is_hand_open = false;
std::string arm_lift_joint_name = "arm_lift_joint";
std::string arm_flex_joint_name = "arm_flex_joint";
std::string wrist_flex_joint_name = "wrist_flex_joint";
while (ros::ok())
{
if(canReceive(kfd))
{
// get the next event from the keyboard
if(read(kfd, &c, 1) < 0)
{
perror("read():");
exit(EXIT_FAILURE);
}
switch(c)
{
case KEYCODE_0:
{
sendMessage(pub_msg, MSG_I_AM_READY);
break;
}
case KEYCODE_1:
{
sendMessage(pub_msg, MSG_ROOM_REACHED);
break;
}
case KEYCODE_2:
{
sendMessage(pub_msg, MSG_OBJECT_GRASPED);
break;
}
case KEYCODE_3:
{
sendMessage(pub_msg, MSG_TASK_FINISHED);
break;
}
case KEYCODE_6:
{
sendMessage(pub_msg, MSG_DOES_NOT_EXIST);
break;
}
case KEYCODE_9:
{
sendMessage(pub_msg, MSG_GIVE_UP);
break;
}
case KEYCODE_UP:
{
ROS_DEBUG("Go Forward");
moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_DOWN:
{
ROS_DEBUG("Go Backward");
moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_RIGHT:
{
ROS_DEBUG("Go Right");
moveBase(pub_base_twist, 0.0, 0.0, -angular_coef*move_speed);
break;
}
case KEYCODE_LEFT:
{
ROS_DEBUG("Go Left");
moveBase(pub_base_twist, 0.0, 0.0, +angular_coef*move_speed);
break;
}
case KEYCODE_S:
{
ROS_DEBUG("Stop");
moveBase(pub_base_twist, 0.0, 0.0, 0.0);
break;
}
case KEYCODE_U:
{
ROS_DEBUG("Move Left Forward");
moveBase(pub_base_twist, +linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_I:
{
ROS_DEBUG("Move Forward");
moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_O:
{
ROS_DEBUG("Move Right Forward");
moveBase(pub_base_twist, +linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_J:
{
ROS_DEBUG("Move Left");
moveBase(pub_base_twist, 0.0, +linear_coef*move_speed, 0.0);
break;
}
case KEYCODE_K:
{
ROS_DEBUG("Stop");
moveBase(pub_base_twist, 0.0, 0.0, 0.0);
break;
}
case KEYCODE_L:
{
ROS_DEBUG("Move Right");
moveBase(pub_base_twist, 0.0, -linear_coef*move_speed, 0.0);
break;
}
case KEYCODE_M:
{
ROS_DEBUG("Move Left Backward");
moveBase(pub_base_twist, -linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_COMMA:
{
ROS_DEBUG("Move Backward");
moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_PERIOD:
{
ROS_DEBUG("Move Right Backward");
moveBase(pub_base_twist, -linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_Q:
{
ROS_DEBUG("Move Speed Up");
move_speed *= 2;
if(move_speed > 2 ){ move_speed=2; }
break;
}
case KEYCODE_Z:
{
ROS_DEBUG("Move Speed Down");
move_speed /= 2;
if(move_speed < 0.125){ move_speed=0.125; }
break;
}
case KEYCODE_Y:
{
ROS_DEBUG("Move Arm Up");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.69, std::max<int>((int)(std::abs(0.69 - arm_lift_joint_pos1_) / 0.05), 1));
break;
}
case KEYCODE_H:
{
ROS_DEBUG("Stop Arm");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 2.0*arm_lift_joint_pos1_-arm_lift_joint_pos2_, 0.5);
break;
}
case KEYCODE_N:
{
ROS_DEBUG("Move Arm Down");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.0, std::max<int>((int)(std::abs(0.0 - arm_lift_joint_pos1_) / 0.05), 1));
break;
}
case KEYCODE_A:
{
ROS_DEBUG("Rotate Arm - Vertical");
moveArm(pub_arm_trajectory, arm_flex_joint_name, 0.0, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, -1.57, 1);
break;
}
case KEYCODE_B:
{
ROS_DEBUG("Rotate Arm - Upward");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -0.785, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, -0.785, 1);
break;
}
case KEYCODE_C:
{
ROS_DEBUG("Rotate Arm - Horizontal");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -1.57, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.0, 1);
break;
}
case KEYCODE_D:
{
ROS_DEBUG("Rotate Arm - Downward");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -2.2, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.35, 1);
break;
}
case KEYCODE_G:
{
moveHand(pub_gripper_trajectory, is_hand_open);
is_hand_open = !is_hand_open;
break;
}
}
}
ros::spinOnce();
loop_rate.sleep();
}
/////////////////////////////////////////////
// cooked mode
tcsetattr(kfd, TCSANOW, &cooked);
/////////////////////////////////////////////
return EXIT_SUCCESS;
}
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;
}
