pilot.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2005-2011 Austin Robot Technology
00003  *  License: Modified BSD Software License Agreement
00004  * 
00005  *  $Id: pilot.cc 1547 2011-05-14 16:36:55Z jack.oquin $
00006  */
00007 
00022 #include <ros/ros.h>
00023 
00024 #include <angles/angles.h>
00025 #include <driver_base/SensorLevels.h>
00026 #include <dynamic_reconfigure/server.h>
00027 
00028 #include "device_impl.h"                // sensor and servo device interfaces
00029 
00030 #include <art_msgs/ArtVehicle.h>
00031 #include <art_msgs/ArtHertz.h>
00032 #include <art_msgs/CarDriveStamped.h>
00033 #include <art_msgs/CarCommand.h>
00034 #include <art_msgs/Epsilon.h>
00035 #include <art_msgs/Gear.h>
00036 #include <art_msgs/LearningCommand.h>
00037 #include <art_msgs/PilotState.h>
00038 
00039 #include <art/conversions.h>
00040 #include <art/steering.h>
00041 
00042 #include <art_pilot/PilotConfig.h>
00043 typedef art_pilot::PilotConfig Config;
00044 
00045 #include "accel.h"
00046 
00047 typedef art_msgs::DriverState DriverState;
00048 using art_msgs::Epsilon;
00049 
00050 
00082 class PilotNode
00083 {
00084 public:
00085 
00086   PilotNode(ros::NodeHandle node);
00087   void spin(void);
00088 
00089 private:
00090 
00091   void adjustSteering(void);
00092   void halt(void);
00093   void monitorHardware(void);
00094   void processCarDrive(const art_msgs::CarDriveStamped::ConstPtr &msg);
00095   void processCarCommand(const art_msgs::CarCommand::ConstPtr &msg);
00096   void processLearning(const art_msgs::LearningCommand::ConstPtr &learningIn);
00097   void reconfig(Config &newconfig, uint32_t level);
00098   void speedControl(void);
00099   void validateTarget(void);
00100 
00101   bool is_shifting_;                    // is transmission active?
00102 
00103   typedef dynamic_reconfigure::Server<Config> ReconfigServer;
00104   boost::shared_ptr<ReconfigServer> reconfig_server_;
00105 
00106   // ROS topics used by this node
00107   ros::Subscriber accel_cmd_;           // CarDriveStamped command
00108   ros::Subscriber car_cmd_;             // CarCommand
00109 
00110   // Device interfaces used by pilot
00111   boost::shared_ptr<device_interface::DeviceBrake> brake_;
00112   boost::shared_ptr<device_interface::DeviceImu> imu_;
00113   boost::shared_ptr<device_interface::DeviceOdom> odom_;
00114   boost::shared_ptr<device_interface::DeviceShifter> shifter_;
00115   boost::shared_ptr<device_interface::DeviceSteering> steering_;
00116   boost::shared_ptr<device_interface::DeviceThrottle> throttle_;
00117 
00118   ros::Subscriber learning_cmd_;
00119 
00120   ros::Publisher pilot_state_;          // pilot state
00121 
00122   // configuration
00123   Config config_;                       // dynamic configuration
00124   ros::Duration timeout_;               // device timeout (sec)
00125 
00126   ros::Time current_time_;              // time current cycle began
00127 
00128   // times when messages received
00129   ros::Time goal_time_;                 // latest goal command
00130 
00131   art_msgs::PilotState pstate_msg_;     // pilot state message
00132 
00133   boost::shared_ptr<pilot::AccelBase> accel_;  // acceleration controller
00134 };
00135 
00136 
00138 // public methods
00140 
00142 PilotNode::PilotNode(ros::NodeHandle node):
00143   is_shifting_(false),
00144   reconfig_server_(new dynamic_reconfigure::Server<Config>)
00145 {
00146   // Must declare dynamic reconfigure callback before initializing
00147   // devices or subscribing to topics.
00148   reconfig_server_->setCallback(boost::bind(&PilotNode::reconfig,
00149                                             this, _1, _2));
00150 
00151   // allocate and initialize device interfaces
00152   brake_.reset(new device_interface::DeviceBrake(node));
00153   imu_.reset(new device_interface::DeviceImu(node));
00154   odom_.reset(new device_interface::DeviceOdom(node));
00155   shifter_.reset(new device_interface::DeviceShifter(node));
00156   steering_.reset(new device_interface::DeviceSteering(node));
00157   throttle_.reset(new device_interface::DeviceThrottle(node));
00158 
00159   // no delay: we always want the most recent data
00160   ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00161   int qDepth = 1;
00162 
00163   // command topics (car_cmd will be deprecated)
00164   accel_cmd_ = node.subscribe("pilot/drive", qDepth,
00165                               &PilotNode::processCarDrive, this, noDelay);
00166   car_cmd_ = node.subscribe("pilot/cmd", qDepth,
00167                             &PilotNode::processCarCommand, this , noDelay);
00168   learning_cmd_ = node.subscribe("pilot/learningCmd", qDepth,
00169                                  &PilotNode::processLearning, this, noDelay);
00170 
00171   // topic for publishing pilot state
00172   pilot_state_ = node.advertise<art_msgs::PilotState>("pilot/state", qDepth);
00173   pstate_msg_.header.frame_id = art_msgs::ArtVehicle::frame_id;
00174 }
00175 
00177 void PilotNode::spin(void)
00178 {
00179   // Main loop
00180   ros::Rate cycle(art_msgs::ArtHertz::PILOT); // set driver cycle rate
00181   while(ros::ok())
00182     {
00183       ros::spinOnce();                  // handle incoming messages
00184 
00185       monitorHardware();                // monitor device status
00186 
00187       // issue control commands
00188       speedControl();
00189       adjustSteering();
00190 
00191       pilot_state_.publish(pstate_msg_); // publish updated state message
00192 
00193       cycle.sleep();                    // sleep until next cycle
00194     }
00195 }
00196 
00197 
00199 // private methods
00201 
00213 void PilotNode::adjustSteering(void)
00214 {
00215   if (config_.human_steering)           // pilot not steering?
00216     return;
00217 
00218   if (fabs(pstate_msg_.target.steering_angle
00219            - pstate_msg_.current.steering_angle)
00220       > Epsilon::steering_angle)
00221     {
00222       // Set the steering angle in degrees.
00223       float steer_degrees =
00224         angles::to_degrees(pstate_msg_.target.steering_angle);
00225       ROS_DEBUG("requesting steering angle = %.1f (degrees)", steer_degrees);
00226       steering_->publish(steer_degrees, current_time_);
00227     }
00228 }
00229 
00245 void PilotNode::halt(void)
00246 {
00247   // absolute value of current velocity in m/sec
00248   float abs_speed = pstate_msg_.current.speed;
00249   if (abs_speed < Epsilon::speed)
00250     {
00251       // Already stopped.  Ease up on the brake to reduce strain on
00252       // the actuator.  Brake hold position *must* be adequate to
00253       // prevent motion, even on a hill.
00254       brake_->publish(config_.brake_hold, current_time_);
00255     }
00256   else
00257     {
00258       // Stop the moving vehicle very quickly.
00259       //
00260       //  Apply min throttle and max brake at the same time.  This is
00261       //  an exception to the general rule of never applying brake and
00262       //  throttle together.  There seems to be enough inertia in the
00263       //  brake mechanism for this to be safe.
00264       throttle_->publish(0.0, current_time_);
00265       brake_->publish(1.0, current_time_);
00266     }
00267   accel_->reset();
00268 }
00269 
00275 void PilotNode::monitorHardware(void)
00276 {
00277   // update current pilot state
00278   current_time_ = ros::Time::now();
00279   pstate_msg_.header.stamp = current_time_;
00280   pstate_msg_.current.acceleration = fabs(imu_->value());
00281   pstate_msg_.current.speed = fabs(odom_->value());
00282   pstate_msg_.current.steering_angle =
00283     angles::from_degrees(steering_->value());
00284   pstate_msg_.current.gear.value = shifter_->value();
00285 
00286   // Stage time should not ever start at zero, but there seems to be a
00287   // bug.  In any case it could be < timeout_ (unlike wall time).
00288   ros::Time recently;              // minimum time for recent messages
00289   if (current_time_ > (recently + timeout_))
00290     {
00291       recently = current_time_ - timeout_;
00292     }
00293 
00294   // accumulate new pilot state based on device states
00295   pstate_msg_.brake.state = brake_->state(recently);
00296   pstate_msg_.imu.state = imu_->state(recently);
00297   pstate_msg_.odom.state = odom_->state(recently);
00298   pstate_msg_.shifter.state = shifter_->state(recently);
00299   pstate_msg_.steering.state = steering_->state(recently);
00300   pstate_msg_.throttle.state = throttle_->state(recently);
00301 
00303 
00304   pstate_msg_.pilot.state = DriverState::RUNNING;
00305   if (pstate_msg_.brake.state != DriverState::RUNNING
00306       || pstate_msg_.imu.state != DriverState::RUNNING
00307       || pstate_msg_.odom.state != DriverState::RUNNING
00308       || (!config_.human_steering
00309           && pstate_msg_.steering.state != DriverState::RUNNING)
00310       || pstate_msg_.throttle.state != DriverState::RUNNING)
00311     {
00312       // pilot is not running
00313       pstate_msg_.pilot.state = DriverState::OPENED;
00314       ROS_WARN_THROTTLE(40, "critical component failure, pilot not running");
00315       // reset latest target request
00316       pstate_msg_.target = art_msgs::CarDrive();
00317     }
00318 }
00319 
00321 void PilotNode::processCarDrive(const art_msgs::CarDriveStamped::ConstPtr &msg)
00322 {
00323   goal_time_ = msg->header.stamp;
00324   pstate_msg_.target = msg->control;
00325   validateTarget();
00326 }
00327 
00329 void PilotNode::processCarCommand(const art_msgs::CarCommand::ConstPtr &msg)
00330 {
00331   ROS_WARN_THROTTLE(100, "CarCommand deprecated: use CarDriveStamped.");
00332 
00333   goal_time_ = msg->header.stamp;
00334   pstate_msg_.target.steering_angle = angles::from_degrees(msg->control.angle);
00335   pstate_msg_.target.behavior.value = art_msgs::PilotBehavior::Run;
00336 
00337   pstate_msg_.target.jerk = 0.0;
00338   pstate_msg_.target.acceleration = 0.0;
00339   pstate_msg_.target.speed = msg->control.velocity;
00340   if (pstate_msg_.target.speed > 0.0)
00341     {
00342       pstate_msg_.target.gear.value = art_msgs::Gear::Drive;
00343     }
00344   else if (pstate_msg_.target.speed < 0.0)
00345     {
00346       // in reverse: make speed positive
00347       pstate_msg_.target.speed = -msg->control.velocity;
00348       pstate_msg_.target.gear.value = art_msgs::Gear::Reverse;
00349     }
00350   else
00351     {
00352       pstate_msg_.target.gear.value = art_msgs::Gear::Naught;
00353     }
00354 
00355   validateTarget();
00356 }
00357 
00359 void PilotNode::processLearning(const art_msgs::LearningCommand::ConstPtr
00360                                 &learningIn)
00361 {
00362   ROS_WARN_THROTTLE(100, "LearningCommand deprecated: use CarDriveStamped.");
00363   pstate_msg_.preempted = (learningIn->pilotActive == 0);
00364   ROS_INFO_STREAM("Pilot is "
00365                   << (pstate_msg_.preempted? "preempted": "active"));
00366 }
00367 
00377 void PilotNode::reconfig(Config &newconfig, uint32_t level)
00378 {
00379   ROS_INFO("pilot dynamic reconfigure, level 0x%08x", level);
00380 
00381   if (level & driver_base::SensorLevels::RECONFIGURE_CLOSE)
00382     {
00383       // reallocate acceleration controller using new configuration
00384       accel_ = pilot::allocAccel(newconfig);
00385     }
00386   else
00387     {
00388       // pass any other parameters to existing acceleration controller
00389       accel_->reconfigure(newconfig);
00390     }
00391 
00392   config_ = newconfig;
00393   timeout_ = ros::Duration(config_.timeout);
00394 }
00395 
00396 
00405 void PilotNode::speedControl(void)
00406 {
00407   // do nothing while pilot preempted for learning speed control (no
00408   // servo commands permitted)
00409   if (pstate_msg_.preempted)
00410     return;
00411 
00412   float dt = 1.0 / art_msgs::ArtHertz::PILOT;
00413   float abs_current_speed = pstate_msg_.current.speed;
00414   float abs_target_speed = pstate_msg_.target.speed;
00415 
00416   if (is_shifting_)
00417     {
00418       if (shifter_->is_reset())
00419         {
00420           // all relays are reset now
00421           is_shifting_ = false;
00422         }
00423       else if (!shifter_->busy())
00424         {
00425           // original operation complete, reset shifter relays
00426           shifter_->publish(art_msgs::Shifter::Reset, current_time_);
00427         }
00428     }
00429 
00430   if (pstate_msg_.current.gear.value == pstate_msg_.target.gear.value
00431       || pstate_msg_.target.gear.value == art_msgs::Gear::Naught)
00432     {
00433       // no shift required
00434       if ((pstate_msg_.pilot.state != DriverState::RUNNING)
00435           || (pstate_msg_.target.gear.value == art_msgs::Gear::Park)
00436           || (pstate_msg_.target.gear.value == art_msgs::Gear::Neutral)
00437           || shifter_->busy())
00438         {
00439           // unable to proceed
00440           halt();
00441         }
00442       else
00443         {
00444           // driving in the correct gear
00445           if ((abs_target_speed < Epsilon::speed)
00446               && ((pstate_msg_.target.acceleration == 0.0)
00447                   || (pstate_msg_.target.acceleration * dt
00448                       > abs_current_speed)))
00449             {
00450               // stop requested and acceleration not specified, or
00451               // almost stopped
00452               halt();
00453             }
00454           else
00455             {
00456               // have acceleration controller adjust speed
00457               accel_->adjust(pstate_msg_, brake_, throttle_);
00458             }
00459         }
00460     }
00461   else
00462     {
00463       // not in desired gear, shift (still) needed
00464       is_shifting_ = true;
00465       if (!shifter_->busy())
00466         {
00467           // request shift until driver reports success
00468           shifter_->publish(pstate_msg_.target.gear.value, current_time_);
00469         }
00470 
00471       halt();                           // never move while shifting
00472     }
00473 }
00474 
00475 
00477 void PilotNode::validateTarget(void)
00478 {
00479   // Warn if negative speed, acceleration and jerk.
00480   if (pstate_msg_.target.speed < 0.0
00481       || pstate_msg_.target.acceleration < 0.0
00482       || pstate_msg_.target.jerk < 0.0)
00483     {
00484       ROS_WARN_THROTTLE(100, "Negative speed, acceleration and jerk "
00485                         "are DEPRECATED (using absolute value).");
00486       pstate_msg_.target.speed = fabs(pstate_msg_.target.speed);
00487       pstate_msg_.target.acceleration = fabs(pstate_msg_.target.acceleration);
00488       pstate_msg_.target.jerk = fabs(pstate_msg_.target.jerk);
00489     }
00490 
00491   if (pstate_msg_.target.gear.value == art_msgs::Gear::Reverse)
00492     {
00493       if (pstate_msg_.target.speed > config_.limit_reverse)
00494         {
00495           ROS_WARN_STREAM_THROTTLE(100, "Requested speed ("
00496                                    << pstate_msg_.target.speed
00497                                    << ") exceeds reverse limit of "
00498                                    << config_.limit_reverse
00499                                    << " m/s");
00500           pstate_msg_.target.speed = config_.limit_reverse;
00501         }
00502     }
00503   else
00504     {
00505       if (pstate_msg_.target.speed > config_.limit_forward)
00506         {
00507           ROS_WARN_STREAM_THROTTLE(100, "Requested speed ("
00508                                    << pstate_msg_.target.speed
00509                                    << ") exceeds limit of "
00510                                    << config_.limit_forward
00511                                    << " m/s");
00512           pstate_msg_.target.speed = config_.limit_forward;
00513         }
00514     }
00515 
00516   // limit steering angle to permitted range
00517   using namespace pilot;
00518   using namespace art_msgs;
00519   pstate_msg_.target.steering_angle = clamp(-ArtVehicle::max_steer_radians,
00520                                             pstate_msg_.target.steering_angle,
00521                                             ArtVehicle::max_steer_radians);
00522 }
00523 
00525 int main(int argc, char** argv)
00526 {
00527   ros::init(argc, argv, "pilot");
00528   ros::NodeHandle node;
00529 
00530   PilotNode pilot(node);
00531   pilot.spin();
00532 
00533   return 0;
00534 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


art_pilot
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:44:02