art_nav: art_nav: queue_mgr.cc Source File

00001 /*
00002  * Queue manager front end for ROS navigator node.
00003  *
00004  *  Copyright (C) 2007, 2010 Austin Robot Technology
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id: queue_mgr.cc 1027 2011-03-06 16:19:07Z jack.oquin $
00008  */
00009 
00010 #include <unistd.h>
00011 
00012 #include <dynamic_reconfigure/server.h>
00013 
00014 #include <art/frames.h>
00015 
00016 #include <art_msgs/IOadrCommand.h>
00017 #include <art_msgs/IOadrState.h>
00018 #include <art/steering.h>
00019 #include <art_map/ZoneOps.h>
00020 
00021 #include <art_msgs/CarCommand.h>
00022 #include <art_nav/NavEstopState.h>
00023 #include <art_nav/NavRoadState.h>
00024 //#include <art_msgs/Observers.h>
00025 
00026 #include "navigator_internal.h"
00027 #include "course.h"
00028 //#include "obstacle.h"
00029 
00041 #define CLASS "NavQueueMgr"
00042 
00043 // The class for the driver
00044 class NavQueueMgr
00045 {
00046 public:
00047     
00048   // Constructor
00049   NavQueueMgr();
00050   ~NavQueueMgr()
00051     {
00052       delete nav_;
00053     };
00054 
00055   bool setup(ros::NodeHandle node);
00056   bool shutdown();
00057   void spin(void);
00058 
00059 private:
00060 
00061   void processNavCmd(const art_msgs::NavigatorCommand::ConstPtr &cmdIn);
00062   void processOdom(const nav_msgs::Odometry::ConstPtr &odomIn);
00063   void processRoadMap(const art_msgs::ArtLanes::ConstPtr &cmdIn);
00064   void processRelays(const art_msgs::IOadrState::ConstPtr &sigIn);
00065   void PublishState(void);
00066   void reconfig(Config &newconfig, uint32_t level);
00067   void SetRelays(void);
00068   void SetSpeed(pilot_command_t pcmd);
00069 
00070   // ROS topics
00071   ros::Publisher  car_cmd_;             // pilot CarCommand
00072   ros::Subscriber nav_cmd_;             // NavigatorCommand topic
00073   ros::Publisher  nav_state_;           // navigator state topic
00074   ros::Subscriber odom_state_;          // odometry
00075   ros::Subscriber roadmap_;             // local road map polygons
00076   ros::Publisher  signals_cmd_;
00077   ros::Subscriber signals_state_;
00078 
00079   //ros::Subscriber observers_;
00080 
00081   // relay variables
00082   bool signal_on_left_;                 // reported turn signal states
00083   bool signal_on_right_;
00084   bool flasher_on_;
00085   bool alarm_on_;
00086 
00087   // Odometry data
00088   nav_msgs::Odometry odom_msg_;
00089 
00090   // time stamps of latest messages received
00091   ros::Time cmd_time_;
00092   ros::Time map_time_;
00093 
00094   // navigator implementation class
00095   Navigator *nav_;
00096 
00097   // configuration callback
00098   dynamic_reconfigure::Server<Config> ccb_;
00099 };
00100 
00102 NavQueueMgr::NavQueueMgr()
00103 {
00104   signal_on_left_ = signal_on_right_ = false;
00105   flasher_on_ = alarm_on_ = false;
00106 
00107   // create control driver, declare dynamic reconfigure callback
00108   nav_ = new Navigator(&odom_msg_);
00109   ccb_.setCallback(boost::bind(&NavQueueMgr::reconfig, this, _1, _2));
00110 }
00111 
00113 void NavQueueMgr::processNavCmd(const
00114                                 art_msgs::NavigatorCommand::ConstPtr &cmdIn)
00115 {
00116   ROS_DEBUG_STREAM("Navigator order:"
00117                    << NavBehavior(cmdIn->order.behavior).Name());
00118   cmd_time_ = cmdIn->header.stamp;
00119   nav_->order = cmdIn->order;
00120 }
00121 
00123 void NavQueueMgr::processOdom(const nav_msgs::Odometry::ConstPtr &odomIn)
00124 {
00125   ROS_DEBUG("Odometry pose: (%.3f, %.3f, %.3f), (%.3f, %.3f, %.3f)",
00126             odomIn->pose.pose.position.x,
00127             odomIn->pose.pose.position.y,
00128             odomIn->pose.pose.position.z,
00129             odomIn->twist.twist.linear.x,
00130             odomIn->twist.twist.linear.y,
00131             odomIn->twist.twist.linear.z);
00132 
00133   float vel = odomIn->twist.twist.linear.x;
00134   ROS_DEBUG("current velocity = %.3f m/sec, (%02.f mph)", vel, mps2mph(vel));
00135   odom_msg_ = *odomIn;
00136 }
00137 
00139 void NavQueueMgr::processRoadMap(const art_msgs::ArtLanes::ConstPtr &mapIn)
00140 {
00141   ROS_DEBUG_STREAM(mapIn->polygons.size() << " lanes polygons received");
00142   map_time_ = mapIn->header.stamp;
00143   nav_->course->lanes_message(*mapIn);
00144 }
00145 
00147 void NavQueueMgr::processRelays(const art_msgs::IOadrState::ConstPtr &sigIn)
00148 {
00149   using art_msgs::IOadrState;
00150 
00151   bool left_on = (sigIn->relays && IOadrState::TURN_LEFT);
00152   if (left_on != signal_on_left_)
00153     {
00154       ROS_INFO("left turn signal now %s", (left_on? "on": "off"));
00155       signal_on_left_ = left_on;
00156     }
00157 
00158   bool right_on = (sigIn->relays && IOadrState::TURN_RIGHT);
00159   if (right_on != signal_on_right_)
00160     {
00161       ROS_INFO("right turn signal now %s", (right_on? "on": "off"));
00162       signal_on_right_ = right_on;
00163     }
00164 
00165   bool flasher_on = (sigIn->relays && IOadrState::FLASHER);
00166   if (flasher_on != flasher_on_)
00167     {
00168       ROS_INFO("flasher now %s", (flasher_on? "on": "off"));
00169       flasher_on_ = flasher_on;
00170     }
00171 
00172   bool alarm_on = (sigIn->relays && IOadrState::ALARM);
00173   if (alarm_on != alarm_on_)
00174     {
00175       ROS_INFO("alarm now %s", (alarm_on? "on": "off"));
00176       alarm_on_ = alarm_on;
00177     }
00178 }
00179 
00180 
00190 void NavQueueMgr::reconfig(Config &newconfig, uint32_t level)
00191 {
00192   ROS_INFO("navigator dynamic reconfigure, level 0x%x", level);
00193   nav_->config_ = newconfig;
00194   nav_->configure();
00195 }
00196 
00198 bool NavQueueMgr::setup(ros::NodeHandle node)
00199 {
00200   // no delay: we always want the most recent data
00201   ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00202   static uint32_t qDepth = 1;
00203 
00204   // topics to read
00205   odom_state_ = node.subscribe("odom", qDepth,
00206                                &NavQueueMgr::processOdom, this, noDelay);
00207   nav_cmd_ = node.subscribe("navigator/cmd", qDepth,
00208                             &NavQueueMgr::processNavCmd, this, noDelay);
00209   roadmap_ = node.subscribe("roadmap_local", qDepth,
00210                             &NavQueueMgr::processRoadMap, this, noDelay);
00211   signals_state_ = node.subscribe("ioadr/state", qDepth,
00212                                   &NavQueueMgr::processRelays, this, noDelay);
00213 
00214   // topics to write
00215   car_cmd_ = node.advertise<art_msgs::CarCommand>("pilot/cmd", qDepth);
00216   nav_state_ =
00217     node.advertise<art_msgs::NavigatorState>("navigator/state", qDepth);
00218   signals_cmd_ = node.advertise<art_msgs::IOadrCommand>("ioadr/cmd", qDepth);
00219 
00220   return true;
00221 }
00222 
00223 // Shutdown the node
00224 bool NavQueueMgr::shutdown()
00225 {
00226   ROS_INFO("Shutting down navigator node");
00227 
00228   // issue immediate halt command to pilot
00229   pilot_command_t cmd;
00230   cmd.velocity = 0.0;
00231   cmd.yawRate = 0.0;
00232   SetSpeed(cmd);
00233 
00234 #if 0
00235   nav_->obstacle->lasers->unsubscribe_lasers();
00236   nav_->odometry->unsubscribe();
00237 #endif
00238 
00239   return 0;
00240 }
00241 
00249 void NavQueueMgr::SetRelays(void)
00250 {
00251   if (signal_on_left_ != (bool) nav_->navdata.signal_left
00252       || signal_on_right_ != (bool) nav_->navdata.signal_right
00253       || flasher_on_ != (bool) nav_->navdata.flasher
00254       || alarm_on_ != (bool) nav_->navdata.alarm)
00255     {
00256       // something needs to change
00257       art_msgs::IOadrCommand sig_cmd;
00258 
00259       // set or reset left signal relay
00260       if (nav_->navdata.signal_left)
00261         sig_cmd.relays_on = art_msgs::IOadrState::TURN_LEFT;
00262       else
00263         sig_cmd.relays_off = art_msgs::IOadrState::TURN_LEFT;
00264 
00265       // or in right signal relay value
00266       if (nav_->navdata.signal_right)
00267         sig_cmd.relays_on |= art_msgs::IOadrState::TURN_RIGHT;
00268       else
00269         sig_cmd.relays_off |= art_msgs::IOadrState::TURN_RIGHT;
00270 
00271       // or in flasher relay value
00272       if (nav_->navdata.flasher)
00273         sig_cmd.relays_on |= art_msgs::IOadrState::FLASHER;
00274       else
00275         sig_cmd.relays_off |= art_msgs::IOadrState::FLASHER;
00276 
00277       // or in alarm relay value
00278       if (nav_->navdata.alarm)
00279         sig_cmd.relays_on |= art_msgs::IOadrState::ALARM;
00280       else
00281         sig_cmd.relays_off |= art_msgs::IOadrState::ALARM;
00282 
00283       ROS_DEBUG("setting relays: left turn %s, right turn %s, "
00284                 "flasher %s, alarm %s",
00285                 (nav_->navdata.signal_left? "on": "off"),
00286                 (nav_->navdata.signal_right? "on": "off"),
00287                 (nav_->navdata.flasher? "on": "off"),
00288                 (nav_->navdata.alarm? "on": "off"));
00289 
00290       // send command to relay driver
00291       signals_cmd_.publish(sig_cmd);
00292     }
00293 }
00294 
00296 void NavQueueMgr::SetSpeed(pilot_command_t pcmd)
00297 {
00298   if (NavEstopState(nav_->navdata.estop) == NavEstopState::Suspend)
00299     {
00300       ROS_DEBUG_THROTTLE(20,
00301                          "Navigator suspended, not sending pilot commands.");
00302       return;
00303     }
00304 
00305   art_msgs::CarCommand cmd;
00306   cmd.header.stamp = ros::Time::now();
00307   cmd.header.frame_id = ArtFrames::vehicle;
00308 
00309   cmd.control.velocity = pcmd.velocity;
00310   float yawRate = pcmd.yawRate;
00311   if (pcmd.velocity < 0)
00312     yawRate = -yawRate;                 // steer in opposite direction
00313   cmd.control.angle =
00314     Steering::steering_angle(fabs(pcmd.velocity), yawRate);
00315     
00316   ROS_DEBUG("Navigator CMD_CAR (%.3f m/s, %.3f degrees)",
00317             cmd.control.velocity, cmd.control.angle);
00318   
00319   car_cmd_.publish(cmd);
00320 }
00321 
00323 void NavQueueMgr::PublishState(void)
00324 {
00325   nav_->navdata.header.stamp = ros::Time::now();
00326   nav_->navdata.header.frame_id = ArtFrames::vehicle;
00327 
00328   ROS_DEBUG("Publishing Navigator state = %s, %s, last_waypt %s"
00329             ", replan_waypt %s, R%d S%d Z%d, next waypt %s, goal chkpt %s",
00330             NavEstopState(nav_->navdata.estop).Name(),
00331             NavRoadState(nav_->navdata.road).Name(),
00332             ElementID(nav_->navdata.last_waypt).name().str,
00333             ElementID(nav_->navdata.replan_waypt).name().str,
00334             (bool) nav_->navdata.reverse,
00335             (bool) nav_->navdata.stopped,
00336             (bool) nav_->navdata.have_zones,
00337             ElementID(nav_->navdata.last_order.waypt[1].id).name().str,
00338             ElementID(nav_->navdata.last_order.chkpt[0].id).name().str);
00339 
00340   // Publish this info for all subscribers
00341   nav_state_.publish(nav_->navdata);
00342 }
00343 
00345 void NavQueueMgr::spin() 
00346 {
00347   ros::Rate cycle(art_msgs::ArtHertz::NAVIGATOR);
00348   while(ros::ok())
00349     {
00350       ros::spinOnce();                  // handle incoming messages
00351 
00352       // invoke appropriate Navigator method, pass result to Pilot
00353       SetSpeed(nav_->navigate());
00354 
00355       SetRelays();
00356 
00357       PublishState();
00358 
00359       // wait for next cycle
00360       cycle.sleep();
00361     }
00362 }
00363 
00365 int main(int argc, char **argv)
00366 {
00367   ros::init(argc, argv, "navigator");
00368   ros::NodeHandle node;
00369   NavQueueMgr nq;
00370   
00371   // set up ROS topics
00372   if (!nq.setup(node))
00373     return 1;
00374 
00375   // keep running until mission completed
00376   nq.spin();
00377 
00378   // shut down node
00379   if (!nq.shutdown())
00380     return 3;
00381 
00382   return 0;
00383 }