pr2_joint_trajectory_client.cpp
Go to the documentation of this file.
1 /*
2  * follow_joint_trajectory_client.cpp
3  *
4  * Created on: 06.11.2011
5  * Author: martin
6  */
7 
9 
10 namespace katana_tutorials
11 {
12 
14  traj_client_("/katana_arm_controller/joint_trajectory_action", true), got_joint_state_(false), spinner_(1)
15 {
16  joint_names_.push_back("katana_motor1_pan_joint");
17  joint_names_.push_back("katana_motor2_lift_joint");
18  joint_names_.push_back("katana_motor3_lift_joint");
19  joint_names_.push_back("katana_motor4_lift_joint");
20  joint_names_.push_back("katana_motor5_wrist_roll_joint");
21 
23  spinner_.start();
24 
25  // wait for action server to come up
27  {
28  ROS_INFO("Waiting for the joint_trajectory_action server");
29  }
30 }
31 
33 {
34 }
35 
36 void Pr2JointTrajectoryClient::jointStateCB(const sensor_msgs::JointState::ConstPtr &msg)
37 {
38  std::vector<double> ordered_js;
39 
40  ordered_js.resize(joint_names_.size());
41 
42  for (size_t i = 0; i < joint_names_.size(); ++i)
43  {
44  bool found = false;
45  for (size_t j = 0; j < msg->name.size(); ++j)
46  {
47  if (joint_names_[i] == msg->name[j])
48  {
49  ordered_js[i] = msg->position[j];
50  found = true;
51  break;
52  }
53  }
54  if (!found)
55  return;
56  }
57 
58  ROS_INFO_ONCE("Got joint state!");
59  current_joint_state_ = ordered_js;
60  got_joint_state_ = true;
61 }
62 
64 void Pr2JointTrajectoryClient::startTrajectory(control_msgs::JointTrajectoryGoal goal)
65 {
66  // When to start the trajectory: 1s from now
67  goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
68  traj_client_.sendGoal(goal);
69 }
70 
71 control_msgs::JointTrajectoryGoal Pr2JointTrajectoryClient::makeArmUpTrajectory()
72 {
73  const size_t NUM_TRAJ_POINTS = 4;
74  const size_t NUM_JOINTS = 5;
75 
76  // positions after calibration
77  std::vector<double> calibration_positions(NUM_JOINTS);
78  calibration_positions[0] = -2.96;
79  calibration_positions[1] = 2.14;
80  calibration_positions[2] = -2.16;
81  calibration_positions[3] = -1.97;
82  calibration_positions[4] = -2.93;
83 
84  // arm pointing straight up
85  std::vector<double> straight_up_positions(NUM_JOINTS);
86  straight_up_positions[0] = 0.0;
87  straight_up_positions[1] = 1.57;
88  straight_up_positions[2] = 0.0;
89  straight_up_positions[3] = 0.0;
90  straight_up_positions[4] = 0.0;
91 
92  trajectory_msgs::JointTrajectory trajectory;
93 
94  for (ros::Rate r = ros::Rate(10); !got_joint_state_; r.sleep())
95  {
96  ROS_DEBUG("waiting for joint state...");
97 
98  if (!ros::ok())
99  exit(-1);
100  }
101 
102  // First, the joint names, which apply to all waypoints
103  trajectory.joint_names = joint_names_;
104 
105  trajectory.points.resize(NUM_TRAJ_POINTS);
106 
107  // trajectory point:
108  int ind = 0;
109  trajectory.points[ind].time_from_start = ros::Duration(5 * ind);
110  trajectory.points[ind].positions = current_joint_state_;
111 
112  // trajectory point:
113  ind++;
114  trajectory.points[ind].time_from_start = ros::Duration(5 * ind);
115  trajectory.points[ind].positions = calibration_positions;
116 
117  // trajectory point:
118  ind++;
119  trajectory.points[ind].time_from_start = ros::Duration(5 * ind);
120  trajectory.points[ind].positions = straight_up_positions;
121 
122  // trajectory point:
123  ind++;
124  trajectory.points[ind].time_from_start = ros::Duration(5 * ind);
125  trajectory.points[ind].positions.resize(NUM_JOINTS);
126  trajectory.points[ind].positions = calibration_positions;
127 
128  // // all Velocities 0
129  // for (size_t i = 0; i < NUM_TRAJ_POINTS; ++i)
130  // {
131  // trajectory.points[i].velocities.resize(NUM_JOINTS);
132  // for (size_t j = 0; j < NUM_JOINTS; ++j)
133  // {
134  // trajectory.points[i].velocities[j] = 0.0;
135  // }
136  // }
137 
138  control_msgs::JointTrajectoryGoal goal;
139  goal.trajectory = trajectory;
140  return goal;
141 }
142 
145 {
146  return traj_client_.getState();
147 }
148 
149 } /* namespace katana_tutorials */
150 
151 int main(int argc, char** argv)
152 {
153  // Init the ROS node
154  ros::init(argc, argv, "pr2_joint_trajectory_client");
155 
157  // Start the trajectory
159  // Wait for trajectory completion
160  while (!arm.getState().isDone() && ros::ok())
161  {
162  usleep(50000);
163  }
164 }
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
#define ROS_INFO_ONCE(...)
void startTrajectory(control_msgs::JointTrajectoryGoal goal)
Sends the command to start a given trajectory.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
control_msgs::JointTrajectoryGoal makeArmUpTrajectory()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
actionlib::SimpleClientGoalState getState()
Returns the current state of the action.
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
static Time now()
SimpleClientGoalState getState() const
void jointStateCB(const sensor_msgs::JointState::ConstPtr &msg)
#define ROS_DEBUG(...)


katana_tutorials
Author(s): Martin Günther
autogenerated on Fri Jun 7 2019 22:06:41