00001 00002 /* 00003 * GraspSponge.cpp 00004 * 00005 * Created on: Sep 27, 2012 00006 * Author: tidyup 00007 */ 00008 00009 #include <ros/ros.h> 00010 #include <pr2_controllers_msgs/JointTrajectoryAction.h> 00011 #include <actionlib/client/simple_action_client.h> 00012 00013 typedef actionlib::SimpleActionClient< 00014 pr2_controllers_msgs::JointTrajectoryAction> TrajClient; 00015 00016 class GraspSponge { 00017 private: 00018 // Action client for the joint trajectory action 00019 // used to trigger the arm movement action 00020 TrajClient* traj_client_; 00021 00022 public: 00024 GraspSponge(); 00025 ~GraspSponge(); 00026 00028 void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal); 00029 pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory(); 00030 00032 GraspSponge::GraspSponge() { 00033 // tell the action client that we want to spin a thread by default 00034 traj_client_ = new TrajClient( 00035 "r_arm_controller/joint_trajectory_action", true); 00036 00037 // wait for action server to come up 00038 while (!traj_client_->waitForServer(ros::Duration(5.0))) { 00039 ROS_INFO("Waiting for the joint_trajectory_action server"); 00040 } 00041 } 00042 }; 00044 GraspSponge::~GraspSponge() { 00045 delete traj_client_; 00046 } 00047 00049 void GraspSponge::startTrajectory( 00050 pr2_controllers_msgs::JointTrajectoryGoal goal) { 00051 // When to start the trajectory: 1s from now 00052 goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0); 00053 traj_client_->sendGoal(goal); 00054 } 00055 00057 00062 pr2_controllers_msgs::JointTrajectoryGoal GraspSponge::armExtensionTrajectory() { 00063 //our goal variable 00064 pr2_controllers_msgs::JointTrajectoryGoal goal; 00065 00066 // First, the joint names, which apply to all waypoints 00067 goal.trajectory.joint_names.push_back("r_shoulder_pan_joint"); 00068 goal.trajectory.joint_names.push_back("r_shoulder_lift_joint"); 00069 goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint"); 00070 goal.trajectory.joint_names.push_back("r_elbow_flex_joint"); 00071 goal.trajectory.joint_names.push_back("r_forearm_roll_joint"); 00072 goal.trajectory.joint_names.push_back("r_wrist_flex_joint"); 00073 goal.trajectory.joint_names.push_back("r_wrist_roll_joint"); 00074 00075 // We will have two waypoints in this goal trajectory 00076 goal.trajectory.points.resize(2); 00077 00078 // First trajectory point 00079 // Positions 00080 int ind = 0; 00081 goal.trajectory.points[ind].positions.resize(7); 00082 goal.trajectory.points[ind].positions[0] = 0.0; 00083 goal.trajectory.points[ind].positions[1] = 0.0; 00084 goal.trajectory.points[ind].positions[2] = 0.0; 00085 goal.trajectory.points[ind].positions[3] = 0.0; 00086 goal.trajectory.points[ind].positions[4] = 0.0; 00087 goal.trajectory.points[ind].positions[5] = 0.0; 00088 goal.trajectory.points[ind].positions[6] = 0.0; 00089 // Velocities 00090 goal.trajectory.points[ind].velocities.resize(7); 00091 for (size_t j = 0; j < 7; ++j) { 00092 goal.trajectory.points[ind].velocities[j] = 0.0; 00093 } 00094 // To be reached 1 second after starting along the trajectory 00095 goal.trajectory.points[ind].time_from_start = ros::Duration(1.0); 00096 00097 // Second trajectory point 00098 // Positions 00099 ind += 1; 00100 goal.trajectory.points[ind].positions.resize(7); 00101 goal.trajectory.points[ind].positions[0] = -0.3; 00102 goal.trajectory.points[ind].positions[1] = 0.2; 00103 goal.trajectory.points[ind].positions[2] = -0.1; 00104 goal.trajectory.points[ind].positions[3] = -1.2; 00105 goal.trajectory.points[ind].positions[4] = 1.5; 00106 goal.trajectory.points[ind].positions[5] = -0.3; 00107 goal.trajectory.points[ind].positions[6] = 0.5; 00108 // Velocities 00109 goal.trajectory.points[ind].velocities.resize(7); 00110 for (size_t j = 0; j < 7; ++j) { 00111 goal.trajectory.points[ind].velocities[j] = 0.0; 00112 } 00113 // To be reached 2 seconds after starting along the trajectory 00114 goal.trajectory.points[ind].time_from_start = ros::Duration(2.0); 00115 00116 //we are done; return the goal 00117 return goal; 00118 } 00119 00121 actionlib::SimpleClientGoalState GraspSponge::getState() { 00122 return traj_client_->getState(); 00123 } 00124 00125 int main(int argc, char** argv) { 00126 // Init the ROS node 00127 ros::init(argc, argv, "robot_driver"); 00128 00129 GraspSponge arm; 00130 // Start the trajectory 00131 arm.startTrajectory(arm.armExtensionTrajectory()); 00132 // Wait for trajectory completion 00133 while (!arm.getState().isDone() && ros::ok()) { 00134 usleep(50000); 00135 } 00136 }