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; }