teleop_pr2.cpp
Go to the documentation of this file.
00001 /*
00002  * teleop_pr2
00003  * Copyright (c) 2008, Willow Garage, Inc.
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the <ORGANIZATION> nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00033 
00034 #include <cstdlib>
00035 #include <cstdio>
00036 #include <unistd.h>
00037 #include <math.h>
00038 #include <fcntl.h>
00039 #include "ros/ros.h"
00040 #include "sensor_msgs/Joy.h"
00041 #include "geometry_msgs/Twist.h"
00042 #include "sensor_msgs/JointState.h"
00043 #include "trajectory_msgs/JointTrajectory.h"
00044 #include "pr2_controllers_msgs/JointTrajectoryControllerState.h"
00045 #include "topic_tools/MuxSelect.h"
00046 #include "std_msgs/String.h"
00047 
00048 #include "std_msgs/Float64.h"
00049 
00050 #define TORSO_TOPIC "torso_controller/command"
00051 #define HEAD_TOPIC "head_traj_controller/command"
00052 const int PUBLISH_FREQ = 20;
00053 
00054 using namespace std;
00055 
00056 class TeleopPR2
00057 {
00058    public:
00059   geometry_msgs::Twist cmd;
00060   double min_torso, max_torso;
00061   double req_torso_vel, torso_step;
00062   //joy::Joy joy;
00063   double req_vx, req_vy, req_vw, req_torso, req_pan, req_tilt;
00064   double req_tilt_vel, req_pan_vel;
00065   double max_vx, max_vy, max_vw, max_vx_run, max_vy_run, max_vw_run;
00066   double max_pan, max_tilt, min_tilt, pan_step, tilt_step;
00067   int axis_vx, axis_vy, axis_vw, axis_pan, axis_tilt;
00068   int deadman_button, run_button, torso_dn_button, torso_up_button, head_button;
00069   bool deadman_no_publish_, torso_publish_, head_publish_;
00070 
00071   bool deadman_, cmd_head;
00072   bool use_mux_, last_deadman_;
00073   std::string last_selected_topic_;
00074 
00075   ros::Time last_recieved_joy_message_time_;
00076   ros::Duration joy_msg_timeout_;
00077 
00078   ros::NodeHandle n_, n_private_;
00079   ros::Publisher vel_pub_;
00080   ros::Publisher head_pub_;
00081   ros::Publisher torso_pub_;
00082   ros::Subscriber joy_sub_;
00083   ros::Subscriber torso_state_sub_;
00084   ros::ServiceClient mux_client_;
00085 
00086   TeleopPR2(bool deadman_no_publish = false) :
00087     max_vx(0.6), max_vy(0.6), max_vw(0.8),
00088     max_vx_run(0.6), max_vy_run(0.6), max_vw_run(0.8),
00089     max_pan(2.7), max_tilt(1.4), min_tilt(-0.4),
00090     pan_step(0.02), tilt_step(0.015),
00091     deadman_no_publish_(deadman_no_publish), 
00092     torso_publish_(false), head_publish_(false),
00093     deadman_(false), cmd_head(false), 
00094     use_mux_(false), last_deadman_(false),
00095     n_private_("~")
00096   { }
00097 
00098   void init()
00099   {
00100         cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
00101         req_pan = req_tilt = 0;
00102         req_torso = 0.0;
00103         req_torso_vel = 0.0;
00104 
00105         //parameters for interaction with a mux on cmd_vel topics
00106         n_private_.param("use_mux", use_mux_, false);
00107 
00108         n_private_.param("max_vx", max_vx, max_vx);
00109         n_private_.param("max_vy", max_vy, max_vy);
00110         n_private_.param("max_vw", max_vw, max_vw);
00111 
00112         // Set max speed while running
00113         n_private_.param("max_vx_run", max_vx_run, max_vx_run);
00114         n_private_.param("max_vy_run", max_vy_run, max_vy_run);
00115         n_private_.param("max_vw_run", max_vw_run, max_vw_run);
00116 
00117         // Head pan/tilt parameters
00118         n_private_.param("max_pan", max_pan, max_pan);
00119         n_private_.param("max_tilt", max_tilt, max_tilt);
00120         n_private_.param("min_tilt", min_tilt, min_tilt);
00121 
00122         n_private_.param("tilt_step", tilt_step, tilt_step);
00123         n_private_.param("pan_step", pan_step, pan_step);
00124 
00125         n_private_.param("axis_pan", axis_pan, 0);
00126         n_private_.param("axis_tilt", axis_tilt, 2);
00127 
00128         n_private_.param("axis_vx", axis_vx, 3);
00129         n_private_.param("axis_vw", axis_vw, 0);
00130         n_private_.param("axis_vy", axis_vy, 2);
00131 
00132         n_private_.param("torso_step", torso_step, 0.01);
00133         n_private_.param("min_torso", min_torso, 0.0);
00134         n_private_.param("max_torso", max_torso, 0.3);
00135 
00136         n_private_.param("deadman_button", deadman_button, 0);
00137         n_private_.param("run_button", run_button, 0);
00138         n_private_.param("torso_dn_button", torso_dn_button, 0);
00139         n_private_.param("torso_up_button", torso_up_button, 0);
00140         n_private_.param("head_button", head_button, 0);
00141 
00142         double joy_msg_timeout;
00143         n_private_.param("joy_msg_timeout", joy_msg_timeout, 0.5); //default to 0.5 seconds timeout
00144         if (joy_msg_timeout <= 0)
00145           {
00146             joy_msg_timeout_ = ros::Duration().fromSec(9999999);//DURATION_MAX;
00147             ROS_DEBUG("joy_msg_timeout <= 0 -> no timeout");
00148           }
00149         else
00150           {
00151             joy_msg_timeout_.fromSec(joy_msg_timeout);
00152             ROS_DEBUG("joy_msg_timeout: %.3f", joy_msg_timeout_.toSec());
00153           }
00154 
00155         ROS_DEBUG("max_vx: %.3f m/s\n", max_vx);
00156         ROS_DEBUG("max_vy: %.3f m/s\n", max_vy);
00157         ROS_DEBUG("max_vw: %.3f deg/s\n", max_vw*180.0/M_PI);
00158 
00159         ROS_DEBUG("max_vx_run: %.3f m/s\n", max_vx_run);
00160         ROS_DEBUG("max_vy_run: %.3f m/s\n", max_vy_run);
00161         ROS_DEBUG("max_vw_run: %.3f deg/s\n", max_vw_run*180.0/M_PI);
00162 
00163         ROS_DEBUG("tilt step: %.3f rad\n", tilt_step);
00164         ROS_DEBUG("pan step: %.3f rad\n", pan_step);
00165 
00166         ROS_DEBUG("axis_vx: %d\n", axis_vx);
00167         ROS_DEBUG("axis_vy: %d\n", axis_vy);
00168         ROS_DEBUG("axis_vw: %d\n", axis_vw);
00169         ROS_DEBUG("axis_pan: %d\n", axis_pan);
00170         ROS_DEBUG("axis_tilt: %d\n", axis_tilt);
00171 
00172         ROS_DEBUG("deadman_button: %d\n", deadman_button);
00173         ROS_DEBUG("run_button: %d\n", run_button);
00174         ROS_DEBUG("torso_dn_button: %d\n", torso_dn_button);
00175         ROS_DEBUG("torso_up_button: %d\n", torso_up_button);
00176         ROS_DEBUG("head_button: %d\n", head_button);
00177         ROS_DEBUG("joy_msg_timeout: %f\n", joy_msg_timeout);
00178 
00179         if (torso_dn_button != 0 && torso_up_button != 0)
00180         {
00181           torso_publish_ = true;
00182           torso_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(TORSO_TOPIC, 1);
00183         }
00184 
00185         if (head_button != 0)
00186         {
00187           head_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(HEAD_TOPIC, 1);
00188           head_publish_ = true;
00189         }
00190 
00191         vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00192 
00193         joy_sub_ = n_.subscribe("joy", 10, &TeleopPR2::joy_cb, this);
00194         torso_state_sub_ = n_.subscribe("torso_controller/state", 1, &TeleopPR2::torsoCB, this);
00195 
00196         //if we're going to use the mux, then we'll subscribe to state changes on the mux
00197         if(use_mux_){
00198           ros::NodeHandle mux_nh("mux");
00199           mux_client_ = mux_nh.serviceClient<topic_tools::MuxSelect>("select");
00200         }
00201       }
00202 
00203   ~TeleopPR2() { }
00204 
00206   void joy_cb(const sensor_msgs::Joy::ConstPtr& joy_msg)
00207   {
00208     //Record this message reciept
00209     last_recieved_joy_message_time_ = ros::Time::now();
00210 
00211     deadman_ = (((unsigned int)deadman_button < joy_msg->buttons.size()) && joy_msg->buttons[deadman_button]);
00212 
00213     if (!deadman_)
00214       return;
00215 
00216     cmd_head = (((unsigned int)head_button < joy_msg->buttons.size()) && joy_msg->buttons[head_button] && head_publish_);
00217 
00218     // Base
00219     bool running = (((unsigned int)run_button < joy_msg->buttons.size()) && joy_msg->buttons[run_button]);
00220     double vx = running ? max_vx_run : max_vx;
00221     double vy = running ? max_vy_run : max_vy;
00222     double vw = running ? max_vw_run : max_vw;
00223 
00224     if((axis_vx >= 0) && (((unsigned int)axis_vx) < joy_msg->axes.size()) && !cmd_head)
00225       req_vx = joy_msg->axes[axis_vx] * vx;
00226     else
00227       req_vx = 0.0;
00228     if((axis_vy >= 0) && (((unsigned int)axis_vy) < joy_msg->axes.size()) && !cmd_head)
00229       req_vy = joy_msg->axes[axis_vy] * vy;
00230     else
00231       req_vy = 0.0;
00232     if((axis_vw >= 0) && (((unsigned int)axis_vw) < joy_msg->axes.size()) && !cmd_head)
00233       req_vw = joy_msg->axes[axis_vw] * vw;
00234     else
00235       req_vw = 0.0;
00236 
00237     // Enforce max/mins for velocity
00238     // Joystick should be [-1, 1], but it might not be
00239     req_vx = max(min(req_vx, vx), -vx);
00240     req_vy = max(min(req_vy, vy), -vy);
00241     req_vw = max(min(req_vw, vw), -vw);
00242 
00243     // Head
00244     // Update commanded position by how joysticks moving
00245     // Don't add commanded position if deadman off
00246     if (deadman_ && cmd_head)
00247     {
00248       if (axis_pan >= 0 && axis_pan < (int)joy_msg->axes.size())
00249       {
00250         req_pan_vel = joy_msg->axes[axis_pan] * pan_step;
00251       }
00252 
00253       if (axis_tilt >= 0 && axis_tilt < (int)joy_msg->axes.size())
00254       {
00255         req_tilt_vel = joy_msg->axes[axis_tilt] * tilt_step;
00256       }
00257     }
00258 
00259     // Torso
00260     bool down = (((unsigned int)torso_dn_button < joy_msg->buttons.size()) && joy_msg->buttons[torso_dn_button]);
00261     bool up = (((unsigned int)torso_up_button < joy_msg->buttons.size()) && joy_msg->buttons[torso_up_button]);
00262 
00263     // Bring torso up/down
00264     if (down && !up)
00265       req_torso_vel = -torso_step;
00266     else if (up && !down)
00267       req_torso_vel = torso_step;
00268     else
00269       req_torso_vel = 0;
00270   }
00271 
00272 
00273   void send_cmd_vel()
00274   {
00275     if(deadman_  &&
00276        last_recieved_joy_message_time_ + joy_msg_timeout_ > ros::Time::now())
00277     {
00278       //check if we need to switch the mux to our topic for teleop
00279       if(use_mux_ && !last_deadman_){
00280         topic_tools::MuxSelect select_srv;
00281         select_srv.request.topic = vel_pub_.getTopic();
00282         if(mux_client_.call(select_srv)){
00283           last_selected_topic_ = select_srv.response.prev_topic;
00284           ROS_DEBUG("Setting mux to %s for teleop", select_srv.request.topic.c_str());
00285         }
00286         else{
00287           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());
00288         }
00289       }
00290 
00291       // Base
00292       cmd.linear.x = req_vx;
00293       cmd.linear.y = req_vy;
00294       cmd.angular.z = req_vw;
00295       vel_pub_.publish(cmd);
00296 
00297       // Torso
00298       if (torso_publish_)
00299       {
00300         double dt = 1.0/double(PUBLISH_FREQ);
00301         double horizon = 5.0 * dt;
00302 
00303         trajectory_msgs::JointTrajectory traj;
00304         traj.header.stamp = ros::Time::now() + ros::Duration(0.01);
00305         traj.joint_names.push_back("torso_lift_joint");
00306         traj.points.resize(1);
00307         traj.points[0].positions.push_back(req_torso + req_torso_vel * horizon);
00308         traj.points[0].velocities.push_back(req_torso_vel);
00309         traj.points[0].time_from_start = ros::Duration(horizon);
00310         torso_pub_.publish(traj);
00311 
00312         // Updates the current positions
00313         req_torso += req_torso_vel * dt;
00314         req_torso = max(min(req_torso, max_torso), min_torso);
00315       }
00316 
00317       // Head
00318       if (cmd_head && head_publish_)
00319       {
00320         double dt = 1.0/double(PUBLISH_FREQ);
00321         double horizon = 3.0 * dt;
00322 
00323         trajectory_msgs::JointTrajectory traj;
00324         traj.header.stamp = ros::Time::now() + ros::Duration(0.01);
00325         traj.joint_names.push_back("head_pan_joint");
00326         traj.joint_names.push_back("head_tilt_joint");
00327         traj.points.resize(1);
00328         traj.points[0].positions.push_back(req_pan + req_pan_vel * horizon);
00329         traj.points[0].velocities.push_back(req_pan_vel);
00330         traj.points[0].positions.push_back(req_tilt + req_tilt_vel * horizon);
00331         traj.points[0].velocities.push_back(req_tilt_vel);
00332         traj.points[0].time_from_start = ros::Duration(horizon);
00333         head_pub_.publish(traj);
00334 
00335         // Updates the current positions
00336         req_pan += req_pan_vel * dt;
00337         req_pan = max(min(req_pan, max_pan), -max_pan);
00338         req_tilt += req_tilt_vel * dt;
00339         req_tilt = max(min(req_tilt, max_tilt), min_tilt);
00340       }
00341 
00342       if (req_torso != 0)
00343         fprintf(stdout,"teleop_base:: %f, %f, %f. Head:: %f, %f. Torso cmd: %f.\n",
00344                 cmd.linear.x, cmd.linear.y, cmd.angular.z, req_pan, req_tilt, req_torso_vel);
00345       else
00346         fprintf(stdout,"teleop_base:: %f, %f, %f. Head:: %f, %f\n",
00347                 cmd.linear.x ,cmd.linear.y, cmd.angular.z, req_pan, req_tilt);
00348     }
00349     else
00350     {
00351       //make sure to set the mux back to whatever topic it was on when we grabbed it if the deadman has just toggled
00352       if(use_mux_ && last_deadman_){
00353         topic_tools::MuxSelect select_srv;
00354         select_srv.request.topic = last_selected_topic_;
00355         if(mux_client_.call(select_srv)){
00356           ROS_DEBUG("Setting mux back to %s", last_selected_topic_.c_str());
00357         }
00358         else{
00359           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());
00360         }
00361       }
00362 
00363       // Publish zero commands iff deadman_no_publish is false
00364       cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
00365       if (!deadman_no_publish_)
00366       {
00367         // Base
00368         vel_pub_.publish(cmd);
00369       }
00370     }
00371 
00372     //make sure we store the state of our last deadman
00373     last_deadman_ = deadman_;
00374   }
00375 
00376   void torsoCB(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr &msg)
00377   {
00378     double xd = req_torso;
00379     const double A = 0.003;
00380     if (fabs(msg->actual.positions[0] - xd) > A*1.001)
00381     {
00382       req_torso = min(max(msg->actual.positions[0] - A, xd), msg->actual.positions[0] + A);
00383     }
00384   }
00385 };
00386 
00387 int main(int argc, char **argv)
00388 {
00389   ros::init(argc, argv, "teleop_pr2");
00390   const char* opt_no_publish    = "--deadman_no_publish";
00391 
00392   bool no_publish = false;
00393   for(int i=1;i<argc;i++)
00394   {
00395     if(!strncmp(argv[i], opt_no_publish, strlen(opt_no_publish)))
00396       no_publish = true;
00397   }
00398 
00399   TeleopPR2 teleop_pr2(no_publish);
00400   teleop_pr2.init();
00401 
00402   ros::Rate pub_rate(PUBLISH_FREQ);
00403 
00404   while (teleop_pr2.n_.ok())
00405   {
00406     ros::spinOnce();
00407     teleop_pr2.send_cmd_vel();
00408     pub_rate.sleep();
00409   }
00410 
00411   exit(0);
00412   return 0;
00413 }
00414 


pr2_teleop
Author(s):
autogenerated on Fri Aug 28 2015 12:05:58