test_gripper_joint_trajectory.cpp
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2011 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * test_gripper_joint_trajectory.cpp
20  *
21  * Created on: 24.10.2011
22  * Author: Karl Glatz <glatz@hs-weingarten.de>
23  * Ravensburg-Weingarten, University of Applied Sciences
24  *
25  * based on: min_max_trajectory.cpp
26  */
27 #include <ros/ros.h>
28 #include <control_msgs/JointTrajectoryAction.h>
30 #include <sensor_msgs/JointState.h>
31 
33 
35 static const double GRIPPER_OPEN_ANGLE = 0.30;
36 
38 static const double GRIPPER_CLOSED_ANGLE = -0.44;
39 
41 //static const double DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD = -0.43;
42 
44 //static const double GRIPPER_OPENING_CLOSING_DURATION = 6.0;
45 
47 {
48 private:
49  // Action client for the joint trajectory action
50  // used to trigger the arm movement action
52 
54 
56  std::vector<std::string> joint_names_;
58  std::vector<double> current_joint_state_;
60 
61 
62  void jointStateCB(const sensor_msgs::JointState::ConstPtr &msg)
63  {
64  std::vector<double> ordered_js;
65 
66  ordered_js.resize(joint_names_.size());
67 
68  for (size_t i = 0; i < joint_names_.size(); ++i)
69  {
70  bool found = false;
71  for (size_t j = 0; j < msg->name.size(); ++j)
72  {
73  if (joint_names_[i] == msg->name[j])
74  {
75  ordered_js[i] = msg->position[j];
76  found = true;
77  break;
78  }
79  }
80  if (!found)
81  return;
82  }
83 
84  ROS_INFO_ONCE("Got joint state!");
85  current_joint_state_ = ordered_js;
86  got_joint_state_ = true;
87  }
88 
89 public:
91  got_joint_state_(false), spinner_(1)
92  {
93  joint_names_.push_back("katana_l_finger_joint");
94  joint_names_.push_back("katana_r_finger_joint");
95 
96  joint_state_sub_ = nh_.subscribe("/joint_states", 1, &TestGripperJointTrajectory::jointStateCB, this);
97  spinner_.start();
98 
99  // tell the action client that we want to spin a thread by default
100  traj_client_ = new TrajClient("katana_arm_controller/gripper_joint_trajectory_action", true);
101 
102  // wait for action server to come up
103  while (!traj_client_->waitForServer(ros::Duration(5.0)) && ros::ok())
104  {
105  ROS_INFO("Waiting for the joint_trajectory_action server");
106  }
107 
108  ROS_INFO("The joint_trajectory_action server is available");
109 
110  }
111 
113  {
114  delete traj_client_;
115  }
116 
118  void startTrajectory(control_msgs::JointTrajectoryGoal goal)
119  {
120  // When to start the trajectory: 1s from now
121  goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
122  traj_client_->sendGoal(goal);
123  }
124 
125  control_msgs::JointTrajectoryGoal closeGoal()
126  {
127  control_msgs::JointTrajectoryGoal goal;
128 
129  // set up the joint names
130  goal.trajectory.joint_names = joint_names_;
131 
132  // set some values
133 
134  // num of points
135  goal.trajectory.points.resize(2);
136 
137  // number of joints in each point
138  goal.trajectory.points[0].positions.resize(2);
139  goal.trajectory.points[0].velocities.resize(2);
140 
141  goal.trajectory.points[0].positions[0] = current_joint_state_[0];
142  goal.trajectory.points[0].velocities[0] = 0.0;
143 
144  goal.trajectory.points[0].positions[1] = current_joint_state_[1];
145  goal.trajectory.points[0].velocities[1] = 0.0;
146 
147  goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
148 
149  // close
150 
151  // number of joints in each point
152  goal.trajectory.points[1].positions.resize(2);
153  goal.trajectory.points[1].velocities.resize(2);
154 
155  goal.trajectory.points[1].positions[0] = GRIPPER_CLOSED_ANGLE;
156  goal.trajectory.points[1].velocities[0] = 0.0;
157 
158  goal.trajectory.points[1].positions[1] = GRIPPER_CLOSED_ANGLE;
159  goal.trajectory.points[1].velocities[1] = 0.0;
160 
161  goal.trajectory.points[1].time_from_start = ros::Duration(3.0);
162 
163 
164  return goal;
165  }
166 
167  control_msgs::JointTrajectoryGoal openGoal()
168  {
169  control_msgs::JointTrajectoryGoal goal;
170 
171  // set up the joint names
172  goal.trajectory.joint_names = joint_names_;
173 
174  // set some values
175 
176  // num of points
177  goal.trajectory.points.resize(2);
178 
179  // number of joints in each point
180  goal.trajectory.points[0].positions.resize(2);
181  goal.trajectory.points[0].velocities.resize(2);
182 
183  goal.trajectory.points[0].positions[0] = current_joint_state_[0];
184  goal.trajectory.points[0].velocities[0] = 0.0;
185 
186  goal.trajectory.points[0].positions[1] = current_joint_state_[1];
187  goal.trajectory.points[0].velocities[1] = 0.0;
188 
189  goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
190 
191  // close
192 
193  // number of joints in each point
194  goal.trajectory.points[1].positions.resize(2);
195  goal.trajectory.points[1].velocities.resize(2);
196 
197  goal.trajectory.points[1].positions[0] = GRIPPER_OPEN_ANGLE;
198  goal.trajectory.points[1].velocities[0] = 0.0;
199 
200  goal.trajectory.points[1].positions[1] = GRIPPER_OPEN_ANGLE;
201  goal.trajectory.points[1].velocities[1] = 0.0;
202 
203  goal.trajectory.points[1].time_from_start = ros::Duration(3.0);
204 
205  return goal;
206  }
207 
210  {
211  return traj_client_->getState();
212  }
213 
214 };
215 
216 int main(int argc, char** argv)
217 {
218 
219  // Init the ROS node
220  ros::init(argc, argv, "test_gripper_joint_trajectory");
221 
223 
224  ROS_INFO("Start, press CRTL+C to stop!");
225  while (ros::ok())
226  {
227 
228  ROS_INFO("Send closeGoal");
229  test.startTrajectory(test.closeGoal());
230  while (!test.getState().isDone() && ros::ok())
231  {
232  usleep(50000);
233  }
234 
235  ROS_INFO("closeGoal %s", test.getState().toString().c_str());
236 
237  usleep(1000000); // 1 sec
238 
239  ROS_INFO("Send openGoal");
240  test.startTrajectory(test.openGoal());
241  while (!test.getState().isDone() && ros::ok())
242  {
243  usleep(50000);
244  }
245 
246  ROS_INFO("openGoal %s", test.getState().toString().c_str());
247 
248  usleep(1000000); // 1 sec
249 
250  }
251 
252  return 0;
253 
254 }
255 
actionlib::SimpleActionClient< control_msgs::JointTrajectoryAction > TrajClient
int main(int argc, char **argv)
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
#define ROS_INFO_ONCE(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
actionlib::SimpleClientGoalState getState()
Returns the current state of the action.
static const double GRIPPER_CLOSED_ANGLE
Constants for gripper fully open or fully closed (should be equal to the value in the urdf descriptio...
std::string toString() const
void jointStateCB(const sensor_msgs::JointState::ConstPtr &msg)
control_msgs::JointTrajectoryGoal closeGoal()
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
static const double GRIPPER_OPEN_ANGLE
Constants for gripper fully open or fully closed (should be equal to the value in the urdf descriptio...
control_msgs::JointTrajectoryGoal openGoal()
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
void startTrajectory(control_msgs::JointTrajectoryGoal goal)
Sends the command to start a given trajectory.
static Time now()
SimpleClientGoalState getState() const
A joint angle below this value indicates there is nothing inside the gripper.
std::vector< std::string > joint_names_


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