p2os_teleop.cc
Go to the documentation of this file.
00001 /*
00002  * teleop_base
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 
00031 /*       modifications to teleop_base to work with p2os
00032  *       Copyright (C) 2010  Hunter Allen [allen286@purdue.edu], David Feil-Seifer [dfseifer@usc.edu], Edward T. Kaszubski [kaszubsk@usc.edu]
00033  *
00034  *       This program is free software; you can redistribute it and/or modify
00035  *      it under the terms of the GNU General Public License as published by
00036  *       the Free Software Foundation; either version 2 of the License, or
00037  *       (at your option) any later version.
00038  *       
00039  *       This program is distributed in the hope that it will be useful,
00040  *       but WITHOUT ANY WARRANTY; without even the implied warranty of
00041  *       MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00042  *       GNU General Public License for more details.
00043  *       
00044  *       You should have received a copy of the GNU General Public License
00045  *       along with this program; if not, write to the Free Software
00046  *       Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
00047  *       MA 02110-1301, USA.
00048  */
00049 
00050 
00051 #include <cstdlib>
00052 #include <unistd.h>
00053 #include <math.h>
00054 #include <ros/ros.h>
00055 #include <sensor_msgs/Joy.h>
00056 #include <geometry_msgs/Twist.h>
00057 
00058 class TeleopBase 
00059 {
00060 public:
00061     geometry_msgs::Twist cmd, passthrough_cmd;
00062     double req_vx, req_vy, req_vw;
00063     double max_vx, max_vy, max_vw, max_vx_run, max_vy_run, max_vw_run;
00064     int axis_vx, axis_vy, axis_vw;
00065     int deadman_button, run_button;
00066     bool deadman_no_publish_;
00067     bool deadman_;
00068     bool running_;
00069     
00070     ros::Time last_recieved_joy_message_time_;
00071     ros::Duration joy_msg_timeout_;
00072     
00073     ros::NodeHandle n_;
00074     ros::Publisher vel_pub_;
00075     ros::Subscriber joy_sub_;
00076     ros::Subscriber passthrough_sub_;
00077     
00078     TeleopBase(bool deadman_no_publish = false) 
00079         : max_vx(0.6),
00080           max_vy(0.6), 
00081           max_vw(0.8),
00082           max_vx_run(0.6),
00083           max_vy_run(0.6),
00084           max_vw_run(0.8),
00085           deadman_no_publish_(deadman_no_publish),
00086           running_(false)
00087     {}
00088     
00089     void init()
00090     {
00091         cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
00092         n_.param("max_vx", max_vx, max_vx);
00093         n_.param("max_vy", max_vy, max_vy);
00094         n_.param("max_vw", max_vw, max_vw);
00095         
00096         // Set max speed while running
00097         n_.param("max_vx_run", max_vx_run, max_vx_run);
00098         n_.param("max_vy_run", max_vy_run, max_vy_run);
00099         n_.param("max_vw_run", max_vw_run, max_vw_run);
00100         
00101         n_.param("axis_vx", axis_vx, 3);
00102         n_.param("axis_vw", axis_vw, 0);
00103         n_.param("axis_vy", axis_vy, 2);
00104         
00105         n_.param("deadman_button", deadman_button, 0);
00106         n_.param("run_button", run_button, 0);
00107         
00108         double joy_msg_timeout;
00109         n_.param("joy_msg_timeout", joy_msg_timeout, -1.0); //default to no timeout
00110         if (joy_msg_timeout <= 0)
00111         {
00112             joy_msg_timeout_ = ros::Duration().fromSec(9999999);//DURATION_MAX;
00113             ROS_DEBUG("joy_msg_timeout <= 0 -> no timeout");
00114         }
00115         else
00116         {
00117             joy_msg_timeout_.fromSec(joy_msg_timeout);
00118             ROS_DEBUG("joy_msg_timeout: %.3f", joy_msg_timeout_.toSec());
00119         }
00120         
00121         ROS_DEBUG("max_vx: %.3f m/s\n", max_vx);
00122         ROS_DEBUG("max_vy: %.3f m/s\n", max_vy);
00123         ROS_DEBUG("max_vw: %.3f deg/s\n", max_vw*180.0/M_PI);
00124         
00125         ROS_DEBUG("max_vx_run: %.3f m/s\n", max_vx_run);
00126         ROS_DEBUG("max_vy_run: %.3f m/s\n", max_vy_run);
00127         ROS_DEBUG("max_vw_run: %.3f deg/s\n", max_vw_run*180.0/M_PI);
00128         
00129         ROS_DEBUG("axis_vx: %d\n", axis_vx);
00130         ROS_DEBUG("axis_vy: %d\n", axis_vy);
00131         ROS_DEBUG("axis_vw: %d\n", axis_vw);
00132         
00133         
00134         ROS_INFO("deadman_button: %d", deadman_button);
00135         ROS_INFO("run_button: %d", run_button);
00136         ROS_DEBUG("joy_msg_timeout: %f\n", joy_msg_timeout);
00137         
00138         vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00139         passthrough_sub_ = n_.subscribe( "des_vel", 10, &TeleopBase::passthrough_cb, this );
00140         joy_sub_ = n_.subscribe("joy", 10, &TeleopBase::joy_cb, this);
00141         
00142         
00143     }
00144     
00145     ~TeleopBase() {}
00146     
00147     void passthrough_cb( const geometry_msgs::TwistConstPtr& pass_msg )
00148     {
00149         ROS_DEBUG( "passthrough_cmd: [%f,%f]", passthrough_cmd.linear.x, passthrough_cmd.angular.z );
00150         passthrough_cmd = *pass_msg;
00151     }
00152     
00153     void joy_cb(const sensor_msgs::Joy::ConstPtr& joy_msg)
00154     {
00155         
00156         deadman_ = (((unsigned int)deadman_button < joy_msg->buttons.size()) && joy_msg->buttons[deadman_button]);
00157         
00158         if (!deadman_)
00159             return;
00160         
00161         //Record this message reciept
00162         last_recieved_joy_message_time_ = ros::Time::now();
00163         
00164         // Base
00165         running_ = (((unsigned int)run_button < joy_msg->buttons.size()) && joy_msg->buttons[run_button]);
00166         double vx = running_ ? max_vx_run : max_vx;
00167         double vy = running_ ? max_vy_run : max_vy;
00168         double vw = running_ ? max_vw_run : max_vw;
00169         
00170         if((axis_vx >= 0) && (((unsigned int)axis_vx) < joy_msg->axes.size()))
00171             req_vx = joy_msg->axes[axis_vx] * vx;
00172         else
00173             req_vx = 0.0;
00174         if((axis_vy >= 0) && (((unsigned int)axis_vy) < joy_msg->axes.size()))
00175             req_vy = joy_msg->axes[axis_vy] * vy;
00176         else
00177             req_vy = 0.0;
00178         if((axis_vw >= 0) && (((unsigned int)axis_vw) < joy_msg->axes.size()))
00179             req_vw = joy_msg->axes[axis_vw] * vw;
00180         else
00181             req_vw = 0.0;
00182         
00183     }
00184     
00185     void send_cmd_vel()
00186     {
00187         if (deadman_ && last_recieved_joy_message_time_ + joy_msg_timeout_ > ros::Time::now() && running_) {
00188             cmd.linear.x = req_vx;
00189             cmd.linear.y = req_vy;
00190             cmd.angular.z = req_vw;
00191             vel_pub_.publish(cmd);
00192             
00193             fprintf(stdout,"teleop_base:: %f, %f, %f\n",cmd.linear.x,cmd.linear.y,cmd.angular.z);
00194         } else {
00195             cmd = passthrough_cmd;
00196             if (!deadman_no_publish_)
00197                 vel_pub_.publish(cmd);//Only publish if deadman_no_publish is enabled
00198         }
00199     }
00200 };
00201 
00202 int main(int argc, char ** argv)
00203 {
00204     ros::init(argc, argv, "teleop_base");
00205     ros::NodeHandle nh;
00206     
00207     bool no_publish = false;
00208     for(unsigned int x = 1; x < argc; ++x)
00209     {
00210         if(!strncmp(argv[x], "--deadman_no_publish", strlen("--deadman_no_publish")))
00211             no_publish = true;
00212     }
00213     
00214     //There's no reason for this to be 20 Hz pub speed...
00215     //Reducing to 10Hz @allen286
00216     ros::Rate pub_rate(10);
00217     
00218     TeleopBase teleop_base(no_publish);
00219     teleop_base.init();
00220     
00221     while (ros::ok())
00222     {
00223         ros::spinOnce(); 
00224         teleop_base.send_cmd_vel();
00225         pub_rate.sleep();
00226     }
00227 
00228     return 0;
00229 }


p2os_teleop
Author(s): Hunter Allen , David Feil-Seifer , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Wed Aug 26 2015 15:15:02