00001 #include "rail_grasping/graspObject.h"
00002
00003 using namespace std;
00004
00005 graspObject::graspObject() :
00006 acPickup("jaco_arm/manipulation/pickup"),
00007 acGrasp("jaco_arm/manipulation/grasp"),
00008 acJointTrajectory("jaco_arm/joint_velocity_controller/trajectory"),
00009 acMoveArm("carl_moveit_wrapper/move_to_pose")
00010 {
00011 ROS_INFO("Starting rail grasping...");
00012 graspTransform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00013 graspTransform.setRotation(tf::Quaternion(0, 0, 0, 1));
00014
00015 cartesianCommandPub = n.advertise<wpi_jaco_msgs::CartesianCommand>("jaco_arm/cartesian_cmd", 1);
00016
00017 armJointSubscriber = n.subscribe("jaco_arm/joint_states", 1, &graspObject::armJointStatesCallback, this);
00018
00019 IKClient = n.serviceClient<carl_moveit::CallIK>("carl_moveit_wrapper/call_ik");
00020 cartesianPositionClient = n.serviceClient<wpi_jaco_msgs::GetCartesianPosition>("jaco_arm/get_cartesian_position");
00021 cartesianPathClient = n.serviceClient<carl_moveit::CartesianPath>("carl_moveit_wrapper/cartesian_path");
00022
00023 jointNamesSet = false;
00024 armJointPos.resize(NUM_JACO_JOINTS);
00025 armJointNames.resize(NUM_JACO_JOINTS);
00026
00027 while (!acPickup.waitForServer(ros::Duration(5.0)) &&
00028 !acGrasp.waitForServer(ros::Duration(5.0)) &&
00029 !acJointTrajectory.waitForServer(ros::Duration(5.0)))
00030 {
00031 ROS_INFO("Waiting for grasp, pickup, and arm trajectory action servers...");
00032 }
00033
00034 requestGraspServer = n.advertiseService("rail_grasping/request_grasp", &graspObject::requestGrasp, this);
00035 requestReleaseServer = n.advertiseService("rail_grasping/request_release", &graspObject::requestRelease, this);
00036
00037 ROS_INFO("Rail grasping started.");
00038 }
00039
00040 void graspObject::armJointStatesCallback(const sensor_msgs::JointState &msg)
00041 {
00042 for (unsigned int i = 0; i < NUM_JACO_JOINTS; i++)
00043 {
00044 armJointPos[i] = msg.position[i];
00045 }
00046
00047 if (!jointNamesSet)
00048 {
00049 for (unsigned int i = 0; i < NUM_JACO_JOINTS; i++)
00050 {
00051 armJointNames[i] = msg.name[i];
00052 }
00053 jointNamesSet = true;
00054 }
00055 }
00056
00057 bool graspObject::requestRelease(rail_grasping::RequestGrasp::Request &req, rail_grasping::RequestGrasp::Response &res)
00058 {
00059 ROS_INFO("Received new release request...");
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00089
00091
00092
00093
00094
00095
00096 geometry_msgs::Pose releasePose = req.graspPose;
00097 releasePose.position.z += .05;
00098
00099
00100 bool success = false;
00101 int counter = 0;
00102 while (!success)
00103 {
00104 ROS_INFO("Moving to approach angle...");
00105 carl_moveit::MoveToPoseGoal movePoseGoal;
00106 movePoseGoal.pose = releasePose;
00107 switch (counter)
00108 {
00109 case 0:
00110 movePoseGoal.pose.position.x += .005;
00111 break;
00112 case 1:
00113 movePoseGoal.pose.position.x -= .005;
00114 break;
00115 case 2:
00116 movePoseGoal.pose.position.y += .005;
00117 break;
00118 case 3:
00119 movePoseGoal.pose.position.y -= .005;
00120 break;
00121 case 4:
00122 movePoseGoal.pose.position.z += .005;
00123 break;
00124 case 5:
00125 movePoseGoal.pose.position.z -= .005;
00126 break;
00127 case 6:
00128 movePoseGoal.pose.position.x += .005;
00129 movePoseGoal.pose.position.y += .005;
00130 break;
00131 case 7:
00132 movePoseGoal.pose.position.x += .005;
00133 movePoseGoal.pose.position.y -= .005;
00134 break;
00135 case 8:
00136 movePoseGoal.pose.position.x -= .005;
00137 movePoseGoal.pose.position.y += .005;
00138 break;
00139 case 9:
00140 movePoseGoal.pose.position.x -= .005;
00141 movePoseGoal.pose.position.y -= .005;
00142 break;
00143 }
00144 acMoveArm.sendGoal(movePoseGoal);
00145 ROS_INFO("Approach angle arm move initiated.");
00146 acMoveArm.waitForResult(ros::Duration(20.0));
00147 carl_moveit::MoveToPoseResultConstPtr movePoseResult = acMoveArm.getResult();
00148
00149 counter++;
00150
00151 res.earlyFailureDetected = !movePoseResult->success;
00152 if (!res.earlyFailureDetected)
00153 {
00154 success = true;
00155 res.result = true;
00156 }
00157 else if (res.earlyFailureDetected && counter > 9)
00158 {
00159 ROS_INFO("Detected failure on moving to approach angle.");
00160 res.result = false;
00161 return false;
00162 }
00163 }
00164
00166 wpi_jaco_msgs::GetCartesianPosition::Request ikReq;
00167 wpi_jaco_msgs::GetCartesianPosition::Response ikRes;
00168 cartesianPositionClient.call(ikReq, ikRes);
00169
00170 wpi_jaco_msgs::CartesianCommand cartesianCmd;
00171 cartesianCmd.position = true;
00172 cartesianCmd.armCommand = true;
00173 cartesianCmd.fingerCommand = false;
00174 cartesianCmd.repeat = false;
00175 cartesianCmd.arm = ikRes.pos;
00176 cartesianCmd.arm.linear.z -= .05;
00177 cartesianCommandPub.publish(cartesianCmd);
00178
00179 ros::Duration(5.0).sleep();
00181
00182
00183 ROS_INFO("Fully opening gripper...");
00184 wpi_jaco_msgs::ExecuteGraspGoal openGripperGoal;
00185 openGripperGoal.closeGripper = false;
00186 openGripperGoal.limitFingerVelocity = false;
00187 acGrasp.sendGoal(openGripperGoal);
00188 acGrasp.waitForResult(ros::Duration(5.0));
00189 ROS_INFO("Gripper opened.");
00190
00191
00192 ROS_INFO("Lifting hand...");
00193 wpi_jaco_msgs::ExecutePickupGoal pickupGoal;
00194 pickupGoal.limitFingerVelocity = false;
00195 pickupGoal.setLiftVelocity = false;
00196 acPickup.sendGoal(pickupGoal);
00197 acPickup.waitForResult(ros::Duration(10));
00198 ROS_INFO("hand lifting complete.");
00199 }
00200
00201 bool graspObject::requestGrasp(rail_grasping::RequestGrasp::Request &req, rail_grasping::RequestGrasp::Response &res)
00202 {
00203 ROS_INFO("Received new grasp request...");
00204
00205 graspTransform.setOrigin(tf::Vector3(req.graspPose.position.x, req.graspPose.position.y, req.graspPose.position.z));
00206 graspTransform.setRotation(tf::Quaternion(req.graspPose.orientation.x, req.graspPose.orientation.y, req.graspPose.orientation.z, req.graspPose.orientation.w));
00207 tfBroadcaster.sendTransform(tf::StampedTransform(graspTransform, ros::Time::now(), "base_footprint", "grasp_frame"));
00208
00209 ros::Duration(1).sleep();
00210
00211 bool earlyFailureDetected;
00212 res.result = executeGrasp(&earlyFailureDetected);
00213 res.earlyFailureDetected = earlyFailureDetected;
00214
00215 return true;
00216 }
00217
00218 bool graspObject::executeGrasp(bool *earlyFailureFlag)
00219 {
00220 ROS_INFO("\nGrasp execution started");
00221
00222
00223 ROS_INFO("Fully opening gripper...");
00224 wpi_jaco_msgs::ExecuteGraspGoal openGripperGoal;
00225 openGripperGoal.closeGripper = false;
00226 openGripperGoal.limitFingerVelocity = false;
00227 acGrasp.sendGoal(openGripperGoal);
00228 acGrasp.waitForResult(ros::Duration(5.0));
00229 ROS_INFO("Gripper opened.");
00230
00231
00232
00233 geometry_msgs::PoseStamped approachPose;
00234 geometry_msgs::PoseStamped poseOut;
00235 approachPose.header.frame_id = "grasp_frame";
00236 approachPose.pose.position.z = 0.1;
00237 approachPose.pose.orientation.w = 1.0;
00238
00239
00240 approachPose.pose.position.z += .19;
00241
00242 tfListener.transformPose("base_footprint", approachPose, poseOut);
00243
00244
00245 ROS_INFO("Moving to approach angle...");
00246 carl_moveit::MoveToPoseGoal movePoseGoal;
00247 movePoseGoal.pose = poseOut.pose;
00248 acMoveArm.sendGoal(movePoseGoal);
00249 ROS_INFO("Approach angle arm move initiated.");
00250 acMoveArm.waitForResult(ros::Duration(20.0));
00251 carl_moveit::MoveToPoseResultConstPtr movePoseResult = acMoveArm.getResult();
00252
00253 *earlyFailureFlag = !movePoseResult->success;
00254 if (*earlyFailureFlag)
00255 {
00256 ROS_INFO("Detected failure on moving to approach angle.");
00257 return false;
00258 }
00259
00260
00261 carl_moveit::CartesianPath pathSrvObj;
00262 geometry_msgs::PoseStamped pickupPose;
00263 geometry_msgs::PoseStamped pickupPoseOut;
00264 pickupPoseOut.header.frame_id = "base_footprint";
00265 pickupPose.header.frame_id = "grasp_frame";
00266 pickupPose.pose.orientation.w = 1.0;
00267
00268
00269 pickupPose.pose.position.z += .19;
00270
00271 tfListener.transformPose("base_footprint", pickupPose, pickupPoseOut);
00272
00273 tf::StampedTransform currentHandTransform;
00274 pathSrvObj.request.waypoints.push_back(pickupPoseOut.pose);
00275 pathSrvObj.request.avoidCollisions = false;
00276
00277 if (!cartesianPathClient.call(pathSrvObj))
00278 {
00279 ROS_INFO("Failed to call cartesian path planner.");
00280 *earlyFailureFlag = true;
00281 return false;
00282 }
00283
00284
00285 ROS_INFO("Closing gripper...");
00286 wpi_jaco_msgs::ExecuteGraspGoal closeGripperGoal;
00287 closeGripperGoal.closeGripper = true;
00288 closeGripperGoal.limitFingerVelocity = false;
00289 acGrasp.sendGoal(closeGripperGoal);
00290 acGrasp.waitForResult(ros::Duration(5.0));
00291 ROS_INFO("Gripper closed.");
00292
00293
00294
00295
00296
00297 ROS_INFO("Lifting object...");
00298 carl_moveit::CartesianPathRequest pickupPathReq;
00299 carl_moveit::CartesianPathResponse pickupPathRes;
00300 tfListener.waitForTransform("jaco_link_hand", "base_footprint", ros::Time::now(), ros::Duration(1.0));
00301 tfListener.lookupTransform("base_footprint", "jaco_link_hand", ros::Time(0), currentHandTransform);
00302 geometry_msgs::Pose pickupPoseWaypoint;
00303 pickupPoseWaypoint.position.x = currentHandTransform.getOrigin().x();
00304 pickupPoseWaypoint.position.y = currentHandTransform.getOrigin().y();
00305 pickupPoseWaypoint.position.z = currentHandTransform.getOrigin().z() + .085;
00306 pickupPoseWaypoint.orientation.x = currentHandTransform.getRotation().x();
00307 pickupPoseWaypoint.orientation.y = currentHandTransform.getRotation().y();
00308 pickupPoseWaypoint.orientation.z = currentHandTransform.getRotation().z();
00309 pickupPoseWaypoint.orientation.w = currentHandTransform.getRotation().w();
00310 pickupPathReq.waypoints.push_back(pickupPoseWaypoint);
00311 pickupPathReq.avoidCollisions = false;
00312
00313 if (!cartesianPathClient.call(pickupPathReq, pickupPathRes))
00314 {
00315 ROS_INFO("Failed to call cartesian path planner.");
00316 return false;
00317 }
00318
00319 return true;
00320 }
00321
00322 void graspObject::publishGraspFrame()
00323 {
00324 tfBroadcaster.sendTransform(tf::StampedTransform(graspTransform, ros::Time::now(), "base_footprint", "grasp_frame"));
00325 }
00326
00327 int main(int argc, char **argv)
00328 {
00329 ros::init(argc, argv, "rail_grasping");
00330
00331 graspObject go;
00332
00333 ros::Rate loop_rate(30);
00334 while (ros::ok())
00335 {
00336 ros::spinOnce();
00337 go.publishGraspFrame();
00338 loop_rate.sleep();
00339 }
00340
00341 return 0;
00342 }