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 follow_joint_trajectory 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(pr2_controllers_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 pr2_controllers_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(ind);
00110   trajectory.points[ind].positions = current_joint_state_;
00111 
00112   // trajectory point:
00113   ind++;
00114   trajectory.points[ind].time_from_start = ros::Duration(ind);
00115   trajectory.points[ind].positions = calibration_positions;
00116 
00117   // trajectory point:
00118   ind++;
00119   trajectory.points[ind].time_from_start = ros::Duration(ind);
00120   trajectory.points[ind].positions = straight_up_positions;
00121 
00122   // trajectory point:
00123   ind++;
00124   trajectory.points[ind].time_from_start = ros::Duration(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   pr2_controllers_msgs::JointTrajectoryGoal goal;
00139   goal.trajectory = filterJointTrajectory(trajectory);
00140   return goal;
00141 }
00142 
00144 actionlib::SimpleClientGoalState Pr2JointTrajectoryClient::getState()
00145 {
00146   return traj_client_.getState();
00147 }
00148 
00149 trajectory_msgs::JointTrajectory Pr2JointTrajectoryClient::filterJointTrajectory(
00150     const trajectory_msgs::JointTrajectory &input)
00151 {
00152   ros::service::waitForService("trajectory_filter/filter_trajectory");
00153   arm_navigation_msgs::FilterJointTrajectory::Request req;
00154   arm_navigation_msgs::FilterJointTrajectory::Response res;
00155   ros::ServiceClient filter_trajectory_client_ = nh_.serviceClient<arm_navigation_msgs::FilterJointTrajectory>(
00156       "trajectory_filter/filter_trajectory");
00157 
00158   req.trajectory = input;
00159   req.allowed_time = ros::Duration(1.0);
00160 
00161   if (filter_trajectory_client_.call(req, res))
00162   {
00163     if (res.error_code.val == res.error_code.SUCCESS)
00164       ROS_INFO("Requested trajectory was filtered");
00165     else
00166       ROS_WARN("Requested trajectory was not filtered. Error code: %d", res.error_code.val);
00167   }
00168   else
00169   {
00170     ROS_ERROR("Service call to filter trajectory failed %s", filter_trajectory_client_.getService().c_str());
00171   }
00172 
00173   return res.trajectory;
00174 }
00175 
00176 } /* namespace katana_tutorials */
00177 
00178 int main(int argc, char** argv)
00179 {
00180   // Init the ROS node
00181   ros::init(argc, argv, "pr2_joint_trajectory_client");
00182 
00183   katana_tutorials::Pr2JointTrajectoryClient arm;
00184   // Start the trajectory
00185   arm.startTrajectory(arm.makeArmUpTrajectory());
00186   // Wait for trajectory completion
00187   while (!arm.getState().isDone() && ros::ok())
00188   {
00189     usleep(50000);
00190   }
00191 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


katana_tutorials
Author(s): Martin Günther
autogenerated on Tue May 28 2013 15:16:02