00001 #include <ros/ros.h> 00002 #include <pr2_controllers_msgs/JointTrajectoryAction.h> 00003 #include <actionlib/client/simple_action_client.h> 00004 #include <string> 00005 00006 typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient; 00007 00008 class RobotArm 00009 { 00010 private: 00011 // Action client for the joint trajectory action 00012 // used to trigger the arm movement action 00013 TrajClient* left_traj_client_; 00014 TrajClient* right_traj_client_; 00015 std::map<std::string, pr2_controllers_msgs::JointTrajectoryGoal*> goals; 00016 00017 public: 00019 RobotArm() ; 00020 00022 ~RobotArm(); 00024 void startTrajectory(std::string goal_name); 00025 00026 pr2_controllers_msgs::JointTrajectoryGoal* createGoal(std::string lr, std::vector<double> positions); 00027 00029 actionlib::SimpleClientGoalState getState(char lr); 00030 00031 }; 00032