pr2_teleop_general_commander.cpp
Go to the documentation of this file.
1 /*
2  * pr2_teleop_booth_commander
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 
31 // Author: E. Gil Jones
32 
33 #include <string>
34 #include <boost/bind.hpp>
37 
38 #include <pr2_mechanism_msgs/SwitchController.h>
39 #include <geometry_msgs/Twist.h>
40 #include <trajectory_msgs/JointTrajectory.h>
41 #include <moveit_msgs/GetPositionFK.h>
42 #include <moveit_msgs/GetPositionIK.h>
43 #include <polled_camera/GetPolledImage.h>
44 
46 
47 #include "urdf_model/pose.h"
48 
49 static const std::string LEFT_HAND_LINK_TO_TRACK = "l_gripper_palm_link";
50 static const std::string RIGHT_HAND_LINK_TO_TRACK = "r_gripper_palm_link";
51 
52 static const double MAX_HEAD_TRACK_SPEED = 2.0;
53 
54 static const double GRIPPER_CLOSE_POSITION = 0.000;
55 static const double GRIPPER_CLOSE_MAX_EFFORT = 10000.0;
56 
57 static const double GRIPPER_OPEN_POSITION = 0.086;
58 static const double GRIPPER_OPEN_MAX_EFFORT = 10000.0;
59 
60 static const std::string RIGHT_ARM_MANNEQUIN_CONTROLLER = "r_arm_controller_loose";
61 static const std::string LEFT_ARM_MANNEQUIN_CONTROLLER = "l_arm_controller_loose";
62 
63 static const std::string HEAD_MANNEQUIN_CONTROLLER = "head_traj_controller_loose";
64 static const std::string HEAD_POSITION_CONTROLLER = "head_traj_controller";
65 
66 static const unsigned int WALK_BUFFER = 10;
67 
69  bool control_head,
70  bool control_rarm,
71  bool control_larm,
72  bool control_prosilica,
73  std::string arm_controller_name)
74  : n_(),
75  control_body_(control_body),
76  control_head_(control_head),
77  control_rarm_(control_rarm),
78  control_larm_(control_larm),
79  control_prosilica_(control_prosilica)
80 {
81 
82  r_arm_controller_name_ = "r_"+arm_controller_name;
83  l_arm_controller_name_ = "l_"+arm_controller_name;
84 
86 
87  std::string urdf_xml,full_urdf_xml;
88  n_.param("urdf_xml", urdf_xml, std::string("robot_description"));
89  if(!n_.getParam(urdf_xml,full_urdf_xml))
90  {
91  ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
93  }
94  else
95  {
96  robot_model_.initString(full_urdf_xml);
98  }
99 
100  //universal
101  switch_controllers_service_ = n_.serviceClient<pr2_mechanism_msgs::SwitchController>("pr2_controller_manager/switch_controller");
103  power_board_sub_ = n_.subscribe<pr2_msgs::PowerBoardState>("power_board/state", 1, &GeneralCommander::powerBoardCallback, this);
104 
105  if(control_head_) {
106  tilt_laser_service_ = n_.serviceClient<pr2_msgs::SetPeriodicCmd>("laser_tilt_controller/set_periodic_cmd");
107  head_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>("head_traj_controller/command", 1);
108  head_track_hand_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction>("/head_traj_controller/point_head_action", true);
109  while(!head_track_hand_client_->waitForServer(ros::Duration(5.0))){
110  ROS_INFO("Waiting for the point_head_action server to come up");
111  }
112  //just in case there's an existing goal
113  head_track_hand_client_->cancelAllGoals();
114  } else {
116  }
117  if(control_body_) {
118  torso_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>("torso_controller/command", 1);
119  base_pub_ = n_.advertise<geometry_msgs::Twist>("base_controller/command", 1);
120  }
121  if(control_rarm_) {
124  right_arm_traj_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(r_arm_controller_name_+"/command", 1);
126  ROS_INFO("Waiting for the right gripper action server to come up");
127  }
128  while(!right_arm_trajectory_client_->waitForServer(ros::Duration(5.0))){
129  ROS_INFO_STREAM("Waiting for the right arm trajectory action server to come up" << r_arm_controller_name_+"/joint_trajectory_action");
130  }
131  } else {
132  right_gripper_client_ = NULL;
134  }
135  if(control_larm_) {
138  left_arm_traj_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(l_arm_controller_name_+"/command", 1);
140  ROS_INFO("Waiting for the right gripper action server to come up");
141  }
142  while(!left_arm_trajectory_client_->waitForServer(ros::Duration(5.0))){
143  ROS_INFO_STREAM("Waiting for the left arm trajectory action server to come up");
144  }
145  } else {
146  left_gripper_client_ = NULL;
148  }
151  } else {
152  tuck_arms_client_ = NULL;
153  }
154 
155  if(control_rarm_) {
156  ROS_INFO("Waiting for right arm kinematics servers");
157  ros::service::waitForService("pr2_right_arm_kinematics/get_fk");
158  ros::service::waitForService("pr2_right_arm_kinematics/get_ik");
159  right_arm_kinematics_forward_client_ = n_.serviceClient<moveit_msgs::GetPositionFK>("pr2_right_arm_kinematics/get_fk", true);
160  right_arm_kinematics_inverse_client_ = n_.serviceClient<moveit_msgs::GetPositionIK>("pr2_right_arm_kinematics/get_ik", true);
161  }
162 
163  if(control_larm_) {
164  ROS_INFO("Waiting for left arm kinematics servers");
165  ros::service::waitForService("pr2_left_arm_kinematics/get_fk");
166  ros::service::waitForService("pr2_left_arm_kinematics/get_ik");
167  left_arm_kinematics_forward_client_ = n_.serviceClient<moveit_msgs::GetPositionFK>("pr2_left_arm_kinematics/get_fk", true);
168  left_arm_kinematics_inverse_client_ = n_.serviceClient<moveit_msgs::GetPositionIK>("pr2_left_arm_kinematics/get_ik", true);
169  }
170 
171 
172  if(control_prosilica_) {
173  ros::service::waitForService("prosilica/request_image");
174  prosilica_polling_client_ = n_.serviceClient<polled_camera::GetPolledImage>("prosilica/request_image", true);
175  }
176 
177  right_walk_along_pose_.push_back(.049);
178  right_walk_along_pose_.push_back(-.116);
179  right_walk_along_pose_.push_back(.036);
180  right_walk_along_pose_.push_back(-1.272);
181  right_walk_along_pose_.push_back(-.084);
182  right_walk_along_pose_.push_back(-.148);
183  right_walk_along_pose_.push_back(-0.027);
184 
185  left_walk_along_pose_.push_back(0.0);
186  left_walk_along_pose_.push_back(-.149);
187  left_walk_along_pose_.push_back(.085);
188  left_walk_along_pose_.push_back(-1.234);
189  left_walk_along_pose_.push_back(0.030);
190  left_walk_along_pose_.push_back(-.141);
191  left_walk_along_pose_.push_back(3.114);
192 
193  head_nod_traj_.joint_names.push_back("head_pan_joint");
194  head_nod_traj_.joint_names.push_back("head_tilt_joint");
195  head_nod_traj_.points.resize(3);
196  head_nod_traj_.points[0].positions.push_back(0.0);
197  head_nod_traj_.points[0].positions.push_back(-0.2);
198  head_nod_traj_.points[0].velocities.push_back(0.0);
199  head_nod_traj_.points[0].velocities.push_back(0.0);
200  head_nod_traj_.points[0].time_from_start = ros::Duration(0.5);
201 
202  head_nod_traj_.points[1].positions.push_back(0.0);
203  head_nod_traj_.points[1].positions.push_back(0.2);
204  head_nod_traj_.points[1].velocities.push_back(0.0);
205  head_nod_traj_.points[1].velocities.push_back(0.0);
206  head_nod_traj_.points[1].time_from_start = ros::Duration(1.0);
207 
208  head_nod_traj_.points[2].positions.push_back(0.0);
209  head_nod_traj_.points[2].positions.push_back(0.0);
210  head_nod_traj_.points[2].velocities.push_back(0.0);
211  head_nod_traj_.points[2].velocities.push_back(0.0);
212  head_nod_traj_.points[2].time_from_start = ros::Duration(1.5);
213 
214  head_shake_traj_.joint_names.push_back("head_pan_joint");
215  head_shake_traj_.joint_names.push_back("head_tilt_joint");
216  head_shake_traj_.points.resize(3);
217  head_shake_traj_.points[0].positions.push_back(.62);
218  head_shake_traj_.points[0].positions.push_back(0.0);
219  head_shake_traj_.points[0].velocities.push_back(0.0);
220  head_shake_traj_.points[0].velocities.push_back(0.0);
221  head_shake_traj_.points[0].time_from_start = ros::Duration(0.5);
222 
223  head_shake_traj_.points[1].positions.push_back(-.62);
224  head_shake_traj_.points[1].positions.push_back(0.0);
225  head_shake_traj_.points[1].velocities.push_back(0.0);
226  head_shake_traj_.points[1].velocities.push_back(0.0);
227  head_shake_traj_.points[1].time_from_start = ros::Duration(1.0);
228 
229  head_shake_traj_.points[2].positions.push_back(0.0);
230  head_shake_traj_.points[2].positions.push_back(0.0);
231  head_shake_traj_.points[2].velocities.push_back(0.0);
232  head_shake_traj_.points[2].velocities.push_back(0.0);
233  head_shake_traj_.points[2].time_from_start = ros::Duration(1.5);
234 
235  //making sure that everything is in the right mode
240  if(control_head_) {
243  }
244 
245  //cribbed from motion planning laser settings
246  laser_slow_period_ = 10;
247  laser_slow_amplitude_ = .75;
248  laser_slow_offset_ = .25;
249 
250  //cribbed from head cart tilt_laser_launch
251  laser_fast_period_ = 2;
252  laser_fast_amplitude_ = .78;
253  laser_fast_offset_ = .3;
254 
257 
258  last_torso_vel_ = 0.0;
259  walk_along_ok_ = false;
260 }
261 
266  }
268  delete right_gripper_client_;
269  }
271  delete left_gripper_client_;
272  }
275  }
278  }
279  if(tuck_arms_client_) {
280  delete tuck_arms_client_;
281  }
282 }
283 
284 void GeneralCommander::jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState)
285 {
286  for(unsigned int i = 0; i < jointState->name.size(); i++) {
287  joint_state_position_map_[jointState->name[i]] = jointState->position[i];
288  joint_state_velocity_map_[jointState->name[i]] = jointState->velocity[i];
289  //if(jointState->name[i] == "r_wrist_roll_joint") {
290  // ROS_INFO_STREAM("Right wrist roll pos " << jointState->position[i] << " vel " << jointState->velocity[i]);
291  //}
292  }
293 }
294 
295 bool GeneralCommander::getJointPosition(const std::string& name, double& pos) const {
296  if(joint_state_position_map_.find(name) == joint_state_position_map_.end()) {
297  return false;
298  }
299  pos = joint_state_position_map_.find(name)->second;
300  return true;
301 }
302 
303 bool GeneralCommander::getJointVelocity(const std::string& name, double& vel) const {
304  if(joint_state_velocity_map_.find(name) == joint_state_velocity_map_.end()) {
305  return false;
306  }
307  vel = joint_state_velocity_map_.find(name)->second;
308  return true;
309 }
310 
312  return laser_control_mode_;
313 }
314 
316  return head_control_mode_;
317 }
318 
320  if(arm == ARMS_RIGHT || arm == ARMS_BOTH) {
322  } else {
323  return left_arm_control_mode_;
324  }
325 }
326 
328 
329  if(!control_head_) return;
330 
331  if(start) {
332  int ok = system("rosrun dynamic_reconfigure dynparam set camera_synchronizer_node narrow_stereo_trig_mode 3");
333  ROS_DEBUG("Trying to send projector on");
334  if(ok < 0) {
335  ROS_WARN("Dynamic reconfigure for setting trigger mode ON failed");
336  }
337  } else {
338  int ok = system("rosrun dynamic_reconfigure dynparam set camera_synchronizer_node narrow_stereo_trig_mode 4");
339  ROS_DEBUG("Trying to send trigger off");
340  if(ok < 0) {
341  ROS_WARN("Dynamic reconfigure for setting trigger mode OFF failed");
342  }
343  }
344 }
345 
347  if(!control_head_) return;
348 
349  if(laser_control_mode_ == mode) return;
350 
351  pr2_msgs::SetPeriodicCmd::Request req;
352  pr2_msgs::SetPeriodicCmd::Response res;
353 
354  req.command.profile = "linear";
355  if(mode == LASER_TILT_SLOW) {
356  ROS_DEBUG("Sending slow");
357  req.command.period = laser_slow_period_;
358  req.command.amplitude = laser_slow_period_;
359  req.command.offset = laser_slow_offset_;
360  } else if(mode == LASER_TILT_FAST) {
361  ROS_DEBUG("Sending fast");
362  req.command.period = laser_fast_period_;
363  req.command.amplitude = laser_fast_period_;
364  req.command.offset = laser_fast_offset_;
365  } else {
366  ROS_DEBUG("Sending off");
367  req.command.period = 1.0;
368  req.command.amplitude = 0;
369  req.command.offset = laser_slow_offset_;
370  }
371 
372  if(tilt_laser_service_.call(req,res)) {
373  //ROS_DEBUG("Resp start time is %g", res.start_time.toSeconds());
374  } else {
375  ROS_ERROR("Tilt laser service call failed.\n");
376  }
377  laser_control_mode_ = mode;
378 }
379 
381  if(!control_head_) return;
382  if(mode == head_control_mode_) return;
383  if(mode == HEAD_TRACK_LEFT_HAND) {
384  ROS_DEBUG("Setting head to track left hand");
385  } else if(mode == HEAD_TRACK_RIGHT_HAND) {
386  ROS_DEBUG("Setting head to track right hand");
387  }
388  std::vector<std::string> start_controllers;
389  std::vector<std::string> stop_controllers;
390  if(mode == HEAD_MANNEQUIN) {
391  start_controllers.push_back(HEAD_MANNEQUIN_CONTROLLER);
392  stop_controllers.push_back(HEAD_POSITION_CONTROLLER);
393  } else if(head_control_mode_ == HEAD_MANNEQUIN) {
394  start_controllers.push_back(HEAD_POSITION_CONTROLLER);
395  stop_controllers.push_back(HEAD_MANNEQUIN_CONTROLLER);
396  }
397  if(!start_controllers.empty() || !stop_controllers.empty()) {
398  switchControllers(start_controllers, stop_controllers);
399  }
400  head_control_mode_ = mode;
401 }
402 
404  if(!control_rarm_ && !control_larm_) return;
405  if(!control_rarm_ && arm == ARMS_RIGHT) return;
406  if(!control_larm_ && arm == ARMS_LEFT) return;
407 
408  if(arm == ARMS_LEFT) {
409  if(mode == left_arm_control_mode_) return;
410  } else if(arm == ARMS_RIGHT) {
411  if(mode == right_arm_control_mode_) return;
412  } else {
413  if(mode == left_arm_control_mode_ && mode == right_arm_control_mode_) return;
414  }
415 
416  std::string left_running_controller;
417  std::string right_running_controller;
418 
420  left_running_controller = LEFT_ARM_MANNEQUIN_CONTROLLER;
422  left_running_controller = l_arm_controller_name_;
423  }
424 
426  right_running_controller = RIGHT_ARM_MANNEQUIN_CONTROLLER;
428  right_running_controller = r_arm_controller_name_;
429  }
430 
431  std::vector<std::string> start_controllers;
432  std::vector<std::string> stop_controllers;
433 
434  if(mode == ARM_NO_CONTROLLER) {
435  if(arm == ARMS_LEFT || arm == ARMS_BOTH) {
436  stop_controllers.push_back(left_running_controller);
437  }
438  if(arm == ARMS_RIGHT || arm == ARMS_BOTH) {
439  stop_controllers.push_back(right_running_controller);
440  }
441  } else if(mode == ARM_MANNEQUIN_MODE) {
442  if(arm == ARMS_LEFT || arm == ARMS_BOTH) {
443  if(!left_running_controller.empty()) {
444  stop_controllers.push_back(left_running_controller);
445  }
446  start_controllers.push_back(LEFT_ARM_MANNEQUIN_CONTROLLER);
447  }
448  if(arm == ARMS_RIGHT || arm == ARMS_BOTH) {
449  if(!right_running_controller.empty()) {
450  stop_controllers.push_back(right_running_controller);
451  }
452  start_controllers.push_back(RIGHT_ARM_MANNEQUIN_CONTROLLER);
453  }
454  } else if(mode == ARM_POSITION_CONTROL) {
455  if(arm == ARMS_LEFT || arm == ARMS_BOTH) {
456  if(!left_running_controller.empty()) {
457  stop_controllers.push_back(left_running_controller);
458  }
459  start_controllers.push_back(l_arm_controller_name_);
460  }
461  if(arm == ARMS_RIGHT || arm == ARMS_BOTH) {
462  if(!right_running_controller.empty()) {
463  stop_controllers.push_back(right_running_controller);
464  }
465  start_controllers.push_back(r_arm_controller_name_);
466  }
467  }
468  switchControllers(start_controllers, stop_controllers);
469  if(arm == ARMS_LEFT || arm == ARMS_BOTH) {
470  left_arm_control_mode_ = mode;
471  }
472  if(arm == ARMS_RIGHT || arm == ARMS_BOTH) {
474  }
475 }
476 
477 void GeneralCommander::sendHeadCommand(double req_pan, double req_tilt) {
478  if(!control_head_) return;
480  return;
481  }
482  //TODO - correct
483  trajectory_msgs::JointTrajectory traj;
484  traj.header.stamp = ros::Time::now() + ros::Duration(0.01);
485  traj.joint_names.push_back("head_pan_joint");
486  traj.joint_names.push_back("head_tilt_joint");
487  traj.points.resize(1);
488  traj.points[0].positions.push_back(req_pan);
489  traj.points[0].velocities.push_back(0.0);//req_pan_vel);
490  traj.points[0].positions.push_back(req_tilt);
491  traj.points[0].velocities.push_back(0.0);//req_tilt_vel);
492  traj.points[0].time_from_start = ros::Duration(0.1);
493  //ROS_INFO_STREAM("Publishing " << req_pan << " " << req_tilt);
494  head_pub_.publish(traj);
495 }
496 
498  if(!control_head_) return;
501  return;
502  }
503 
504  //the goal message we will be sending
505  pr2_controllers_msgs::PointHeadGoal goal;
506 
507  //the target point, expressed in the requested frame
508  geometry_msgs::PointStamped point;
510  point.header.frame_id = LEFT_HAND_LINK_TO_TRACK;
511  } else {
512  point.header.frame_id = RIGHT_HAND_LINK_TO_TRACK;
513  }
514  point.point.x = 0.0;
515  point.point.y = 0.0;
516  point.point.z = 0.0;
517  goal.target = point;
518 
519  //we are pointing the high-def camera frame
520  //(pointing_axis defaults to X-axis)
521  goal.pointing_frame = "high_def_frame";
522 
523  //take at least 0.5 seconds to get there
524  goal.min_duration = ros::Duration(0.1);
525 
526  //and go no faster than 1 rad/s
527  goal.max_velocity = MAX_HEAD_TRACK_SPEED;
528 
529  //send the goal
531 
532  //TODO - preempty going to be problematic?
533 
534 }
535 
537  if(!control_rarm_ && !control_larm_) return;
538  if(!control_rarm_ && which == ARMS_RIGHT) return;
539  if(!control_larm_ && which == ARMS_LEFT) return;
540  if(which == ARMS_RIGHT || which == ARMS_BOTH) {
541  pr2_controllers_msgs::Pr2GripperCommandGoal com;
542  if(close) {
543  com.command.position = GRIPPER_CLOSE_POSITION;
544  com.command.max_effort = GRIPPER_CLOSE_MAX_EFFORT;
545  } else {
546  com.command.position = GRIPPER_OPEN_POSITION;
547  com.command.max_effort = GRIPPER_OPEN_MAX_EFFORT;
548  }
552  ROS_DEBUG("Right gripper command succeeded");
553  else
554  ROS_WARN("Right gripper command failed");
555  }
556  if(which == ARMS_LEFT || which == ARMS_BOTH) {
557  pr2_controllers_msgs::Pr2GripperCommandGoal com;
558  if(close) {
559  com.command.position = GRIPPER_CLOSE_POSITION;
560  com.command.max_effort = GRIPPER_CLOSE_MAX_EFFORT;
561  } else {
562  com.command.position = GRIPPER_OPEN_POSITION;
563  com.command.max_effort = GRIPPER_OPEN_MAX_EFFORT;
564  }
568  ROS_DEBUG("Left gripper command succeeded");
569  else
570  ROS_WARN("Left gripper command failed");
571  }
572 
573 }
574 
575 void GeneralCommander::sendTorsoCommand(double pos, double vel) {
576  if(!control_body_) return;
577  //only do this if we are commanding some velocity and not transitioning
578  if(fabs(vel) < .0001 && fabs(last_torso_vel_ < .0001)) {
579  //return;
580  }
581  last_torso_vel_ = vel;
582  //ROS_INFO_STREAM("Torso pos " << pos << " vel " << vel);
583  trajectory_msgs::JointTrajectory traj;
584  traj.header.stamp = ros::Time::now() + ros::Duration(0.01);
585  traj.joint_names.push_back("torso_lift_joint");
586  traj.points.resize(1);
587  traj.points[0].positions.push_back(pos);
588  traj.points[0].velocities.push_back(vel);
589  traj.points[0].time_from_start = ros::Duration(0.25);
590  torso_pub_.publish(traj);
591 }
592 
593 void GeneralCommander::sendBaseCommand(double vx, double vy, double vw) {
594  if(!control_body_) return;
595  geometry_msgs::Twist cmd;
596 
597  cmd.linear.x = vx;
598  cmd.linear.y = vy;
599  cmd.angular.z = vw;
600  base_pub_.publish(cmd);
601 }
602 
603 void GeneralCommander::switchControllers(const std::vector<std::string>& start_controllers, const std::vector<std::string>& stop_controllers) {
604  pr2_mechanism_msgs::SwitchController::Request req;
605  pr2_mechanism_msgs::SwitchController::Response res;
606  req.start_controllers = start_controllers;
607  req.stop_controllers = stop_controllers;
608  for(std::vector<std::string>::const_iterator it = start_controllers.begin();
609  it != start_controllers.end();
610  it++) {
611  ROS_DEBUG_STREAM("Trying to start controller " << (*it));
612  }
613  for(std::vector<std::string>::const_iterator it = stop_controllers.begin();
614  it != stop_controllers.end();
615  it++) {
616  ROS_DEBUG_STREAM("Trying to stop controller " << (*it));
617  }
618  req.strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT;
619  if(!switch_controllers_service_.call(req,res)) {
620  ROS_WARN("Call to switch controllers failed entirely");
621  }
622  if(res.ok != true) {
623  ROS_WARN("Call to switch controllers reports not ok");
624  }
625 }
626 
627 void GeneralCommander::sendWristVelCommands(double right_wrist_vel, double left_wrist_vel, double hz) {
628 
630 
631  if(control_rarm_) {
632  if(abs(right_wrist_vel) > .01) {
633  if((ros::Time::now()-last_right_wrist_goal_stamp_).toSec() > .5) {
635  }
637 
638  //our goal variable
639  pr2_controllers_msgs::JointTrajectoryGoal right_goal;
640  composeWristRotGoal("r", right_goal, right_des_joint_states_ ,right_wrist_vel, hz);
641  right_arm_traj_pub_.publish(right_goal.trajectory);
642  //right_arm_trajectory_client_->sendGoal(right_goal);
643  }
644  }
645  if(control_larm_) {
646  if(abs(left_wrist_vel) > .01) {
647  if((ros::Time::now()-last_left_wrist_goal_stamp_).toSec() > .5) {
649  }
651  //our goal variable
652  pr2_controllers_msgs::JointTrajectoryGoal left_goal;
653  composeWristRotGoal("l", left_goal, left_des_joint_states_, left_wrist_vel, hz);
654  left_arm_traj_pub_.publish(left_goal.trajectory);
655  //left_arm_trajectory_client_->sendGoal(left_goal);
656  }
657  }
658 }
659 
660 void GeneralCommander::composeWristRotGoal(const std::string pref, pr2_controllers_msgs::JointTrajectoryGoal& goal,
661  std::vector<double>& des_joints,
662  double des_vel, double hz) const {
663 
664  double trajectory_duration = .2;
665 
666  std::vector<std::string> joint_names;
667 
668  // First, the joint names, which apply to all waypoints
669  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
670  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
671  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
672  joint_names.push_back(pref+"_"+"elbow_flex_joint");
673  joint_names.push_back(pref+"_"+"forearm_roll_joint");
674  joint_names.push_back(pref+"_"+"wrist_flex_joint");
675  joint_names.push_back(pref+"_"+"wrist_roll_joint");
676 
677  double end_pos_diff = (trajectory_duration)*des_vel;
678  double cur_pos_diff = (1.0/hz)*des_vel;
679 
680  goal.trajectory.joint_names=joint_names;
681  goal.trajectory.points.resize(1);
682  goal.trajectory.points[0].positions = des_joints;
683 
684  goal.trajectory.points[0].velocities.resize(7,0.0);
685  //goal.trajectory.points[0].velocities[6] = des_vel;
686 
687  goal.trajectory.points[0].positions[6] += end_pos_diff;
688  des_joints[6] += cur_pos_diff;
689  //ROS_INFO_STREAM("Sending pos goal " << goal.trajectory.points[1].positions[6]);
690 
691  goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.030);
692  goal.trajectory.points[0].time_from_start = ros::Duration(trajectory_duration);
693 }
694 
696 
697  if(control_rarm_) {
698  moveit_msgs::GetPositionFK::Request right_fk_request;
699  moveit_msgs::GetPositionFK::Response right_fk_response;
700 
701  right_fk_request.header.frame_id = "base_link";
702  right_fk_request.fk_link_names.push_back("r_wrist_roll_link");
703  std::vector<std::string> right_joint_names;
704  right_joint_names.push_back("r_shoulder_pan_joint");
705  right_joint_names.push_back("r_shoulder_lift_joint");
706  right_joint_names.push_back("r_upper_arm_roll_joint");
707  right_joint_names.push_back("r_elbow_flex_joint");
708  right_joint_names.push_back("r_forearm_roll_joint");
709  right_joint_names.push_back("r_wrist_flex_joint");
710  right_joint_names.push_back("r_wrist_roll_joint");
711 
712  right_fk_request.robot_state.joint_state.position.resize(right_joint_names.size());
713  right_fk_request.robot_state.joint_state.name = right_joint_names;
714  for(unsigned int i = 0; i < right_fk_request.robot_state.joint_state.name.size(); i++) {
715  bool ok = getJointPosition(right_fk_request.robot_state.joint_state.name[i], right_fk_request.robot_state.joint_state.position[i]);
716  if(!ok) {
717  ROS_WARN_STREAM("No joint state yet for " << right_fk_request.robot_state.joint_state.name[i]);
718  return;
719  }
720  }
721  if(right_arm_kinematics_forward_client_.call(right_fk_request, right_fk_response)) {
722  if(right_fk_response.error_code.val == right_fk_response.error_code.SUCCESS) {
723  right_wrist_roll_pose_ = right_fk_response.pose_stamped[0].pose;
724  } else {
725  ROS_DEBUG("Right fk not a success");
726  }
727  } else {
728  ROS_WARN("Right fk call failed all together");
729  }
730  }
731 
732  if(control_larm_) {
733  moveit_msgs::GetPositionFK::Request left_fk_request;
734  moveit_msgs::GetPositionFK::Response left_fk_response;
735 
736  left_fk_request.header.frame_id = "base_link";
737  left_fk_request.fk_link_names.push_back("l_wrist_roll_link");
738  std::vector<std::string> left_joint_names;
739  left_joint_names.push_back("l_shoulder_pan_joint");
740  left_joint_names.push_back("l_shoulder_lift_joint");
741  left_joint_names.push_back("l_upper_arm_roll_joint");
742  left_joint_names.push_back("l_elbow_flex_joint");
743  left_joint_names.push_back("l_forearm_roll_joint");
744  left_joint_names.push_back("l_wrist_flex_joint");
745  left_joint_names.push_back("l_wrist_roll_joint");
746 
747  left_fk_request.robot_state.joint_state.position.resize(left_joint_names.size());
748  left_fk_request.robot_state.joint_state.name = left_joint_names;
749  for(unsigned int i = 0; i < left_fk_request.robot_state.joint_state.name.size(); i++) {
750  bool ok = getJointPosition(left_fk_request.robot_state.joint_state.name[i], left_fk_request.robot_state.joint_state.position[i]);
751  if(!ok) {
752  ROS_WARN_STREAM("No joint state yet for " << left_fk_request.robot_state.joint_state.name[i]);
753  return;
754  }
755  }
756  if(left_arm_kinematics_forward_client_.call(left_fk_request, left_fk_response)) {
757  if(left_fk_response.error_code.val == left_fk_response.error_code.SUCCESS) {
758  left_wrist_roll_pose_ = left_fk_response.pose_stamped[0].pose;
759  } else {
760  ROS_DEBUG("Left fk not a success");
761  }
762  } else {
763  ROS_WARN("Left fk call failed all together");
764  }
765 
766  //ROS_INFO_STREAM("Current x " << left_wrist_roll_pose_.position.x << " y " << left_wrist_roll_pose_.position.y << " z " << left_wrist_roll_pose_.position.z);
767  }
768 }
769 
771 
773 
774  double tot_distance;
775  if(control_rarm_) {
776  tot_distance = sqrt(pow(des_r_wrist_roll_pose_.position.x-right_wrist_roll_pose_.position.x,2.0)+
777  pow(des_r_wrist_roll_pose_.position.y-right_wrist_roll_pose_.position.y,2.0)+
778  pow(des_r_wrist_roll_pose_.position.z-right_wrist_roll_pose_.position.z,2.0)+
779  pow(des_r_wrist_roll_pose_.orientation.x-right_wrist_roll_pose_.orientation.x,2.0)+
780  pow(des_r_wrist_roll_pose_.orientation.y-right_wrist_roll_pose_.orientation.y,2.0)+
781  pow(des_r_wrist_roll_pose_.orientation.z-right_wrist_roll_pose_.orientation.z,2.0)+
782  pow(des_r_wrist_roll_pose_.orientation.w-right_wrist_roll_pose_.orientation.w,2.0));
783 
784  //ROS_INFO_STREAM("Cur " << right_wrist_roll_pose_.position.x << " " << right_wrist_roll_pose_.position.y << " " << right_wrist_roll_pose_.position.z
785  // << " des " << des_r_wrist_roll_pose_.position.x << " " << des_r_wrist_roll_pose_.position.y << " " << des_r_wrist_roll_pose_.position.z);
786 
787  //ROS_INFO_STREAM("Total distance " << tot_distance);
788 
789  if(tot_distance > max_dist) {
791 
792  std::vector<std::string> joint_names;
793  std::string pref = "r";
794  // First, the joint names, which apply to all waypoints
795  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
796  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
797  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
798  joint_names.push_back(pref+"_"+"elbow_flex_joint");
799  joint_names.push_back(pref+"_"+"forearm_roll_joint");
800  joint_names.push_back(pref+"_"+"wrist_flex_joint");
801  joint_names.push_back(pref+"_"+"wrist_roll_joint");
802  right_des_joint_states_.clear();
803  for(unsigned int i = 0; i < joint_names.size(); i++) {
804  double cur_pos;
805  getJointPosition(joint_names[i], cur_pos);
806  right_des_joint_states_.push_back(cur_pos);
807  }
808  //ROS_INFO("Clamping right");
809  }
810  }
811 
812  if(control_larm_) {
813  tot_distance = sqrt(pow(des_l_wrist_roll_pose_.position.x-left_wrist_roll_pose_.position.x,2.0)+
814  pow(des_l_wrist_roll_pose_.position.y-left_wrist_roll_pose_.position.y,2.0)+
815  pow(des_l_wrist_roll_pose_.position.z-left_wrist_roll_pose_.position.z,2.0)+
816  pow(des_l_wrist_roll_pose_.orientation.x-left_wrist_roll_pose_.orientation.x,2.0)+
817  pow(des_l_wrist_roll_pose_.orientation.y-left_wrist_roll_pose_.orientation.y,2.0)+
818  pow(des_l_wrist_roll_pose_.orientation.z-left_wrist_roll_pose_.orientation.z,2.0)+
819  pow(des_l_wrist_roll_pose_.orientation.w-left_wrist_roll_pose_.orientation.w,2.0));
820 
821 
822  if(tot_distance > max_dist) {
824  std::vector<std::string> joint_names;
825  std::string pref = "l";
826  // First, the joint names, which apply to all waypoints
827  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
828  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
829  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
830  joint_names.push_back(pref+"_"+"elbow_flex_joint");
831  joint_names.push_back(pref+"_"+"forearm_roll_joint");
832  joint_names.push_back(pref+"_"+"wrist_flex_joint");
833  joint_names.push_back(pref+"_"+"wrist_roll_joint");
834  left_des_joint_states_.clear();
835  for(unsigned int i = 0; i < joint_names.size(); i++) {
836  double cur_pos;
837  getJointPosition(joint_names[i], cur_pos);
838  left_des_joint_states_.push_back(cur_pos);
839  }
840  //ROS_INFO("Clamping left");
841  }
842  }
843 }
844 
845 void GeneralCommander::unnormalizeTrajectory(trajectory_msgs::JointTrajectory& traj) const {
846 
847  std::vector<double> current_values;
848  std::vector<bool> wraparound;
849  trajectory_msgs::JointTrajectory input_trajectory = traj;
850 
851  for (size_t i=0; i<input_trajectory.joint_names.size(); i++)
852  {
853  std::string name = input_trajectory.joint_names[i];
854 
855  double pos;
856  if(!getJointPosition(name, pos)) {
857  ROS_WARN_STREAM("Can't unnormalize as no current joint state for " << name);
858  return;
859  }
860 
861  //first waypoint is unnormalized relative to current joint states
862  current_values.push_back(pos);
863 
864  boost::shared_ptr<const urdf::Joint> joint = robot_model_.getJoint(name);
865  if (joint.get() == NULL)
866  {
867  ROS_ERROR("Joint name %s not found in urdf model", name.c_str());
868  return;
869  }
870  if (joint->type == urdf::Joint::CONTINUOUS) {
871  wraparound.push_back(true);
872  } else {
873  wraparound.push_back(false);
874  }
875  }
876 
877  trajectory_msgs::JointTrajectory unnormalized_trajectory = input_trajectory;
878  for (size_t i=0; i<unnormalized_trajectory.points.size(); i++)
879  {
880  for (size_t j=0; j<unnormalized_trajectory.points[i].positions.size(); j++)
881  {
882  if(!wraparound[j]){
883  continue;
884  }
885  double current = current_values[j];
886  double traj = unnormalized_trajectory.points[i].positions[j];
887  while ( current - traj > M_PI ) traj += 2*M_PI;
888  while ( traj - current > M_PI ) traj -= 2*M_PI;
889  ROS_DEBUG("Normalizing joint %s from %f to %f", unnormalized_trajectory.joint_names.at(j).c_str(),
890  unnormalized_trajectory.points[i].positions[j], traj);
891  unnormalized_trajectory.points[i].positions[j] = traj;
892  //all other waypoints are unnormalized relative to the previous waypoint
893  current_values[j] = traj;
894  }
895  }
896  traj = unnormalized_trajectory;
897 }
898 
899 void GeneralCommander::sendArmVelCommands(double r_x_vel, double r_y_vel, double r_z_vel, double r_roll_vel, double r_pitch_vel, double r_yaw_vel,
900  double l_x_vel, double l_y_vel, double l_z_vel, double l_roll_vel, double l_pitch_vel, double l_yaw_vel,
901  double hz) {
902 
903 
904  ros::Time beforeCall = ros::Time::now();
905 
906  double trajectory_duration = .2;
907 
908  //ROS_INFO_STREAM("Got vels " << r_x_vel << " " << r_y_vel << " " << r_z_vel);
910 
911  if(control_rarm_) {
912 
913  if(fabs(r_x_vel) > .001 || fabs(r_y_vel) > .001 || fabs(r_z_vel) > .001 ||
914  fabs(r_roll_vel) > .001 || fabs(r_pitch_vel) > .001 || fabs(r_yaw_vel) > .001) {
915 
916  geometry_msgs::Pose right_trajectory_endpoint = des_r_wrist_roll_pose_;
917 
918  double pos_diff_x = r_x_vel*(1.0/hz);//*look_ahead;
919  double pos_diff_y = r_y_vel*(1.0/hz);//*look_ahead;
920  double pos_diff_z = r_z_vel*(1.0/hz);//*look_ahead;
921 
922  double pos_diff_roll = r_roll_vel*(1.0/hz);//*look_ahead;
923  double pos_diff_pitch = r_pitch_vel*(1.0/hz);//*look_ahead;
924  double pos_diff_yaw = r_yaw_vel;//*(1.0/hz);//*look_ahead;
925 
926  tf::Quaternion endpoint_quat, des_quat;
927  tf::Matrix3x3 end_rot, des_rot, diff_rot, duration_rot;
928  end_rot.setRotation(tf::Quaternion(
929  right_trajectory_endpoint.orientation.x,
930  right_trajectory_endpoint.orientation.y,
931  right_trajectory_endpoint.orientation.z,
932  right_trajectory_endpoint.orientation.w));
933  des_rot = end_rot;
934  duration_rot.setEulerYPR(r_yaw_vel*trajectory_duration,
935  r_pitch_vel*trajectory_duration,
936  r_roll_vel*trajectory_duration);
937  diff_rot.setEulerYPR(pos_diff_yaw, pos_diff_pitch, pos_diff_roll);
938 
939  (duration_rot *= end_rot).getRotation(endpoint_quat);
940  (diff_rot *= des_rot).getRotation(des_quat);
941 
942  right_trajectory_endpoint.position.x += r_x_vel*trajectory_duration;
943  right_trajectory_endpoint.position.y += r_y_vel*trajectory_duration;
944  right_trajectory_endpoint.position.z += r_z_vel*trajectory_duration;
945 
946  right_trajectory_endpoint.orientation.x = endpoint_quat.getAxis().getX();
947  right_trajectory_endpoint.orientation.y = endpoint_quat.getAxis().getY();
948  right_trajectory_endpoint.orientation.z = endpoint_quat.getAxis().getZ();
949  right_trajectory_endpoint.orientation.w = endpoint_quat.getW();
950 
951  des_r_wrist_roll_pose_.position.x += pos_diff_x;
952  des_r_wrist_roll_pose_.position.y += pos_diff_y;
953  des_r_wrist_roll_pose_.position.z += pos_diff_z;
954 
955  des_r_wrist_roll_pose_.orientation.x = des_quat.getAxis().getX();
956  des_r_wrist_roll_pose_.orientation.y = des_quat.getAxis().getY();
957  des_r_wrist_roll_pose_.orientation.z = des_quat.getAxis().getZ();
958  des_r_wrist_roll_pose_.orientation.w = des_quat.getW();
959 
960  //ROS_INFO_STREAM("Desired " << des_r_wrist_roll_pose_.position.x << " " << des_r_wrist_roll_pose_.position.y << " " << des_r_wrist_roll_pose_.position.z);
961 
962  //now call ik
963 
964  moveit_msgs::GetPositionIK::Request ik_req;
965  moveit_msgs::GetPositionIK::Response ik_res;
966 
967  ik_req.ik_request.timeout = ros::Duration(0.05);
968  ik_req.ik_request.ik_link_name = "r_wrist_roll_link";
969  ik_req.ik_request.pose_stamped.header.frame_id = "base_link";
970  ik_req.ik_request.pose_stamped.header.stamp = ros::Time::now();
971  ik_req.ik_request.pose_stamped.pose = right_trajectory_endpoint;
972 
973  std::vector<std::string> right_joint_names;
974  right_joint_names.push_back("r_shoulder_pan_joint");
975  right_joint_names.push_back("r_shoulder_lift_joint");
976  right_joint_names.push_back("r_upper_arm_roll_joint");
977  right_joint_names.push_back("r_elbow_flex_joint");
978  right_joint_names.push_back("r_forearm_roll_joint");
979  right_joint_names.push_back("r_wrist_flex_joint");
980  right_joint_names.push_back("r_wrist_roll_joint");
981  ik_req.ik_request.robot_state.joint_state.position.resize(right_joint_names.size());
982  ik_req.ik_request.robot_state.joint_state.name = right_joint_names;
983  for(unsigned int i = 0; i < ik_req.ik_request.robot_state.joint_state.name.size(); i++) {
984  bool ok = getJointPosition(ik_req.ik_request.robot_state.joint_state.name[i], ik_req.ik_request.robot_state.joint_state.position[i]);
985  if(!ok) {
986  ROS_WARN_STREAM("No joint state yet for " << ik_req.ik_request.robot_state.joint_state.name[i]);
987  return;
988  } else {
989  //ROS_INFO_STREAM("Setting position " << ik_req.ik_request.ik_seed_state.joint_state.position[i] << " For name "
990  // << ik_req.ik_request.ik_seed_state.joint_state.name[i]);
991  }
992  }
993 
994 
995  //otherwise not a lot to be done
996  if(right_arm_kinematics_inverse_client_.call(ik_req, ik_res) && ik_res.error_code.val == ik_res.error_code.SUCCESS) {
997  //ROS_INFO("Ik succeeded");
998  pr2_controllers_msgs::JointTrajectoryGoal goal;
999 
1000  std::vector<std::string> joint_names;
1001  std::string pref = "r";
1002 
1003  // First, the joint names, which apply to all waypoints
1004  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
1005  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
1006  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
1007  joint_names.push_back(pref+"_"+"elbow_flex_joint");
1008  joint_names.push_back(pref+"_"+"forearm_roll_joint");
1009  joint_names.push_back(pref+"_"+"wrist_flex_joint");
1010  joint_names.push_back(pref+"_"+"wrist_roll_joint");
1011 
1012  goal.trajectory.joint_names = joint_names;
1013 
1014  // We will have two waypoints in this goal trajectory
1015  goal.trajectory.points.resize(1);
1016 
1017  for(unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
1018  goal.trajectory.points[0].positions.push_back(ik_res.solution.joint_state.position[i]);
1019  goal.trajectory.points[0].velocities.push_back(0.0);
1020  }
1021  goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.200);
1022  goal.trajectory.points[0].time_from_start = ros::Duration(trajectory_duration);
1023 
1024  unnormalizeTrajectory(goal.trajectory);
1025  right_arm_traj_pub_.publish(goal.trajectory);
1026  } else {
1027  ROS_DEBUG_STREAM("Ik failed with response " << ik_res.error_code.val);
1028  }
1029  }
1030 
1031  ros::Time afterCall = ros::Time::now();
1032  }
1033  if(control_larm_) {
1034  if(fabs(l_x_vel) > .001 || fabs(l_y_vel) > .001 || fabs(l_z_vel) > .001 ||
1035  fabs(l_roll_vel) > .001 || fabs(l_pitch_vel) > .001 || fabs(l_yaw_vel) > .001) {
1036 
1037  geometry_msgs::Pose left_trajectory_endpoint = des_l_wrist_roll_pose_;
1038 
1039  double pos_diff_x = l_x_vel*(1.0/hz);
1040  double pos_diff_y = l_y_vel*(1.0/hz);
1041  double pos_diff_z = l_z_vel*(1.0/hz);
1042  double pos_diff_roll = l_roll_vel*(1.0/hz);//*look_ahead;
1043  double pos_diff_pitch = l_pitch_vel*(1.0/hz);//*look_ahead;
1044  double pos_diff_yaw = l_yaw_vel*(1.0/hz);
1045 
1046  tf::Quaternion endpoint_quat, des_quat;
1047  tf::Matrix3x3 end_rot, des_rot, diff_rot, duration_rot;
1048  end_rot.setRotation(tf::Quaternion(
1049  left_trajectory_endpoint.orientation.x,
1050  left_trajectory_endpoint.orientation.y,
1051  left_trajectory_endpoint.orientation.z,
1052  left_trajectory_endpoint.orientation.w));
1053  des_rot = end_rot;
1054  duration_rot.setEulerYPR(l_yaw_vel*trajectory_duration,
1055  l_pitch_vel*trajectory_duration,
1056  l_roll_vel*trajectory_duration);
1057  diff_rot.setEulerYPR(pos_diff_yaw, pos_diff_pitch, pos_diff_roll);
1058 
1059  (duration_rot *= end_rot).getRotation(endpoint_quat);
1060  (diff_rot *= des_rot).getRotation(des_quat);
1061 
1062  left_trajectory_endpoint.position.x += l_x_vel*trajectory_duration;
1063  left_trajectory_endpoint.position.y += l_y_vel*trajectory_duration;
1064  left_trajectory_endpoint.position.z += l_z_vel*trajectory_duration;
1065 
1066  left_trajectory_endpoint.orientation.x = endpoint_quat.getAxis().getX();
1067  left_trajectory_endpoint.orientation.y = endpoint_quat.getAxis().getY();
1068  left_trajectory_endpoint.orientation.z = endpoint_quat.getAxis().getZ();
1069  left_trajectory_endpoint.orientation.w = endpoint_quat.getW();
1070 
1071  des_l_wrist_roll_pose_.position.x += pos_diff_x;
1072  des_l_wrist_roll_pose_.position.y += pos_diff_y;
1073  des_l_wrist_roll_pose_.position.z += pos_diff_z;
1074 
1075  des_l_wrist_roll_pose_.orientation.x = des_quat.getAxis().getX();
1076  des_l_wrist_roll_pose_.orientation.y = des_quat.getAxis().getY();
1077  des_l_wrist_roll_pose_.orientation.z = des_quat.getAxis().getZ();
1078  des_l_wrist_roll_pose_.orientation.w = des_quat.getW();
1079 
1080  //now call ik
1081 
1082  moveit_msgs::GetPositionIK::Request ik_req;
1083  moveit_msgs::GetPositionIK::Response ik_res;
1084 
1085  ik_req.ik_request.timeout = ros::Duration(0.05);
1086  ik_req.ik_request.ik_link_name = "l_wrist_roll_link";
1087  ik_req.ik_request.pose_stamped.header.frame_id = "base_link";
1088  ik_req.ik_request.pose_stamped.header.stamp = ros::Time::now();
1089  ik_req.ik_request.pose_stamped.pose = left_trajectory_endpoint;
1090 
1091  std::vector<std::string> left_joint_names;
1092  left_joint_names.push_back("l_shoulder_pan_joint");
1093  left_joint_names.push_back("l_shoulder_lift_joint");
1094  left_joint_names.push_back("l_upper_arm_roll_joint");
1095  left_joint_names.push_back("l_elbow_flex_joint");
1096  left_joint_names.push_back("l_forearm_roll_joint");
1097  left_joint_names.push_back("l_wrist_flex_joint");
1098  left_joint_names.push_back("l_wrist_roll_joint");
1099 
1100  ik_req.ik_request.robot_state.joint_state.position.resize(left_joint_names.size());
1101  ik_req.ik_request.robot_state.joint_state.name = left_joint_names;
1102  for(unsigned int i = 0; i < ik_req.ik_request.robot_state.joint_state.name.size(); i++) {
1103  bool ok = getJointPosition(ik_req.ik_request.robot_state.joint_state.name[i], ik_req.ik_request.robot_state.joint_state.position[i]);
1104  if(!ok) {
1105  ROS_WARN_STREAM("No joint state yet for " << ik_req.ik_request.robot_state.joint_state.name[i]);
1106  return;
1107  } else {
1108  //ROS_INFO_STREAM("Setting position " << ik_req.ik_request.ik_seed_state.joint_state.position[i] << " For name "
1109  // << ik_req.ik_request.ik_seed_state.joint_state.name[i]);
1110  }
1111  }
1112 
1113  //otherwise not a lot to be done
1114  if(left_arm_kinematics_inverse_client_.call(ik_req, ik_res) && ik_res.error_code.val == ik_res.error_code.SUCCESS) {
1115  //ROS_INFO("Ik succeeded");
1116  pr2_controllers_msgs::JointTrajectoryGoal goal;
1117 
1118  std::vector<std::string> joint_names;
1119  std::string pref = "l";
1120 
1121  // First, the joint names, which apply to all waypoints
1122  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
1123  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
1124  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
1125  joint_names.push_back(pref+"_"+"elbow_flex_joint");
1126  joint_names.push_back(pref+"_"+"forearm_roll_joint");
1127  joint_names.push_back(pref+"_"+"wrist_flex_joint");
1128  joint_names.push_back(pref+"_"+"wrist_roll_joint");
1129 
1130  goal.trajectory.joint_names = joint_names;
1131 
1132  // We will have one waypoint in this goal trajectory
1133  goal.trajectory.points.resize(1);
1134 
1135  for(unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
1136  goal.trajectory.points[0].positions.push_back(ik_res.solution.joint_state.position[i]);
1137  goal.trajectory.points[0].velocities.push_back(0.0);
1138  }
1139  goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.200);
1140  goal.trajectory.points[0].time_from_start = ros::Duration(trajectory_duration);
1141 
1142  unnormalizeTrajectory(goal.trajectory);
1143  left_arm_traj_pub_.publish(goal.trajectory);
1144  } else {
1145  ROS_DEBUG_STREAM("Ik failed with response " << ik_res.error_code.val);
1146  }
1147  }
1148  }
1149 }
1150 
1152 
1153  //not implementing for one-armed robots
1154  if(!control_rarm_ || !control_larm_) {
1155  return false;
1156  }
1157 
1159 
1160  ROS_DEBUG("Attempting to set arms to walk along pose");
1161 
1162  std::vector<std::string> joint_names;
1163  std::string pref;
1164  pr2_controllers_msgs::JointTrajectoryGoal goal;
1165 
1166  pref = "r";
1167 
1168  // First, the joint names, which apply to all waypoints
1169  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
1170  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
1171  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
1172  joint_names.push_back(pref+"_"+"elbow_flex_joint");
1173  joint_names.push_back(pref+"_"+"forearm_roll_joint");
1174  joint_names.push_back(pref+"_"+"wrist_flex_joint");
1175  joint_names.push_back(pref+"_"+"wrist_roll_joint");
1176 
1177  goal.trajectory.joint_names = joint_names;
1178 
1179  // We will have one waypoint in this goal trajectory
1180  goal.trajectory.points.resize(1);
1181  goal.trajectory.points[0].positions = right_walk_along_pose_;
1182 
1183  for(unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
1184  goal.trajectory.points[0].velocities.push_back(0.0);
1185  }
1186 
1187  geometry_msgs::Pose right_walk_pose = getPositionFromJointsPose(right_arm_kinematics_forward_client_,
1188  "r_wrist_roll_link",
1189  joint_names,
1191  double tot_distance = sqrt(pow(right_walk_pose.position.x-right_wrist_roll_pose_.position.x,2.0)+
1192  pow(right_walk_pose.position.y-right_wrist_roll_pose_.position.y,2.0)+
1193  pow(right_walk_pose.position.z-right_wrist_roll_pose_.position.z,2.0));
1194  ROS_DEBUG_STREAM("Right dist is " << tot_distance);
1195 
1196  if(tot_distance > .02) {
1197 
1198  goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.030);
1199  goal.trajectory.points[0].time_from_start = ros::Duration(3.0);
1200 
1201  unnormalizeTrajectory(goal.trajectory);
1202 
1203  ROS_DEBUG("Sending right arm goal");
1204 
1206  bool finished_before_timeout = right_arm_trajectory_client_->waitForResult(ros::Duration(5.0));
1207 
1209 
1210  if(!finished_before_timeout || state != actionlib::SimpleClientGoalState::SUCCEEDED) {
1212  finished_before_timeout = right_arm_trajectory_client_->waitForResult(ros::Duration(5.0));
1214  if(!finished_before_timeout || state != actionlib::SimpleClientGoalState::SUCCEEDED) {
1215  ROS_WARN("Not in walk along pose");
1216  return false;
1217  }
1218  } else {
1219  ROS_DEBUG("Right arm goal successful");
1220  }
1221  } else {
1222  ROS_DEBUG("No need for right arm goal");
1223  }
1224  joint_names.clear();
1225  pref = "l";
1226 
1227  // First, the joint names, which apply to all waypoints
1228  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
1229  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
1230  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
1231  joint_names.push_back(pref+"_"+"elbow_flex_joint");
1232  joint_names.push_back(pref+"_"+"forearm_roll_joint");
1233  joint_names.push_back(pref+"_"+"wrist_flex_joint");
1234  joint_names.push_back(pref+"_"+"wrist_roll_joint");
1235 
1236  goal.trajectory.joint_names = joint_names;
1237  goal.trajectory.points[0].positions = left_walk_along_pose_;
1238  goal.trajectory.points[0].velocities.clear();
1239  for(unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
1240  goal.trajectory.points[0].velocities.push_back(0.0);
1241  }
1242 
1243  geometry_msgs::Pose left_walk_pose = getPositionFromJointsPose(left_arm_kinematics_forward_client_,
1244  "l_wrist_roll_link",
1245  joint_names,
1247  tot_distance = sqrt(pow(left_walk_pose.position.x-left_wrist_roll_pose_.position.x,2.0)+
1248  pow(left_walk_pose.position.y-left_wrist_roll_pose_.position.y,2.0)+
1249  pow(left_walk_pose.position.z-left_wrist_roll_pose_.position.z,2.0));
1250  ROS_DEBUG_STREAM("Left dist is " << tot_distance);
1251 
1252  if(tot_distance > .02) {
1253 
1254  //goal.trajectory.points[0].positions[3] = 1.8;
1255 
1256  goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.030);
1257  goal.trajectory.points[0].time_from_start = ros::Duration(3.0);
1258 
1259  unnormalizeTrajectory(goal.trajectory);
1260 
1261  ROS_DEBUG("Sending left arm goal");
1262 
1264  bool finished_before_timeout = left_arm_trajectory_client_->waitForResult(ros::Duration(5.0));
1265 
1267 
1268  if(!finished_before_timeout || state != actionlib::SimpleClientGoalState::SUCCEEDED) {
1270  finished_before_timeout = left_arm_trajectory_client_->waitForResult(ros::Duration(5.0));
1272  if(!finished_before_timeout || state != actionlib::SimpleClientGoalState::SUCCEEDED) {
1273  ROS_WARN("Not in walk along pose");
1274  return false;
1275  }
1276  } else {
1277  ROS_DEBUG("Left arm goal successful");
1278  }
1279  } else {
1280  ROS_DEBUG("No need for arm goal");
1281  }
1282 
1283  //ros::Duration(1.0).sleep();
1284 
1288  //ROS_INFO_STREAM("walk right " << walk_along_right_des_pose_.position.x << " " << walk_along_right_des_pose_.position.y << " " << walk_along_right_des_pose_.position.z);
1289  //ROS_INFO_STREAM("walk left " << walk_along_left_des_pose_.position.x << " " << walk_along_left_des_pose_.position.y << " " << walk_along_left_des_pose_.position.z);
1290 
1291  walk_rdx_vals_.clear();
1292  walk_rdy_vals_.clear();
1293  walk_ldx_vals_.clear();
1294  walk_ldy_vals_.clear();
1295 
1296  return true;
1297 }
1298 
1300 
1301  //not implementing for one-armed robots
1302  if(!control_rarm_ || !control_larm_) {
1303  return false;
1304  }
1305 
1307 
1308  std::vector<std::string> joint_names;
1309  std::string pref = "r";
1310 
1311  // First, the joint names, which apply to all waypoints
1312  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
1313  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
1314  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
1315  joint_names.push_back(pref+"_"+"elbow_flex_joint");
1316  joint_names.push_back(pref+"_"+"forearm_roll_joint");
1317  joint_names.push_back(pref+"_"+"wrist_flex_joint");
1318  joint_names.push_back(pref+"_"+"wrist_roll_joint");
1319 
1320  geometry_msgs::Pose right_walk_pose = getPositionFromJointsPose(right_arm_kinematics_forward_client_,
1321  "r_wrist_roll_link",
1322  joint_names,
1324  double tot_distance = sqrt(pow(right_walk_pose.position.x-right_wrist_roll_pose_.position.x,2.0)+
1325  pow(right_walk_pose.position.y-right_wrist_roll_pose_.position.y,2.0)+
1326  pow(right_walk_pose.position.z-right_wrist_roll_pose_.position.z,2.0));
1327  ROS_DEBUG_STREAM("Right dist is " << tot_distance);
1328 
1329  if(tot_distance > .02) {
1330  walk_along_ok_ = false;
1331  return false;
1332  }
1333 
1334  joint_names.clear();
1335  pref = "l";
1336 
1337  // First, the joint names, which apply to all waypoints
1338  joint_names.push_back(pref+"_"+"shoulder_pan_joint");
1339  joint_names.push_back(pref+"_"+"shoulder_lift_joint");
1340  joint_names.push_back(pref+"_"+"upper_arm_roll_joint");
1341  joint_names.push_back(pref+"_"+"elbow_flex_joint");
1342  joint_names.push_back(pref+"_"+"forearm_roll_joint");
1343  joint_names.push_back(pref+"_"+"wrist_flex_joint");
1344  joint_names.push_back(pref+"_"+"wrist_roll_joint");
1345  geometry_msgs::Pose left_walk_pose = getPositionFromJointsPose(left_arm_kinematics_forward_client_,
1346  "l_wrist_roll_link",
1347  joint_names,
1349  tot_distance = sqrt(pow(left_walk_pose.position.x-left_wrist_roll_pose_.position.x,2.0)+
1350  pow(left_walk_pose.position.y-left_wrist_roll_pose_.position.y,2.0)+
1351  pow(left_walk_pose.position.z-left_wrist_roll_pose_.position.z,2.0));
1352  ROS_DEBUG_STREAM("Left dist is " << tot_distance);
1353 
1354  if(tot_distance > .02) {
1355  walk_along_ok_ = false;
1356  return false;
1357  }
1358  walk_along_ok_ = true;
1359  return true;
1360 }
1361 
1363 
1364  //not implementing for one-armed robots
1365  if(!control_rarm_ || !control_larm_) {
1366  return;
1367  }
1368 
1369  if(walk_rdx_vals_.size() > WALK_BUFFER) {
1370  walk_rdx_vals_.pop_front();
1371  }
1372  if(walk_rdy_vals_.size() > WALK_BUFFER) {
1373  walk_rdy_vals_.pop_front();
1374  }
1375  if(walk_ldx_vals_.size() > WALK_BUFFER) {
1376  walk_ldx_vals_.pop_front();
1377  }
1378  if(walk_ldy_vals_.size() > WALK_BUFFER) {
1379  walk_ldy_vals_.pop_front();
1380  }
1381 
1383 
1384  //ROS_INFO_STREAM("Current right " << right_wrist_roll_pose_.position.x << " " << right_wrist_roll_pose_.position.y << " " << right_wrist_roll_pose_.position.z);
1385  //ROS_INFO_STREAM("Current left " << left_wrist_roll_pose_.position.x << " " << left_wrist_roll_pose_.position.y << " " << left_wrist_roll_pose_.position.z);
1386 
1387  double rdx = right_wrist_roll_pose_.position.x-walk_along_right_des_pose_.position.x;
1388  double rdy = right_wrist_roll_pose_.position.y-walk_along_right_des_pose_.position.y;
1389 
1390  double ldx = left_wrist_roll_pose_.position.x-walk_along_left_des_pose_.position.x;
1391  double ldy = left_wrist_roll_pose_.position.y-walk_along_left_des_pose_.position.y;
1392 
1393  //ROS_INFO_STREAM(rdx << " " << rdy << " " << ldx << " " << ldy);
1394 
1395  walk_rdx_vals_.push_back(rdx);
1396  walk_rdy_vals_.push_back(rdy);
1397  walk_ldx_vals_.push_back(ldx);
1398  walk_ldy_vals_.push_back(ldy);
1399 
1400 }
1401 
1403  double x_dist_max, double x_speed_scale,
1404  double y_dist_max, double y_speed_scale,
1405  double rot_speed_scale) {
1406  //not implementing for one-armed robots
1407  if(!control_rarm_ || !control_larm_) {
1408  return;
1409  }
1410 
1411  if(!walk_along_ok_) {
1412  return;
1413  }
1414 
1416 
1417  double av_rdx = calcAverage(walk_rdx_vals_);
1418  double av_rdy = calcAverage(walk_rdy_vals_);
1419  double av_ldx = calcAverage(walk_ldx_vals_);
1420  double av_ldy = calcAverage(walk_ldy_vals_);
1421 
1422  //ROS_INFO_STREAM(av_rdx << " " << av_rdy << " " << av_ldx << " " << av_ldy);
1423 
1424  if(fabs(av_rdx) < thresh) {
1425  av_rdx = 0.0;
1426  }
1427  if(fabs(av_rdy) < thresh) {
1428  av_rdy = 0.0;
1429  }
1430  if(fabs(av_ldx) < thresh) {
1431  av_ldx = 0.0;
1432  }
1433  if(fabs(av_ldy) < thresh) {
1434  av_ldy = 0.0;
1435  }
1436 
1437  double vx = 0.0;
1438  double vy = 0.0;
1439  double vw = 0.0;
1440 
1441  // if(ldx < -.0001 && rdx > .0001) {
1442 // vw = (fabs(rdx)+fabs(ldx))*rot_scale;
1443 // } else if(ldx > .0001 && rdx < -.0001) {
1444 // vw = (fabs(rdx)+fabs(ldx))*(-rot_scale);
1445 // } else {
1446 // vx = xy_scale*(fmax(fabs(rdx), fabs(ldx)))*((rdx > .001 || ldx > .001) ? 1.0 : -1.0);
1447 // vy = xy_scale*(fmax(fabs(rdy), fabs(ldy)))*((rdy > .001 || ldy > .001) ? 1.0 : -1.0);
1448 // }
1449  //if(fabs(vx) > .0001 || fabs(vy) > .0001 || fabs(vw) > .0001) {
1450  //ROS_INFO_STREAM("Walk along sending " << vx << " " << vy << " " << vw << " scale " << xy_scale);
1451  //}
1452  //sendBaseCommand(vx, vy, vw);
1453 
1454  double av_x = av_rdx/2.0+av_ldx/2.0;
1455  double per_x = fabs(av_x)/x_dist_max;
1456  per_x = std::min(per_x, 1.0);
1457  vx = (per_x*per_x)*x_speed_scale*((av_x > 0) ? 1 : -1);
1458 
1459  double per_y = fabs(av_ldy/2.0)/y_dist_max;
1460  per_y = std::min(per_y, 1.0);
1461  vy = (per_y*per_y)*y_speed_scale*((av_ldy > 0) ? 1 : -1);
1462 
1463  //vx = xy_scale*((av_rdx+av_ldx)/2.0);//((av_rdx, fabs(av_ldx)))*((av_rdx > .001 || av_ldx > .001) ? 1.0 : -1.0);
1464  //vy = av_ldy*xy_scale;
1465  double per_rot = fabs(av_rdy/2.0)/y_dist_max;
1466  per_rot = std::min(per_rot, 1.0);
1467  vw = (per_rot*per_rot)*rot_speed_scale*((av_rdy > 0) ? 1 : -1);;
1468 
1469  //ROS_INFO_STREAM("Walk along sending " << vx << " " << vy << " " << vw);
1470 
1471  sendBaseCommand(vx, vy, vw);
1472 
1473  //Eric's hacking below this line
1474  // vx = ldx * 7.0 + rdx * 2;
1475 // vy = ldy * 7.0 + rdy * 2 - rdx * 3;
1476 // vw = rdx * 12.0;
1477 // sendBaseCommand(vx, vy, vw);
1478 }
1479 
1480 double GeneralCommander::calcAverage(const std::list<double>& av_list) const {
1481  double av = 0.0;
1482  for(std::list<double>::const_iterator it = av_list.begin();
1483  it != av_list.end();
1484  it++) {
1485  av += (*it);
1486  }
1487  av /= av_list.size();
1488  return av;
1489 }
1490 
1492  std::string fk_link,
1493  const std::vector<std::string>& joint_names, const std::vector<double>& joint_pos) {
1494  moveit_msgs::GetPositionFK::Request fk_request;
1495  moveit_msgs::GetPositionFK::Response fk_response;
1496 
1497  geometry_msgs::Pose ret_pose;
1498 
1499  fk_request.header.frame_id = "base_link";
1500  fk_request.fk_link_names.push_back(fk_link);
1501  fk_request.robot_state.joint_state.position.resize(joint_names.size());
1502  fk_request.robot_state.joint_state.name = joint_names;
1503  fk_request.robot_state.joint_state.position = joint_pos;
1504  if(service_client.call(fk_request, fk_response)) {
1505  if(fk_response.error_code.val == fk_response.error_code.SUCCESS) {
1506  ret_pose = fk_response.pose_stamped[0].pose;
1507  } else {
1508  ROS_DEBUG("fk not a success");
1509  }
1510  } else {
1511  ROS_WARN("fk call failed all together");
1512  }
1513  return ret_pose;
1514 }
1515 
1517 
1518  if(!control_head_) return;
1519 
1521 
1522  trajectory_msgs::JointTrajectory traj;
1523 
1524  if(seq == HEAD_NOD) {
1525  traj = head_nod_traj_;
1526  } else if(seq == HEAD_SHAKE){
1527  traj = head_shake_traj_;
1528  }
1529  traj.header.stamp = ros::Time::now() + ros::Duration(0.01);
1530  head_pub_.publish(traj);
1531 }
1532 
1533 void GeneralCommander::powerBoardCallback(const pr2_msgs::PowerBoardStateConstPtr &powerBoardState) {
1534  if(walk_along_ok_) {
1535  if(!powerBoardState->run_stop || !powerBoardState->wireless_stop) {
1536  ROS_DEBUG("Killing walk along due to stop");
1537  walk_along_ok_ = false;
1538  }
1539  }
1540 }
1541 
1543  if(!control_prosilica_) return;
1544  polled_camera::GetPolledImage::Request gpi_req;
1545  polled_camera::GetPolledImage::Response gpi_res;
1546  gpi_req.response_namespace = ns;
1547  if(!prosilica_polling_client_.call(gpi_req, gpi_res)) {
1548  ROS_WARN("Prosilica polling request failed");
1549  }
1550 }
1551 
1553 
1554  //can't tuck just one arm for now
1555  if(!control_rarm_ || !control_larm_) {
1556  return;
1557  }
1558 
1560 
1561  pr2_common_action_msgs::TuckArmsGoal tuck_arm_goal;
1562 
1563  if(arm == ARMS_BOTH) {
1564  tuck_arm_goal.tuck_right = true;
1565  tuck_arm_goal.tuck_left = true;
1566  } else {
1567  ROS_DEBUG("Tucking one arm not supported");
1568  }
1569 
1570  ROS_DEBUG("Sending tuck arms");
1571 
1572  tuck_arms_client_->sendGoalAndWait(tuck_arm_goal, ros::Duration(10.0), ros::Duration(5.0));
1573 }
1574 
1576 
1577  //can't tuck just one arm for now
1578  if(!control_rarm_ || !control_larm_) {
1579  return;
1580  }
1581 
1583 
1584  pr2_common_action_msgs::TuckArmsGoal tuck_arm_goal;
1585 
1586  if(arm == ARMS_BOTH) {
1587  tuck_arm_goal.tuck_right = false;
1588  tuck_arm_goal.tuck_left = false;
1589  } else {
1590  ROS_DEBUG("Untucking one arm not supported");
1591  }
1592 
1593  ROS_DEBUG("Sending untuck arms");
1594 
1595  tuck_arms_client_->sendGoalAndWait(tuck_arm_goal, ros::Duration(10.0), ros::Duration(5.0));
1596 
1597 }
void sendWalkAlongCommand(double thresh, double x_dist_max, double x_speed_scale, double y_dist_max, double y_speed_scale, double rot_scale)
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
void switchControllers(const std::vector< std::string > &start_controllers, const std::vector< std::string > &stop_controllers)
ros::ServiceClient left_arm_kinematics_inverse_client_
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
ros::ServiceClient tilt_laser_service_
std::vector< double > right_des_joint_states_
void unnormalizeTrajectory(trajectory_msgs::JointTrajectory &traj) const
void publish(const boost::shared_ptr< M > &message) const
URDF_EXPORT bool initString(const std::string &xmlstring)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::ServiceClient prosilica_polling_client_
string cmd
static const double MAX_HEAD_TRACK_SPEED
void setHeadMode(HeadControlMode mode)
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
def stop_controllers(names)
void sendHeadSequence(HeadSequence seq)
void sendArmVelCommands(double r_x_vel, double r_y_vel, double r_z_vel, double r_roll_vel, double r_pitch_vel, double r_yaw_vel, double l_x_vel, double l_y_vel, double l_z_vel, double l_roll_vel, double l_pitch_vel, double l_yaw_vel, double hz)
std::list< double > walk_ldy_vals_
void setArmMode(WhichArm which, ArmControlMode mode)
ros::ServiceClient right_arm_kinematics_forward_client_
bool call(MReq &req, MRes &res)
static const unsigned int WALK_BUFFER
void setEulerYPR(tfScalar eulerZ, tfScalar eulerY, tfScalar eulerX)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
void clampDesiredArmPositionsToActual(double max_dist)
#define M_PI
TFSIMD_FORCE_INLINE const tfScalar & getW() const
std::list< double > walk_ldx_vals_
static const double GRIPPER_CLOSE_POSITION
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
#define ROS_WARN(...)
static const double GRIPPER_CLOSE_MAX_EFFORT
void sendWristVelCommands(double right_wrist_vel, double left_wrist_vel, double hz)
actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > * left_arm_trajectory_client_
void setRotation(const Quaternion &q)
geometry_msgs::Pose des_l_wrist_roll_pose_
std::list< double > walk_rdy_vals_
ros::ServiceClient switch_controllers_service_
static const double GRIPPER_OPEN_MAX_EFFORT
static const std::string RIGHT_ARM_MANNEQUIN_CONTROLLER
static const std::string HEAD_POSITION_CONTROLLER
static const std::string RIGHT_HAND_LINK_TO_TRACK
ros::ServiceClient right_arm_kinematics_inverse_client_
void sendBaseCommand(double vx, double vy, double vw)
trajectory_msgs::JointTrajectory head_nod_traj_
std::vector< double > right_walk_along_pose_
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
actionlib::SimpleActionClient< pr2_common_action_msgs::TuckArmsAction > * tuck_arms_client_
ArmControlMode right_arm_control_mode_
static const std::string LEFT_ARM_MANNEQUIN_CONTROLLER
actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > * right_arm_trajectory_client_
void sendProjectorStartStop(bool start)
Vector3 getAxis() const
geometry_msgs::Pose des_r_wrist_roll_pose_
geometry_msgs::Pose left_wrist_roll_pose_
SimpleClientGoalState sendGoalAndWait(const Goal &goal, const ros::Duration &execute_timeout=ros::Duration(0, 0), const ros::Duration &preempt_timeout=ros::Duration(0, 0))
geometry_msgs::Pose right_wrist_roll_pose_
void powerBoardCallback(const pr2_msgs::PowerBoardStateConstPtr &powerBoardState)
void jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
ArmControlMode getArmMode(WhichArm which)
#define ROS_WARN_STREAM(args)
void sendHeadCommand(double req_pan, double req_tilt)
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > * left_gripper_client_
#define ROS_DEBUG_STREAM(args)
bool getJointPosition(const std::string &name, double &pos) const
void requestProsilicaImage(std::string ns)
list joint_names
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > * right_gripper_client_
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
#define ROS_INFO_STREAM(args)
void setLaserMode(LaserControlMode mode)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
std::list< double > walk_rdx_vals_
std::vector< double > left_des_joint_states_
static const std::string LEFT_HAND_LINK_TO_TRACK
double calcAverage(const std::list< double > &av_list) const
bool getParam(const std::string &key, std::string &s) const
void composeWristRotGoal(const std::string pref, pr2_controllers_msgs::JointTrajectoryGoal &goal, std::vector< double > &des_joints, double des_vel, double hz) const
LaserControlMode laser_control_mode_
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static Time now()
void sendGripperCommand(WhichArm which, bool close)
void sendTorsoCommand(double pos, double vel)
trajectory_msgs::JointTrajectory head_shake_traj_
bool robot_model_initialized_
Flag that tells us if the robot model was initialized successfully.
SimpleClientGoalState getState() const
GeneralCommander(bool control_body, bool control_head, bool control_rarm, bool control_larm, bool control_prosilica, std::string arm_controller_name=default_arm_controller_name)
std::map< std::string, double > joint_state_velocity_map_
static const double GRIPPER_OPEN_POSITION
urdf::Model robot_model_
A model of the robot to see which joints wrap around.
std::map< std::string, double > joint_state_position_map_
bool getJointVelocity(const std::string &name, double &vel) const
geometry_msgs::Pose getPositionFromJointsPose(ros::ServiceClient &service_client, std::string fk_link, const std::vector< std::string > &joint_names, const std::vector< double > &joint_pos)
ros::ServiceClient left_arm_kinematics_forward_client_
#define ROS_ERROR(...)
geometry_msgs::Pose walk_along_left_des_pose_
static const std::string HEAD_MANNEQUIN_CONTROLLER
actionlib::SimpleActionClient< pr2_controllers_msgs::PointHeadAction > * head_track_hand_client_
std::vector< double > left_walk_along_pose_
def start_controllers(names)
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
geometry_msgs::Pose walk_along_right_des_pose_
#define ROS_DEBUG(...)


pr2_teleop_general
Author(s): Gil Jones
autogenerated on Sat Feb 27 2021 04:01:05