joystick_teleop.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2015, Fetch Robotics Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Fetch Robotics Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY
00021  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00026  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027  */
00028 
00029 // Author: Michael Ferguson, Hanjun Song
00030 
00031 /*
00032  * This is still a work in progress
00033  * In the future, each teleop component would probably be a plugin
00034  */
00035 
00036 #include <algorithm>
00037 #include <boost/thread/mutex.hpp>
00038 
00039 #include <ros/ros.h>
00040 #include <actionlib/client/simple_action_client.h>
00041 
00042 #include <control_msgs/FollowJointTrajectoryAction.h>
00043 #include <control_msgs/GripperCommandAction.h>
00044 #include <geometry_msgs/TwistStamped.h>
00045 #include <nav_msgs/Odometry.h>
00046 #include <sensor_msgs/JointState.h>
00047 #include <sensor_msgs/Joy.h>
00048 #include <topic_tools/MuxSelect.h>
00049 
00050 double integrate(double desired, double present, double max_rate, double dt)
00051 {
00052   if (desired > present)
00053     return std::min(desired, present + max_rate * dt);
00054   else
00055     return std::max(desired, present - max_rate * dt);
00056 }
00057 
00058 
00059 class TeleopComponent
00060 {
00061 public:
00062   TeleopComponent() : active_(false) {}
00063   virtual ~TeleopComponent() {}
00064 
00065   // This gets called whenever new joy message comes in
00066   // returns whether lower priority teleop components should be stopped
00067   virtual bool update(const sensor_msgs::Joy::ConstPtr& joy,
00068                       const sensor_msgs::JointState::ConstPtr& state) = 0;
00069 
00070   // This gets called at set frequency
00071   virtual void publish(const ros::Duration& dt) = 0;
00072 
00073   // Start the component. Must be idempotent.
00074   virtual bool start()
00075   {
00076     active_ = true;
00077     return active_;
00078   }
00079 
00080   // Stop the component. Must be idempotent.
00081   virtual bool stop()
00082   {
00083     active_ = false;
00084     return active_;
00085   }
00086 
00087 protected:
00088   bool active_;
00089 };
00090 
00091 
00092 class BaseTeleop : public TeleopComponent
00093 {
00094 public:
00095   BaseTeleop(const std::string& name, ros::NodeHandle& nh)
00096   {
00097     ros::NodeHandle pnh(nh, name);
00098 
00099     // Button mapping
00100     pnh.param("button_deadman", deadman_, 10);
00101     pnh.param("axis_x", axis_x_, 3);
00102     pnh.param("axis_w", axis_w_, 0);
00103 
00104     // Base limits
00105     pnh.param("max_vel_x", max_vel_x_, 1.0);
00106     pnh.param("min_vel_x", min_vel_x_, -0.5);
00107     pnh.param("max_vel_w", max_vel_w_, 3.0);
00108     pnh.param("max_acc_x", max_acc_x_, 1.0);
00109     pnh.param("max_acc_w", max_acc_w_, 3.0);
00110 
00111     // Maximum windup of acceleration ramping
00112     pnh.param("max_windup_time", max_windup_time, 0.25);
00113 
00114     // Mux for overriding navigation, etc.
00115     pnh.param("use_mux", use_mux_, true);
00116     if (use_mux_)
00117     {
00118       mux_ = nh.serviceClient<topic_tools::MuxSelect>("/cmd_vel_mux/select");
00119     }
00120 
00121     cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/teleop/cmd_vel", 1);
00122     odom_sub_ = nh.subscribe("/odom", 1, &BaseTeleop::odomCallback, this);
00123   }
00124 
00125   virtual bool update(const sensor_msgs::Joy::ConstPtr& joy,
00126                       const sensor_msgs::JointState::ConstPtr& state)
00127   {
00128     bool deadman_pressed = joy->buttons[deadman_];
00129 
00130     if (!deadman_pressed)
00131     {
00132       stop();
00133       return false;
00134     }
00135 
00136     start();
00137 
00138     if (joy->axes[axis_x_] > 0.0)
00139       desired_.linear.x = joy->axes[axis_x_] * max_vel_x_;
00140     else
00141       desired_.linear.x = joy->axes[axis_x_] * -min_vel_x_;
00142     desired_.angular.z = joy->axes[axis_w_] * max_vel_w_;
00143 
00144     // We are active, don't process lower priority components
00145     return true;
00146   }
00147 
00148   virtual void publish(const ros::Duration& dt)
00149   {
00150     if (active_)
00151     {
00152       {
00153         boost::mutex::scoped_lock lock(odom_mutex_);
00154         // Make sure base is actually keeping up with commands
00155         // When accelerating (in either direction) do not continue to ramp our
00156         //   acceleration more than max_windup_time ahead of actually attained speeds.
00157         // This is especially important if robot gets stuck.
00158         if (last_.linear.x >= 0)
00159           last_.linear.x = std::min(last_.linear.x, odom_.twist.twist.linear.x + max_acc_x_ * max_windup_time);
00160         else
00161           last_.linear.x = std::max(last_.linear.x, odom_.twist.twist.linear.x - max_acc_x_ * max_windup_time);
00162         if (last_.angular.z >= 0)
00163           last_.angular.z = std::min(last_.angular.z, odom_.twist.twist.angular.z + max_acc_w_ * max_windup_time);
00164         else
00165           last_.angular.z = std::max(last_.angular.z, odom_.twist.twist.angular.z - max_acc_w_ * max_windup_time);
00166       }
00167       // Ramp commands based on acceleration limits
00168       last_.linear.x = integrate(desired_.linear.x, last_.linear.x, max_acc_x_, dt.toSec());
00169       last_.angular.z = integrate(desired_.angular.z, last_.angular.z, max_acc_w_, dt.toSec());
00170       cmd_vel_pub_.publish(last_);
00171     }
00172   }
00173 
00174   virtual bool start()
00175   {
00176     if (!active_ && use_mux_)
00177     {
00178       // Connect mux
00179       topic_tools::MuxSelect req;
00180       req.request.topic = cmd_vel_pub_.getTopic();
00181       if (mux_.call(req))
00182       {
00183         prev_mux_topic_ = req.response.prev_topic;
00184       }
00185       else
00186       {
00187         ROS_ERROR("Unable to switch mux");
00188       }
00189     }
00190     active_ = true;
00191     return active_;
00192   }
00193 
00194   virtual bool stop()
00195   {
00196     // Publish stop message
00197     last_ = desired_ = geometry_msgs::Twist();
00198     cmd_vel_pub_.publish(last_);
00199     // Disconnect mux
00200     if (active_ && use_mux_)
00201     {
00202       topic_tools::MuxSelect req;
00203       req.request.topic = prev_mux_topic_;
00204       if (!mux_.call(req))
00205       {
00206         ROS_ERROR("Unable to switch mux");
00207         return active_;
00208       }
00209     }
00210     active_ = false;
00211     return active_;
00212   }
00213 
00214 private:
00215   void odomCallback(const nav_msgs::OdometryConstPtr& odom)
00216   {
00217     // Lock mutex on state message
00218     boost::mutex::scoped_lock lock(odom_mutex_);
00219     odom_ = *odom;
00220   }
00221 
00222   // Buttons from params
00223   int deadman_, axis_x_, axis_w_;
00224 
00225   // Limits from params
00226   double max_vel_x_, min_vel_x_, max_vel_w_;
00227   double max_acc_x_, max_acc_w_;
00228 
00229   // Support for multiplexor between teleop and application base commands
00230   bool use_mux_;
00231   std::string prev_mux_topic_;
00232   ros::ServiceClient mux_;
00233 
00234   // Twist output, odometry feedback
00235   ros::Publisher cmd_vel_pub_;
00236   ros::Subscriber odom_sub_;
00237 
00238   // Latest feedback, mutex around it
00239   boost::mutex odom_mutex_;
00240   nav_msgs::Odometry odom_;
00241   // Maximum timestep that our ramping can get ahead of actual velocities
00242   double max_windup_time;
00243 
00244   geometry_msgs::Twist desired_;
00245   geometry_msgs::Twist last_;
00246 };
00247 
00248 
00249 // This controls a single joint through a follow controller (for instance, torso)
00250 class FollowTeleop : public TeleopComponent
00251 {
00252   typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> client_t;
00253 
00254 public:
00255   FollowTeleop(const std::string& name, ros::NodeHandle& nh)
00256   {
00257     ros::NodeHandle pnh(nh, name);
00258 
00259     // Button mapping
00260     pnh.param("button_deadman", deadman_, 10);
00261     pnh.param("button_increase", inc_button_, 12);
00262     pnh.param("button_decrease", dec_button_, 14);
00263 
00264     // Joint Limits
00265     pnh.param("min_position", min_position_, 0.0);
00266     pnh.param("max_position", max_position_, 0.4);
00267     pnh.param("max_velocity", max_velocity_, 0.075);
00268     pnh.param("max_accel", max_acceleration_, 0.25);
00269 
00270     // Should we inhibit lower priority components if running?
00271     pnh.param("inhibit", inhibit_, false);
00272 
00273     // Load topic/joint info
00274     pnh.param<std::string>("joint_name", joint_name_, "torso_lift_joint");
00275     std::string action_name;
00276     pnh.param<std::string>("action_name", action_name, "torso_controller/follow_joint_trajectory");
00277 
00278     client_.reset(new client_t(action_name, true));
00279     if (!client_->waitForServer(ros::Duration(2.0)))
00280     {
00281       ROS_ERROR("%s may not be connected.", action_name.c_str());
00282     }
00283   }
00284 
00285   // This gets called whenever new joy message comes in
00286   virtual bool update(const sensor_msgs::Joy::ConstPtr& joy,
00287                       const sensor_msgs::JointState::ConstPtr& state)
00288   {
00289     bool deadman_pressed = joy->buttons[deadman_];
00290 
00291     if (!deadman_pressed)
00292     {
00293       stop();
00294       // Update joint position
00295       for (size_t i = 0; i < state->name.size(); i++)
00296       {
00297         if (state->name[i] == joint_name_)
00298         {
00299           actual_position_ = state->position[i];
00300           break;
00301         }
00302       }
00303       return false;
00304     }
00305 
00306     if (joy->buttons[inc_button_])
00307     {
00308       desired_velocity_ = max_velocity_;
00309       start();
00310     }
00311     else if (joy->buttons[dec_button_])
00312     {
00313       desired_velocity_ = -max_velocity_;
00314       start();
00315     }
00316     else
00317     {
00318       desired_velocity_ = 0.0;
00319     }
00320 
00321     return inhibit_;
00322   }
00323 
00324   // This gets called at set frequency
00325   virtual void publish(const ros::Duration& dt)
00326   {
00327     if (active_)
00328     {
00329       // Fill in a message (future dated at fixed time step)
00330       double step = 0.25;
00331       double vel = integrate(desired_velocity_, last_velocity_, max_acceleration_, step);
00332       double travel = step * (vel + last_velocity_) / 2.0;
00333       double pos = std::max(min_position_, std::min(max_position_, actual_position_ + travel));
00334       // Send message
00335       control_msgs::FollowJointTrajectoryGoal goal;
00336       goal.trajectory.joint_names.push_back(joint_name_);
00337       trajectory_msgs::JointTrajectoryPoint p;
00338       p.positions.push_back(pos);
00339       p.velocities.push_back(vel);
00340       p.time_from_start = ros::Duration(step);
00341       goal.trajectory.points.push_back(p);
00342       goal.goal_time_tolerance = ros::Duration(0.0);
00343       client_->sendGoal(goal);
00344       // Update based on actual timestep
00345       vel = integrate(desired_velocity_, last_velocity_, max_acceleration_, dt.toSec());
00346       travel = dt.toSec() * (vel + last_velocity_) / 2.0;
00347       actual_position_ = std::max(min_position_, std::min(max_position_, actual_position_ + travel));
00348       last_velocity_ = vel;
00349     }
00350   }
00351 
00352   virtual bool stop()
00353   {
00354     active_ = false;
00355     last_velocity_ = 0.0;
00356     return active_;
00357   }
00358 
00359 private:
00360   int deadman_, inc_button_, dec_button_;
00361   double min_position_, max_position_, max_velocity_, max_acceleration_;
00362   bool inhibit_;
00363   std::string joint_name_;
00364   double actual_position_;
00365   double desired_velocity_, last_velocity_;
00366   boost::shared_ptr<client_t> client_;
00367 };
00368 
00369 // Gripper Teleop
00370 class GripperTeleop : public TeleopComponent
00371 {
00372   typedef actionlib::SimpleActionClient<control_msgs::GripperCommandAction> client_t;
00373 
00374 public:
00375   GripperTeleop(const std::string& name, ros::NodeHandle& nh) :
00376     req_close_(false),
00377     req_open_(false)
00378   {
00379     ros::NodeHandle pnh(nh, name);
00380 
00381     // Button mapping
00382     pnh.param("button_deadman", deadman_, 10);
00383     pnh.param("button_open", open_button_, 0);
00384     pnh.param("button_close", close_button_, 3);
00385 
00386     // Joint Limits
00387     pnh.param("closed_position", min_position_, 0.0);
00388     pnh.param("open_position", max_position_, 0.115);
00389     pnh.param("max_effort", max_effort_, 100.0);
00390 
00391     std::string action_name = "gripper_controller/gripper_action";
00392     client_.reset(new client_t(action_name, true));
00393     if (!client_->waitForServer(ros::Duration(2.0)))
00394     {
00395       ROS_ERROR("%s may not be connected.", action_name.c_str());
00396     }
00397   }
00398 
00399   // This gets called whenever new joy message comes in
00400   // returns whether lower priority teleop components should be stopped
00401   virtual bool update(const sensor_msgs::Joy::ConstPtr& joy,
00402                       const sensor_msgs::JointState::ConstPtr& state)
00403   {
00404     bool deadman_pressed = joy->buttons[deadman_];
00405 
00406     if (deadman_pressed)
00407     {
00408       if (joy->buttons[open_button_])
00409         req_open_ = true;
00410       else if (joy->buttons[close_button_])
00411         req_close_ = true;
00412     }
00413 
00414     return false;
00415   }
00416 
00417   // This gets called at set frequency
00418   virtual void publish(const ros::Duration& dt)
00419   {
00420     if (req_open_)
00421     {
00422       control_msgs::GripperCommandGoal goal;
00423       goal.command.position = max_position_;
00424       goal.command.max_effort = max_effort_;
00425       client_->sendGoal(goal);
00426       req_open_ = false;
00427     }
00428     else if (req_close_)
00429     {
00430       control_msgs::GripperCommandGoal goal;
00431       goal.command.position = min_position_;
00432       goal.command.max_effort = max_effort_;
00433       client_->sendGoal(goal);
00434       req_close_ = false;
00435     }
00436   }
00437 
00438 private:
00439   int deadman_, open_button_, close_button_;
00440   double min_position_, max_position_, max_effort_;
00441   bool req_close_, req_open_;
00442   boost::shared_ptr<client_t> client_;
00443 };
00444 
00445 
00446 // Head Teleop
00447 class HeadTeleop : public TeleopComponent
00448 {
00449   typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> client_t;
00450 
00451 public:
00452   HeadTeleop(const std::string& name, ros::NodeHandle& nh) :
00453     last_pan_(0.0),
00454     last_tilt_(0.0)
00455   {
00456     ros::NodeHandle pnh(nh, name);
00457 
00458     // Button mapping
00459     pnh.param("button_deadman", deadman_, 8);
00460     pnh.param("axis_pan", axis_pan_, 0);
00461     pnh.param("axis_tilt", axis_tilt_, 3);
00462 
00463     // Joint limits
00464     pnh.param("max_vel_pan", max_vel_pan_, 1.5);
00465     pnh.param("max_vel_tilt", max_vel_tilt_, 1.5);
00466     pnh.param("max_acc_pan", max_acc_pan_, 3.0);
00467     pnh.param("max_acc_tilt", max_acc_tilt_, 3.0);
00468     pnh.param("min_pos_pan", min_pos_pan_, -1.57);
00469     pnh.param("max_pos_pan", max_pos_pan_, 1.57);
00470     pnh.param("min_pos_tilt", min_pos_tilt_, -0.76);
00471     pnh.param("max_pos_tilt", max_pos_tilt_, 1.45);
00472 
00473     // TODO: load topic from params
00474     head_pan_joint_ = "head_pan_joint";
00475     head_tilt_joint_ = "head_tilt_joint";
00476 
00477     std::string action_name = "head_controller/follow_joint_trajectory";
00478     client_.reset(new client_t(action_name, true));
00479     if (!client_->waitForServer(ros::Duration(2.0)))
00480     {
00481       ROS_ERROR("%s may not be connected.", action_name.c_str());
00482     }
00483   }
00484 
00485   // This gets called whenever new joy message comes in
00486   virtual bool update(const sensor_msgs::Joy::ConstPtr& joy,
00487                       const sensor_msgs::JointState::ConstPtr& state)
00488   {
00489     bool deadman_pressed = joy->buttons[deadman_];
00490 
00491     if (!deadman_pressed)
00492     {
00493       stop();
00494       // Update joint positions
00495       for (size_t i = 0; i < state->name.size(); i++)
00496       {
00497         if (state->name[i] == head_pan_joint_)
00498           actual_pos_pan_ = state->position[i];
00499         if (state->name[i] == head_tilt_joint_)
00500           actual_pos_tilt_ = state->position[i];
00501       }
00502       return false;
00503     }
00504 
00505     desired_pan_ = joy->axes[axis_pan_] * max_vel_pan_;
00506     desired_tilt_ = joy->axes[axis_tilt_] * max_vel_tilt_;
00507     start();
00508 
00509     return true;
00510   }
00511 
00512   // This gets called at set frequency
00513   virtual void publish(const ros::Duration& dt)
00514   {
00515     if (active_)
00516     {
00517       // Fill in message (future dated with fixed time step)
00518       double step = 0.125;
00519       double pan_vel = integrate(desired_pan_, last_pan_, max_acc_pan_, step);
00520       double pan_travel = step * (pan_vel + last_pan_) / 2.0;
00521       double pan = std::max(min_pos_pan_, std::min(max_pos_pan_, actual_pos_pan_ + pan_travel));
00522       double tilt_vel = integrate(desired_tilt_, last_tilt_, max_acc_tilt_, step);
00523       double tilt_travel = step * (tilt_vel + last_tilt_) / 2.0;
00524       double tilt = std::max(min_pos_tilt_, std::min(max_pos_tilt_, actual_pos_tilt_ + tilt_travel));
00525       // Publish message
00526       control_msgs::FollowJointTrajectoryGoal goal;
00527       goal.trajectory.joint_names.push_back(head_pan_joint_);
00528       goal.trajectory.joint_names.push_back(head_tilt_joint_);
00529       trajectory_msgs::JointTrajectoryPoint p;
00530       p.positions.push_back(pan);
00531       p.positions.push_back(tilt);
00532       p.velocities.push_back(pan_vel);
00533       p.velocities.push_back(tilt_vel);
00534       p.time_from_start = ros::Duration(step);
00535       goal.trajectory.points.push_back(p);
00536       goal.goal_time_tolerance = ros::Duration(0.0);
00537       client_->sendGoal(goal);
00538       // Update based on actual timestep
00539       pan_vel = integrate(desired_pan_, last_pan_, max_acc_pan_, dt.toSec());
00540       pan_travel = dt.toSec() * (pan_vel + last_pan_) / 2.0;
00541       actual_pos_pan_ = std::max(min_pos_pan_, std::min(max_pos_pan_, actual_pos_pan_ + pan_travel));
00542       last_pan_ = pan_vel;
00543       tilt_vel = integrate(desired_tilt_, last_tilt_, max_acc_tilt_, dt.toSec());
00544       tilt_travel = dt.toSec() * (tilt_vel + last_tilt_) / 2.0;
00545       actual_pos_tilt_ = std::max(min_pos_tilt_, std::min(max_pos_tilt_, actual_pos_tilt_ + tilt_travel));
00546       last_tilt_ = tilt_vel;
00547     }
00548   }
00549 
00550   virtual bool stop()
00551   {
00552     active_ = false;
00553     last_pan_ = last_tilt_ = 0.0;  // reset velocities
00554     return active_;
00555   }
00556 
00557 private:
00558   int deadman_, axis_pan_, axis_tilt_;
00559   double max_vel_pan_, max_vel_tilt_;
00560   double max_acc_pan_, max_acc_tilt_;
00561   double min_pos_pan_, max_pos_pan_, min_pos_tilt_, max_pos_tilt_;
00562   std::string head_pan_joint_, head_tilt_joint_;
00563   double actual_pos_pan_, actual_pos_tilt_;  // actual positions
00564   double desired_pan_, desired_tilt_;  // desired velocities
00565   double last_pan_, last_tilt_;
00566   boost::shared_ptr<client_t> client_;
00567 };
00568 
00569 class ArmTeleop : public TeleopComponent
00570 {
00571 public:
00572   ArmTeleop(const std::string& name, ros::NodeHandle& nh)
00573   {
00574     ros::NodeHandle pnh(nh, name);
00575 
00576     pnh.param("axis_x", axis_x_, 3);
00577     pnh.param("axis_y", axis_y_, 2);
00578     pnh.param("axis_z", axis_z_, 1);
00579     pnh.param("axis_roll", axis_roll_, 2);
00580     pnh.param("axis_pitch", axis_pitch_, 3);
00581     pnh.param("axis_yaw", axis_yaw_, 0);
00582 
00583     pnh.param("button_arm_linear", button_linear_, 9);
00584     pnh.param("button_arm_angular", button_angular_, 11);
00585 
00586     // Twist limits
00587     pnh.param("max_vel_x", max_vel_x_, 1.0);
00588     pnh.param("max_vel_y", max_vel_y_, 1.0);
00589     pnh.param("max_vel_z", max_vel_z_, 1.0);
00590     pnh.param("max_acc_x", max_acc_x_, 10.0);
00591     pnh.param("max_acc_y", max_acc_y_, 10.0);
00592     pnh.param("max_acc_z", max_acc_z_, 10.0);
00593 
00594     pnh.param("max_vel_roll", max_vel_roll_, 2.0);
00595     pnh.param("max_vel_pitch", max_vel_pitch_, 2.0);
00596     pnh.param("max_vel_yaw", max_vel_yaw_, 2.0);
00597     pnh.param("max_acc_roll", max_acc_roll_, 10.0);
00598     pnh.param("max_acc_pitch", max_acc_pitch_, 10.0);
00599     pnh.param("max_acc_yaw", max_acc_yaw_, 10.0);
00600 
00601     cmd_pub_ = nh.advertise<geometry_msgs::TwistStamped>("/arm_controller/cartesian_twist/command", 10);
00602   }
00603 
00604   virtual bool update(const sensor_msgs::Joy::ConstPtr& joy,
00605                       const sensor_msgs::JointState::ConstPtr& state)
00606   {
00607     bool button_linear_pressed = joy->buttons[button_linear_];
00608     bool button_angular_pressed = joy->buttons[button_angular_];
00609 
00610     if (!(button_linear_pressed || button_angular_pressed) &&
00611         (ros::Time::now() - last_update_ > ros::Duration(0.5)))
00612     {
00613       stop();
00614       return false;
00615     }
00616 
00617     start();
00618 
00619     if (button_linear_pressed)
00620     {
00621       desired_.twist.linear.x = joy->axes[axis_x_] * max_vel_x_;
00622       desired_.twist.linear.y = joy->axes[axis_y_] * max_vel_y_;
00623       desired_.twist.linear.z = joy->axes[axis_z_] * max_vel_z_;
00624       desired_.twist.angular.x = 0.0;
00625       desired_.twist.angular.y = 0.0;
00626       desired_.twist.angular.z = 0.0;
00627       last_update_ = ros::Time::now();
00628     }
00629     else if (button_angular_pressed)
00630     {
00631       desired_.twist.linear.x = 0.0;
00632       desired_.twist.linear.y = 0.0;
00633       desired_.twist.linear.z = 0.0;
00634       desired_.twist.angular.x = joy->axes[axis_roll_] * max_vel_roll_;
00635       desired_.twist.angular.y = joy->axes[axis_pitch_] * max_vel_pitch_;
00636       desired_.twist.angular.z = joy->axes[axis_yaw_] * max_vel_yaw_;
00637       last_update_ = ros::Time::now();
00638     }
00639     else
00640     {
00641       desired_.twist.linear.x = 0.0;
00642       desired_.twist.linear.y = 0.0;
00643       desired_.twist.linear.z = 0.0;
00644       desired_.twist.angular.x = 0.0;
00645       desired_.twist.angular.y = 0.0;
00646       desired_.twist.angular.z = 0.0;
00647     }
00648 
00649     return true;
00650   }
00651 
00652   virtual void publish(const ros::Duration& dt)
00653   {
00654     if (active_)
00655     {
00656       // Ramp commands based on acceleration limits
00657       last_.twist.linear.x = integrate(desired_.twist.linear.x, last_.twist.linear.x, max_acc_x_, dt.toSec());
00658       last_.twist.linear.y = integrate(desired_.twist.linear.y, last_.twist.linear.y, max_acc_y_, dt.toSec());
00659       last_.twist.linear.z = integrate(desired_.twist.linear.z, last_.twist.linear.z, max_acc_z_, dt.toSec());
00660 
00661       last_.twist.angular.x = integrate(desired_.twist.angular.x, last_.twist.angular.x, max_acc_roll_, dt.toSec());
00662       last_.twist.angular.y = integrate(desired_.twist.angular.y, last_.twist.angular.y, max_acc_pitch_, dt.toSec());
00663       last_.twist.angular.z = integrate(desired_.twist.angular.z, last_.twist.angular.z, max_acc_yaw_, dt.toSec());
00664 
00665       last_.header.frame_id = "base_link";
00666 
00667       cmd_pub_.publish(last_);
00668     }
00669   }
00670 
00671   virtual bool start()
00672   {
00673     active_ = true;
00674     return active_;
00675   }
00676 
00677 
00678   virtual bool stop()
00679   {
00680     // Publish stop message
00681     if (active_)
00682     {
00683       last_ = desired_ = geometry_msgs::TwistStamped();
00684       cmd_pub_.publish(last_);
00685     }
00686 
00687     active_ = false;
00688     return active_;
00689   }
00690 
00691 private:
00692 
00693   // Buttons from params
00694   int axis_x_, axis_y_, axis_z_, axis_roll_, axis_pitch_, axis_yaw_;
00695   int button_linear_, button_angular_;
00696 
00697   // Limits from params
00698   double max_vel_x_, max_vel_y_, max_vel_z_;
00699   double max_vel_roll_, max_vel_pitch_, max_vel_yaw_;
00700   double max_acc_x_, max_acc_y_, max_acc_z_;
00701   double max_acc_roll_, max_acc_pitch_, max_acc_yaw_;
00702 
00703   // Twist output
00704   ros::Publisher cmd_pub_;
00705 
00706   geometry_msgs::TwistStamped desired_;
00707   geometry_msgs::TwistStamped last_;
00708   ros::Time last_update_;
00709 };
00710 
00711 // This pulls all the components together
00712 class Teleop
00713 {
00714   typedef boost::shared_ptr<TeleopComponent> TeleopComponentPtr;
00715 
00716 public:
00717   void init(ros::NodeHandle& nh)
00718   {
00719     bool is_fetch;
00720     nh.param("is_fetch", is_fetch, true);
00721 
00722     // TODO: load these from YAML
00723 
00724     TeleopComponentPtr c;
00725     if (is_fetch)
00726     {
00727       // Torso does not override
00728       c.reset(new FollowTeleop("torso", nh));
00729       components_.push_back(c);
00730 
00731       // Gripper does not override
00732       c.reset(new GripperTeleop("gripper", nh));
00733       components_.push_back(c);
00734 
00735       // Head overrides base
00736       c.reset(new HeadTeleop("head", nh));
00737       components_.push_back(c);
00738 
00739       c.reset(new ArmTeleop("arm", nh));
00740       components_.push_back(c);
00741     }
00742 
00743     // BaseTeleop goes last
00744     c.reset(new BaseTeleop("base", nh));
00745     components_.push_back(c);
00746 
00747     state_msg_.reset(new sensor_msgs::JointState());
00748     joy_sub_ = nh.subscribe("/joy", 1, &Teleop::joyCallback, this);
00749     state_sub_ = nh.subscribe("/joint_states", 10, &Teleop::stateCallback, this);
00750   }
00751 
00752   void publish(const ros::Duration& dt)
00753   {
00754     if (ros::Time::now() - last_update_ > ros::Duration(0.25))
00755     {
00756       // Timed out
00757       for (size_t c = 0; c < components_.size(); c++)
00758       {
00759         components_[c]->stop();
00760       }
00761     }
00762     else
00763     {
00764       for (size_t c = 0; c < components_.size(); c++)
00765       {
00766         components_[c]->publish(dt);
00767       }
00768     }
00769   }
00770 
00771 private:
00772   void joyCallback(const sensor_msgs::Joy::ConstPtr& msg)
00773   {
00774     // Lock mutex on state message
00775     boost::mutex::scoped_lock lock(state_mutex_);
00776 
00777     bool ok = true;
00778     for (size_t c = 0; c < components_.size(); c++)
00779     {
00780       if (ok)
00781       {
00782         ok &= !components_[c]->update(msg, state_msg_);
00783       }
00784       else
00785       {
00786         // supressed by a higher priority component
00787         components_[c]->stop();
00788       }
00789     }
00790     last_update_ = ros::Time::now();
00791   }
00792 
00793   void stateCallback(const sensor_msgs::JointStateConstPtr& msg)
00794   {
00795     // Lock mutex on state message
00796     boost::mutex::scoped_lock lock(state_mutex_);
00797 
00798     // Update each joint based on message
00799     for (size_t msg_j = 0; msg_j < msg->name.size(); msg_j++)
00800     {
00801       size_t state_j;
00802       for (state_j = 0; state_j < state_msg_->name.size(); state_j++)
00803       {
00804         if (state_msg_->name[state_j] == msg->name[msg_j])
00805         {
00806           state_msg_->position[state_j] = msg->position[msg_j];
00807           state_msg_->velocity[state_j] = msg->velocity[msg_j];
00808           break;
00809         }
00810       }
00811       if (state_j == state_msg_->name.size())
00812       {
00813         // New joint
00814         state_msg_->name.push_back(msg->name[msg_j]);
00815         state_msg_->position.push_back(msg->position[msg_j]);
00816         state_msg_->velocity.push_back(msg->velocity[msg_j]);
00817       }
00818     }
00819   }
00820 
00821   std::vector<TeleopComponentPtr> components_;
00822   ros::Time last_update_;
00823   boost::mutex state_mutex_;
00824   sensor_msgs::JointStatePtr state_msg_;
00825   ros::Subscriber joy_sub_, state_sub_;
00826 };
00827 
00828 int main(int argc, char** argv)
00829 {
00830   ros::init(argc, argv, "teleop");
00831   ros::NodeHandle n("~");
00832 
00833   Teleop teleop;
00834   teleop.init(n);
00835 
00836   ros::Rate r(30.0);
00837   while (ros::ok())
00838   {
00839     ros::spinOnce();
00840     teleop.publish(ros::Duration(1/30.0));
00841     r.sleep();
00842   }
00843 
00844   return 0;
00845 }


fetch_teleop
Author(s): Michael Ferguson
autogenerated on Sat Aug 5 2017 04:00:42