throttle.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2007, 2007, 2009 Austin Robot Technology
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: throttle.cc 1161 2011-03-26 02:10:49Z jack.oquin $
00007  */
00008 
00009 #include <math.h>
00010 
00011 #include <ros/ros.h>
00012 
00013 #include <art_msgs/ArtHertz.h>
00014 #include <art_msgs/ThrottleCommand.h>
00015 #include <art_msgs/ThrottleState.h>
00016 
00017 #include "devthrottle.h"                // servo device interface
00018 
00019 #define IOADR_MAX_INPUTS  8
00020 
00062 #define CLASS "Throttle"
00063 
00064 class Throttle
00065 {
00066 public:
00067 
00068   Throttle();
00069   ~Throttle()
00070   {
00071     delete dev_;
00072   }
00073   void  Main();
00074   int   Setup(ros::NodeHandle node);
00075   int   Shutdown();
00076 
00077 private:
00078 
00079   void GetCmd(const art_msgs::ThrottleCommand::ConstPtr &cmd);
00080   void PollDevice(void);
00081 
00082   // configuration parameters
00083   std::string port_;                    // tty port name
00084   bool  training_;                      // use training mode
00085   bool  diagnostic_;                    // enable diagnostic mode
00086 
00087   ros::Subscriber throttle_cmd_;        // throttle/cmd
00088   ros::Publisher  throttle_state_;      // throttle/state
00089 
00090   devthrottle *dev_;                    // servo device interface
00091 };
00092 
00093 // constructor, use pull mode with replace
00094 Throttle::Throttle()
00095 {
00096   // use private node handle to get parameters
00097   ros::NodeHandle mynh("~");
00098 
00099   port_ = "/dev/throttle";
00100   mynh.getParam("port", port_);
00101   ROS_INFO_STREAM("steering port = " << port_);
00102   
00103   diagnostic_ = false;
00104   mynh.getParam("diagnostic", diagnostic_);
00105   if (diagnostic_)
00106     ROS_INFO("using diagnostic mode");
00107 
00108   training_ = false;
00109   mynh.getParam("training", training_);
00110   if (training_)
00111     ROS_INFO("using training mode");
00112 
00113   // allocate and initialize the devthrottle interface
00114   dev_ = new devthrottle(training_);
00115 }
00116 
00117 // Set up the device.  Return 0 if things go well, and -1 otherwise.
00118 int Throttle::Setup(ros::NodeHandle node)
00119 {   
00120   int rc = dev_->Open(port_.c_str());
00121   if (rc != 0)
00122     return -1;
00123 
00124   if (training_ == false)
00125     {
00126       rc = dev_->throttle_absolute(0.0); // set idle throttle
00127       if (rc != 0)
00128         {
00129           dev_->Close();
00130           return -1;
00131         }
00132     }
00133 
00134   // subscribe to relevant ROS topics
00135   static int qDepth = 1;
00136   ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00137   throttle_cmd_ =
00138     node.subscribe("throttle/cmd", qDepth, &Throttle::GetCmd, this, noDelay);
00139   throttle_state_ =
00140     node.advertise<art_msgs::ThrottleState>("throttle/state", qDepth);
00141 
00142   return 0;
00143 }
00144 
00145 // Shutdown the device
00146 int Throttle::Shutdown()
00147 {
00148   if (training_ == false)
00149     dev_->throttle_absolute(0.0);       // set idle throttle
00150   dev_->Close();
00151   return 0;
00152 }
00153 
00154 void Throttle::GetCmd(const art_msgs::ThrottleCommand::ConstPtr &cmd)
00155 {
00156   // ignore all throttle command messages when in training mode
00157   if (training_)
00158     return;
00159 
00160   switch (cmd->request)
00161     {
00162     case art_msgs::ThrottleCommand::Absolute:
00163       dev_->throttle_absolute(cmd->position);
00164       break;
00165     case art_msgs::ThrottleCommand::Relative:
00166       dev_->throttle_relative(cmd->position);
00167       break;
00168     default:
00169       {
00170         ROS_WARN("invalid throttle request %u (ignored)", cmd->request);
00171       }
00172     }
00173 }
00174 
00175 // poll device for current status
00176 //
00177 // if an I/O fails, the corresponding voltages[i] remains unchanged
00178 //
00179 void Throttle::PollDevice(void)
00180 {
00181   int rc = dev_->query_status();        // get controller status
00182   if (rc == 0)                          // any news?
00183     {
00184       art_msgs::ThrottleState msg;
00185 
00186       msg.position = dev_->get_position();
00187       dev_->query_rpms(&msg.rpms);
00188       msg.estop = dev_->query_estop();
00189 
00190       if (diagnostic_)             // publish extra diagnostic data?
00191         {
00192           dev_->query_pid(&msg.pwm, &msg.dstate, &msg.istate);
00193         }
00194 
00195       msg.header.stamp = ros::Time::now();
00196       throttle_state_.publish(msg);
00197     }
00198 }
00199 
00200 // Main function for device thread
00201 void Throttle::Main() 
00202 {
00203   ros::Rate cycle(art_msgs::ArtHertz::THROTTLE); // set driver cycle rate
00204 
00205   while(ros::ok())
00206     {
00207       PollDevice();
00208       ros::spinOnce();                  // handle incoming commands
00209       cycle.sleep();                    // sleep until next cycle
00210     }
00211 }
00212   
00213 
00214 int main(int argc, char** argv)
00215 {
00216   ros::init(argc, argv, "throttle");
00217   ros::NodeHandle node;
00218   Throttle dvr;
00219 
00220   if (dvr.Setup(node) != 0)
00221     return 2;
00222   dvr.Main();
00223   dvr.Shutdown();
00224 
00225   return 0;
00226 }


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12