teleop_pr2.cpp
Go to the documentation of this file.
1 /*
2  * teleop_pr2
3  * Copyright (c) 2008, Willow Garage, Inc.
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * * Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in the
13  * documentation and/or other materials provided with the distribution.
14  * * Neither the name of the <ORGANIZATION> nor the names of its
15  * contributors may be used to endorse or promote products derived from
16  * this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28  * POSSIBILITY OF SUCH DAMAGE.
29  */
30 
33 
34 #include <cstdlib>
35 #include <cstdio>
36 #include <unistd.h>
37 #include <math.h>
38 #include <fcntl.h>
39 #include "ros/ros.h"
40 #include "sensor_msgs/Joy.h"
41 #include "geometry_msgs/Twist.h"
42 #include "sensor_msgs/JointState.h"
43 #include "trajectory_msgs/JointTrajectory.h"
44 #include "pr2_controllers_msgs/JointTrajectoryControllerState.h"
45 #include "topic_tools/MuxSelect.h"
46 #include "std_msgs/String.h"
47 
48 #include "std_msgs/Float64.h"
49 
50 #define TORSO_TOPIC "torso_controller/command"
51 #define HEAD_TOPIC "head_traj_controller/command"
52 const int PUBLISH_FREQ = 20;
53 
54 using namespace std;
55 
56 class TeleopPR2
57 {
58  public:
59  geometry_msgs::Twist cmd;
60  double min_torso, max_torso;
61  double req_torso_vel, torso_step;
62  //joy::Joy joy;
63  double req_vx, req_vy, req_vw, req_torso, req_pan, req_tilt;
64  double req_tilt_vel, req_pan_vel;
65  double max_vx, max_vy, max_vw, max_vx_run, max_vy_run, max_vw_run;
66  double max_pan, max_tilt, min_tilt, pan_step, tilt_step;
67  int axis_vx, axis_vy, axis_vw, axis_pan, axis_tilt;
68  int deadman_button, run_button, torso_dn_button, torso_up_button, head_button;
69  bool deadman_no_publish_, torso_publish_, head_publish_;
70 
71  bool deadman_, cmd_head;
72  bool use_mux_, last_deadman_;
73  std::string last_selected_topic_;
74 
75  sensor_msgs::Joy last_processed_joy_message_;
76 
79 
88 
89  TeleopPR2(bool deadman_no_publish = false) :
90  max_vx(0.6), max_vy(0.6), max_vw(0.8),
91  max_vx_run(0.6), max_vy_run(0.6), max_vw_run(0.8),
92  max_pan(2.7), max_tilt(1.4), min_tilt(-0.4),
93  pan_step(0.02), tilt_step(0.015),
94  deadman_no_publish_(deadman_no_publish),
95  torso_publish_(false), head_publish_(false),
96  deadman_(false), cmd_head(false),
97  use_mux_(false), last_deadman_(false),
98  n_private_("~")
99  { }
100 
101  void init()
102  {
103  cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
104  req_pan = req_tilt = 0;
105  req_torso = 0.0;
106  req_torso_vel = 0.0;
107 
108  //parameters for interaction with a mux on cmd_vel topics
109  n_private_.param("use_mux", use_mux_, false);
110 
111  n_private_.param("max_vx", max_vx, max_vx);
112  n_private_.param("max_vy", max_vy, max_vy);
113  n_private_.param("max_vw", max_vw, max_vw);
114 
115  // Set max speed while running
116  n_private_.param("max_vx_run", max_vx_run, max_vx_run);
117  n_private_.param("max_vy_run", max_vy_run, max_vy_run);
118  n_private_.param("max_vw_run", max_vw_run, max_vw_run);
119 
120  // Head pan/tilt parameters
121  n_private_.param("max_pan", max_pan, max_pan);
122  n_private_.param("max_tilt", max_tilt, max_tilt);
123  n_private_.param("min_tilt", min_tilt, min_tilt);
124 
125  n_private_.param("tilt_step", tilt_step, tilt_step);
126  n_private_.param("pan_step", pan_step, pan_step);
127 
128  n_private_.param("axis_pan", axis_pan, 0);
129  n_private_.param("axis_tilt", axis_tilt, 2);
130 
131  n_private_.param("axis_vx", axis_vx, 3);
132  n_private_.param("axis_vw", axis_vw, 0);
133  n_private_.param("axis_vy", axis_vy, 2);
134 
135  n_private_.param("torso_step", torso_step, 0.01);
136  n_private_.param("min_torso", min_torso, 0.0);
137  n_private_.param("max_torso", max_torso, 0.3);
138 
139  n_private_.param("deadman_button", deadman_button, 0);
140  n_private_.param("run_button", run_button, 0);
141  n_private_.param("torso_dn_button", torso_dn_button, 0);
142  n_private_.param("torso_up_button", torso_up_button, 0);
143  n_private_.param("head_button", head_button, 0);
144 
145  double joy_msg_timeout;
146  n_private_.param("joy_msg_timeout", joy_msg_timeout, 0.5); //default to 0.5 seconds timeout
147  if (joy_msg_timeout <= 0)
148  {
149  joy_msg_timeout_ = ros::Duration().fromSec(9999999);//DURATION_MAX;
150  ROS_DEBUG("joy_msg_timeout <= 0 -> no timeout");
151  }
152  else
153  {
154  joy_msg_timeout_.fromSec(joy_msg_timeout);
155  ROS_DEBUG("joy_msg_timeout: %.3f", joy_msg_timeout_.toSec());
156  }
157 
158  ROS_DEBUG("max_vx: %.3f m/s\n", max_vx);
159  ROS_DEBUG("max_vy: %.3f m/s\n", max_vy);
160  ROS_DEBUG("max_vw: %.3f deg/s\n", max_vw*180.0/M_PI);
161 
162  ROS_DEBUG("max_vx_run: %.3f m/s\n", max_vx_run);
163  ROS_DEBUG("max_vy_run: %.3f m/s\n", max_vy_run);
164  ROS_DEBUG("max_vw_run: %.3f deg/s\n", max_vw_run*180.0/M_PI);
165 
166  ROS_DEBUG("tilt step: %.3f rad\n", tilt_step);
167  ROS_DEBUG("pan step: %.3f rad\n", pan_step);
168 
169  ROS_DEBUG("axis_vx: %d\n", axis_vx);
170  ROS_DEBUG("axis_vy: %d\n", axis_vy);
171  ROS_DEBUG("axis_vw: %d\n", axis_vw);
172  ROS_DEBUG("axis_pan: %d\n", axis_pan);
173  ROS_DEBUG("axis_tilt: %d\n", axis_tilt);
174 
175  ROS_DEBUG("deadman_button: %d\n", deadman_button);
176  ROS_DEBUG("run_button: %d\n", run_button);
177  ROS_DEBUG("torso_dn_button: %d\n", torso_dn_button);
178  ROS_DEBUG("torso_up_button: %d\n", torso_up_button);
179  ROS_DEBUG("head_button: %d\n", head_button);
180  ROS_DEBUG("joy_msg_timeout: %f\n", joy_msg_timeout);
181 
182  if (torso_dn_button != 0 && torso_up_button != 0)
183  {
184  torso_publish_ = true;
185  torso_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(TORSO_TOPIC, 1);
186  }
187 
188  if (head_button != 0)
189  {
190  head_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(HEAD_TOPIC, 1);
191  head_publish_ = true;
192  }
193 
194  vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
195 
196  joy_sub_ = n_.subscribe("joy", 10, &TeleopPR2::joy_cb, this);
197  torso_state_sub_ = n_.subscribe("torso_controller/state", 1, &TeleopPR2::torsoCB, this);
198  head_state_sub_ = n_.subscribe("head_traj_controller/state", 1, &TeleopPR2::headCB, this);
199 
200  //if we're going to use the mux, then we'll subscribe to state changes on the mux
201  if(use_mux_){
202  ros::NodeHandle mux_nh("mux");
203  mux_client_ = mux_nh.serviceClient<topic_tools::MuxSelect>("select");
204  }
205  }
206 
208 
210  void joy_cb(const sensor_msgs::Joy::ConstPtr& joy_msg)
211  {
212  // Do not process the same message twice.
213  if(joy_msg->header.stamp == last_processed_joy_message_.header.stamp) {
214  // notify the user only if the problem persists
215  if(ros::Time::now() - joy_msg->header.stamp > ros::Duration(5.0/PUBLISH_FREQ))
216  ROS_WARN_THROTTLE(1.0, "Received Joy message with same timestamp multiple times. Ignoring subsequent messages.");
217  deadman_ = false;
218  return;
219  }
220  last_processed_joy_message_ = *joy_msg;
221 
222  //Record this message reciept
223  last_recieved_joy_message_time_ = ros::Time::now();
224 
225  deadman_ = (((unsigned int)deadman_button < joy_msg->buttons.size()) && joy_msg->buttons[deadman_button]);
226 
227  if (!deadman_)
228  return;
229 
230  cmd_head = (((unsigned int)head_button < joy_msg->buttons.size()) && joy_msg->buttons[head_button] && head_publish_);
231 
232  // Base
233  bool running = (((unsigned int)run_button < joy_msg->buttons.size()) && joy_msg->buttons[run_button]);
234  double vx = running ? max_vx_run : max_vx;
235  double vy = running ? max_vy_run : max_vy;
236  double vw = running ? max_vw_run : max_vw;
237 
238  if((axis_vx >= 0) && (((unsigned int)axis_vx) < joy_msg->axes.size()) && !cmd_head)
239  req_vx = joy_msg->axes[axis_vx] * vx;
240  else
241  req_vx = 0.0;
242  if((axis_vy >= 0) && (((unsigned int)axis_vy) < joy_msg->axes.size()) && !cmd_head)
243  req_vy = joy_msg->axes[axis_vy] * vy;
244  else
245  req_vy = 0.0;
246  if((axis_vw >= 0) && (((unsigned int)axis_vw) < joy_msg->axes.size()) && !cmd_head)
247  req_vw = joy_msg->axes[axis_vw] * vw;
248  else
249  req_vw = 0.0;
250 
251  // Enforce max/mins for velocity
252  // Joystick should be [-1, 1], but it might not be
253  req_vx = max(min(req_vx, vx), -vx);
254  req_vy = max(min(req_vy, vy), -vy);
255  req_vw = max(min(req_vw, vw), -vw);
256 
257  // Head
258  // Update commanded position by how joysticks moving
259  // Don't add commanded position if deadman off
260  if (deadman_ && cmd_head)
261  {
262  if (axis_pan >= 0 && axis_pan < (int)joy_msg->axes.size())
263  {
264  req_pan_vel = joy_msg->axes[axis_pan] * pan_step;
265  }
266 
267  if (axis_tilt >= 0 && axis_tilt < (int)joy_msg->axes.size())
268  {
269  req_tilt_vel = joy_msg->axes[axis_tilt] * tilt_step;
270  }
271  }
272 
273  // Torso
274  bool down = (((unsigned int)torso_dn_button < joy_msg->buttons.size()) && joy_msg->buttons[torso_dn_button]);
275  bool up = (((unsigned int)torso_up_button < joy_msg->buttons.size()) && joy_msg->buttons[torso_up_button]);
276 
277  // Bring torso up/down
278  if (down && !up)
279  req_torso_vel = -torso_step;
280  else if (up && !down)
281  req_torso_vel = torso_step;
282  else
283  req_torso_vel = 0;
284  }
285 
286 
288  {
289  if(deadman_ &&
290  last_recieved_joy_message_time_ + joy_msg_timeout_ > ros::Time::now())
291  {
292  //check if we need to switch the mux to our topic for teleop
293  if(use_mux_ && !last_deadman_){
294  topic_tools::MuxSelect select_srv;
295  select_srv.request.topic = vel_pub_.getTopic();
296  if(mux_client_.call(select_srv)){
297  last_selected_topic_ = select_srv.response.prev_topic;
298  ROS_DEBUG("Setting mux to %s for teleop", select_srv.request.topic.c_str());
299  }
300  else{
301  ROS_ERROR("Failed to call select service %s on mux. Are you sure that it is up and connected correctly to the teleop node?", mux_client_.getService().c_str());
302  }
303  }
304 
305  // Base
306  cmd.linear.x = req_vx;
307  cmd.linear.y = req_vy;
308  cmd.angular.z = req_vw;
309  vel_pub_.publish(cmd);
310 
311  // Torso
312  if (torso_publish_)
313  {
314  double dt = 1.0/double(PUBLISH_FREQ);
315  double horizon = 5.0 * dt;
316 
317  trajectory_msgs::JointTrajectory traj;
318  traj.header.stamp = ros::Time::now() + ros::Duration(0.01);
319  traj.joint_names.push_back("torso_lift_joint");
320  traj.points.resize(1);
321  traj.points[0].positions.push_back(req_torso + req_torso_vel * horizon);
322  traj.points[0].velocities.push_back(req_torso_vel);
323  traj.points[0].time_from_start = ros::Duration(horizon);
324  torso_pub_.publish(traj);
325 
326  // Updates the current positions
327  req_torso += req_torso_vel * dt;
328  req_torso = max(min(req_torso, max_torso), min_torso);
329  }
330 
331  // Head
332  if (cmd_head && head_publish_)
333  {
334  double dt = 1.0/double(PUBLISH_FREQ);
335  double horizon = 3.0 * dt;
336 
337  trajectory_msgs::JointTrajectory traj;
338  traj.header.stamp = ros::Time::now() + ros::Duration(0.01);
339  traj.joint_names.push_back("head_pan_joint");
340  traj.joint_names.push_back("head_tilt_joint");
341  traj.points.resize(1);
342  traj.points[0].positions.push_back(req_pan + req_pan_vel * horizon);
343  traj.points[0].velocities.push_back(req_pan_vel);
344  traj.points[0].positions.push_back(req_tilt + req_tilt_vel * horizon);
345  traj.points[0].velocities.push_back(req_tilt_vel);
346  traj.points[0].time_from_start = ros::Duration(horizon);
347  head_pub_.publish(traj);
348  }
349 
350  if (req_torso != 0)
351  fprintf(stdout,"teleop_base:: %f, %f, %f. Head:: %f, %f. Torso cmd: %f.\n",
352  cmd.linear.x, cmd.linear.y, cmd.angular.z, req_pan, req_tilt, req_torso_vel);
353  else
354  fprintf(stdout,"teleop_base:: %f, %f, %f. Head:: %f, %f\n",
355  cmd.linear.x ,cmd.linear.y, cmd.angular.z, req_pan, req_tilt);
356  }
357  else
358  {
359  //make sure to set the mux back to whatever topic it was on when we grabbed it if the deadman has just toggled
360  if(use_mux_ && last_deadman_){
361  topic_tools::MuxSelect select_srv;
362  select_srv.request.topic = last_selected_topic_;
363  if(mux_client_.call(select_srv)){
364  ROS_DEBUG("Setting mux back to %s", last_selected_topic_.c_str());
365  }
366  else{
367  ROS_ERROR("Failed to call select service %s on mux. Are you sure that it is up and connected correctly to the teleop node?", mux_client_.getService().c_str());
368  }
369  }
370 
371  // Publish zero commands iff deadman_no_publish is false
372  cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
373  if (!deadman_no_publish_)
374  {
375  // Base
376  vel_pub_.publish(cmd);
377  }
378  }
379 
380  //make sure we store the state of our last deadman
381  last_deadman_ = deadman_;
382  }
383 
384  void torsoCB(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr &msg)
385  {
386  double xd = req_torso;
387  const double A = 0.003;
388  if (fabs(msg->actual.positions[0] - xd) > A*1.001)
389  {
390  req_torso = min(max(msg->actual.positions[0] - A, xd), msg->actual.positions[0] + A);
391  }
392  }
393 
394  void headCB(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr &msg)
395  {
396  // Updates the current positions
397  req_pan = msg->desired.positions[0];
398  req_pan = max(min(req_pan, max_pan), -max_pan);
399  req_tilt = msg->desired.positions[1];
400  req_tilt = max(min(req_tilt, max_tilt), min_tilt);
401  }
402 };
403 
404 int main(int argc, char **argv)
405 {
406  ros::init(argc, argv, "teleop_pr2");
407  const char* opt_no_publish = "--deadman_no_publish";
408 
409  bool no_publish = false;
410  for(int i=1;i<argc;i++)
411  {
412  if(!strncmp(argv[i], opt_no_publish, strlen(opt_no_publish)))
413  no_publish = true;
414  }
415 
416  TeleopPR2 teleop_pr2(no_publish);
417  teleop_pr2.init();
418 
419  ros::Rate pub_rate(PUBLISH_FREQ);
420 
421  while (teleop_pr2.n_.ok())
422  {
423  ros::spinOnce();
424  teleop_pr2.send_cmd_vel();
425  pub_rate.sleep();
426  }
427 
428  exit(0);
429  return 0;
430 }
431 
void joy_cb(const sensor_msgs::Joy::ConstPtr &joy_msg)
Definition: teleop_pr2.cpp:210
int torso_up_button
Definition: teleop_pr2.cpp:68
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::Subscriber torso_state_sub_
Definition: teleop_pr2.cpp:85
void publish(const boost::shared_ptr< M > &message) const
bool torso_publish_
Definition: teleop_pr2.cpp:69
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Publisher head_pub_
Definition: teleop_pr2.cpp:82
TeleopPR2(bool deadman_no_publish=false)
Definition: teleop_pr2.cpp:89
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
#define HEAD_TOPIC
Definition: teleop_pr2.cpp:51
void torsoCB(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr &msg)
Definition: teleop_pr2.cpp:384
double max_vy_run
Definition: teleop_pr2.cpp:65
double req_vy
Definition: teleop_pr2.cpp:63
std::string last_selected_topic_
Definition: teleop_pr2.cpp:73
double req_tilt_vel
Definition: teleop_pr2.cpp:64
double min_torso
Definition: teleop_pr2.cpp:60
ros::Publisher vel_pub_
Definition: teleop_pr2.cpp:81
ros::Subscriber head_state_sub_
Definition: teleop_pr2.cpp:86
void init()
Definition: teleop_pr2.cpp:101
ros::Publisher torso_pub_
Definition: teleop_pr2.cpp:83
double torso_step
Definition: teleop_pr2.cpp:61
#define ROS_WARN_THROTTLE(period,...)
Duration & fromSec(double t)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::NodeHandle n_private_
Definition: teleop_pr2.cpp:80
int main(int argc, char **argv)
Definition: teleop_pr2.cpp:404
void send_cmd_vel()
Definition: teleop_pr2.cpp:287
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Subscriber joy_sub_
Definition: teleop_pr2.cpp:84
bool sleep()
ros::Duration joy_msg_timeout_
Definition: teleop_pr2.cpp:78
int min(int a, int b)
ros::NodeHandle n_
Definition: teleop_pr2.cpp:80
static Time now()
#define TORSO_TOPIC
Converts joystick commands on /joy to commands to PR2 base, spine, head.
Definition: teleop_pr2.cpp:50
std::string getService()
std::string getTopic() const
bool ok() const
geometry_msgs::Twist cmd
Definition: teleop_pr2.cpp:59
double tilt_step
Definition: teleop_pr2.cpp:66
ROSCPP_DECL void spinOnce()
sensor_msgs::Joy last_processed_joy_message_
Definition: teleop_pr2.cpp:75
bool deadman_
Definition: teleop_pr2.cpp:71
void headCB(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr &msg)
Definition: teleop_pr2.cpp:394
ros::ServiceClient mux_client_
Definition: teleop_pr2.cpp:87
#define ROS_ERROR(...)
ros::Time last_recieved_joy_message_time_
Definition: teleop_pr2.cpp:77
const int PUBLISH_FREQ
Definition: teleop_pr2.cpp:52
#define ROS_DEBUG(...)
bool use_mux_
Definition: teleop_pr2.cpp:72


pr2_teleop
Author(s):
autogenerated on Thu Jun 6 2019 19:18:47