pr2_joint_trajectory_client.cpp
Go to the documentation of this file.
00001 /*
00002  * follow_joint_trajectory_client.cpp
00003  *
00004  *  Created on: 06.11.2011
00005  *      Author: martin
00006  */
00007 
00008 #include <katana_tutorials/pr2_joint_trajectory_client.h>
00009 
00010 namespace katana_tutorials
00011 {
00012 
00013 Pr2JointTrajectoryClient::Pr2JointTrajectoryClient() :
00014     traj_client_("/katana_arm_controller/joint_trajectory_action", true), got_joint_state_(false), spinner_(1)
00015 {
00016   joint_names_.push_back("katana_motor1_pan_joint");
00017   joint_names_.push_back("katana_motor2_lift_joint");
00018   joint_names_.push_back("katana_motor3_lift_joint");
00019   joint_names_.push_back("katana_motor4_lift_joint");
00020   joint_names_.push_back("katana_motor5_wrist_roll_joint");
00021 
00022   joint_state_sub_ = nh_.subscribe("/joint_states", 1, &Pr2JointTrajectoryClient::jointStateCB, this);
00023   spinner_.start();
00024 
00025   // wait for action server to come up
00026   while (!traj_client_.waitForServer(ros::Duration(5.0)))
00027   {
00028     ROS_INFO("Waiting for the joint_trajectory_action server");
00029   }
00030 }
00031 
00032 Pr2JointTrajectoryClient::~Pr2JointTrajectoryClient()
00033 {
00034 }
00035 
00036 void Pr2JointTrajectoryClient::jointStateCB(const sensor_msgs::JointState::ConstPtr &msg)
00037 {
00038   std::vector<double> ordered_js;
00039 
00040   ordered_js.resize(joint_names_.size());
00041 
00042   for (size_t i = 0; i < joint_names_.size(); ++i)
00043   {
00044     bool found = false;
00045     for (size_t j = 0; j < msg->name.size(); ++j)
00046     {
00047       if (joint_names_[i] == msg->name[j])
00048       {
00049         ordered_js[i] = msg->position[j];
00050         found = true;
00051         break;
00052       }
00053     }
00054     if (!found)
00055       return;
00056   }
00057 
00058   ROS_INFO_ONCE("Got joint state!");
00059   current_joint_state_ = ordered_js;
00060   got_joint_state_ = true;
00061 }
00062 
00064 void Pr2JointTrajectoryClient::startTrajectory(control_msgs::JointTrajectoryGoal goal)
00065 {
00066   // When to start the trajectory: 1s from now
00067   goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00068   traj_client_.sendGoal(goal);
00069 }
00070 
00071 control_msgs::JointTrajectoryGoal Pr2JointTrajectoryClient::makeArmUpTrajectory()
00072 {
00073   const size_t NUM_TRAJ_POINTS = 4;
00074   const size_t NUM_JOINTS = 5;
00075 
00076   // positions after calibration
00077   std::vector<double> calibration_positions(NUM_JOINTS);
00078   calibration_positions[0] = -2.96;
00079   calibration_positions[1] = 2.14;
00080   calibration_positions[2] = -2.16;
00081   calibration_positions[3] = -1.97;
00082   calibration_positions[4] = -2.93;
00083 
00084   // arm pointing straight up
00085   std::vector<double> straight_up_positions(NUM_JOINTS);
00086   straight_up_positions[0] = 0.0;
00087   straight_up_positions[1] = 1.57;
00088   straight_up_positions[2] = 0.0;
00089   straight_up_positions[3] = 0.0;
00090   straight_up_positions[4] = 0.0;
00091 
00092   trajectory_msgs::JointTrajectory trajectory;
00093 
00094   for (ros::Rate r = ros::Rate(10); !got_joint_state_; r.sleep())
00095   {
00096     ROS_DEBUG("waiting for joint state...");
00097 
00098     if (!ros::ok())
00099       exit(-1);
00100   }
00101 
00102   // First, the joint names, which apply to all waypoints
00103   trajectory.joint_names = joint_names_;
00104 
00105   trajectory.points.resize(NUM_TRAJ_POINTS);
00106 
00107   // trajectory point:
00108   int ind = 0;
00109   trajectory.points[ind].time_from_start = ros::Duration(5 * ind);
00110   trajectory.points[ind].positions = current_joint_state_;
00111 
00112   // trajectory point:
00113   ind++;
00114   trajectory.points[ind].time_from_start = ros::Duration(5 * ind);
00115   trajectory.points[ind].positions = calibration_positions;
00116 
00117   // trajectory point:
00118   ind++;
00119   trajectory.points[ind].time_from_start = ros::Duration(5 * ind);
00120   trajectory.points[ind].positions = straight_up_positions;
00121 
00122   // trajectory point:
00123   ind++;
00124   trajectory.points[ind].time_from_start = ros::Duration(5 * ind);
00125   trajectory.points[ind].positions.resize(NUM_JOINTS);
00126   trajectory.points[ind].positions = calibration_positions;
00127 
00128   //  // all Velocities 0
00129   //  for (size_t i = 0; i < NUM_TRAJ_POINTS; ++i)
00130   //  {
00131   //    trajectory.points[i].velocities.resize(NUM_JOINTS);
00132   //    for (size_t j = 0; j < NUM_JOINTS; ++j)
00133   //    {
00134   //      trajectory.points[i].velocities[j] = 0.0;
00135   //    }
00136   //  }
00137 
00138   control_msgs::JointTrajectoryGoal goal;
00139   goal.trajectory = trajectory;
00140   return goal;
00141 }
00142 
00144 actionlib::SimpleClientGoalState Pr2JointTrajectoryClient::getState()
00145 {
00146   return traj_client_.getState();
00147 }
00148 
00149 } /* namespace katana_tutorials */
00150 
00151 int main(int argc, char** argv)
00152 {
00153   // Init the ROS node
00154   ros::init(argc, argv, "pr2_joint_trajectory_client");
00155 
00156   katana_tutorials::Pr2JointTrajectoryClient arm;
00157   // Start the trajectory
00158   arm.startTrajectory(arm.makeArmUpTrajectory());
00159   // Wait for trajectory completion
00160   while (!arm.getState().isDone() && ros::ok())
00161   {
00162     usleep(50000);
00163   }
00164 }


katana_tutorials
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:00