ros_node.cc
Go to the documentation of this file.
00001 /*
00002  * Commander node ROS front end
00003  *
00004  *  Copyright (C) 2007, 2010, Austin Robot Technology
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id: ros_node.cc 620 2010-09-25 01:11:51Z jack.oquin $
00008  *
00009  *  Author:  Patrick Beeson, Jack O'Quin
00010  */
00011 
00012 #include <iostream>
00013 
00014 #include <ros/ros.h>
00015 
00016 #include <art_msgs/ArtHertz.h>
00017 #include <art_map/ZoneOps.h>
00018 
00019 #include <art_msgs/NavigatorState.h>
00020 #include <art_nav/NavEstopState.h>
00021 #include <art_nav/NavRoadState.h>
00022 
00023 #include "command.h"
00024 
00076 class CommanderNode
00077 {
00078 public:
00079   CommanderNode()
00080   {
00081     verbose_ = 1;
00082 
00083     // use private node handle to get parameters
00084     ros::NodeHandle nh("~");
00085 
00086     // ROS parameters (in alphabetical order)
00087     frame_id_ = "/map";
00088     if (nh.getParam("frame_id", frame_id_))
00089       {
00090         ROS_INFO_STREAM("map frame ID = " << frame_id_);
00091       }
00092 
00093     nh.param("mdf", mdf_name_, std::string(""));
00094     ROS_INFO_STREAM("MDF: " << mdf_name_);
00095       
00096     nh.param("mission_state", mission_file_, std::string(""));
00097     load_mission_ = (mission_file_ != std::string(""));
00098     if (load_mission_)
00099       ROS_INFO_STREAM("mission state file: " << mission_file_);
00100 
00101     rndf_name_ = "";
00102     std::string rndf_param;
00103     if (nh.searchParam("rndf", rndf_param))
00104       {
00105         nh.param(rndf_param, rndf_name_, std::string(""));
00106         ROS_INFO_STREAM("RNDF: " << rndf_name_);
00107       }
00108     else
00109       {
00110         ROS_ERROR("RNDF not defined");
00111       }
00112 
00113     nh.param("speed_limit", speed_limit_, 7.5);
00114     if (speed_limit_ < 0)
00115       {
00116         ROS_ERROR("Maximum speed must be >= 0");
00117         speed_limit_ = 7.5;
00118       }
00119     ROS_INFO_STREAM("Maximum speed: " << speed_limit_);
00120 
00121     nh.param("start_run", startrun_, false);
00122 
00123     // class objects
00124     rndf_ = new RNDF(rndf_name_);
00125     mdf_ = new MDF(mdf_name_);
00126     graph_ = NULL;
00127     mission_ = NULL;
00128   }
00129 
00130   ~CommanderNode()
00131   {
00132     if (graph_ != NULL)
00133       delete graph_;
00134     if (mission_ != NULL)
00135       delete mission_;
00136     delete mdf_;
00137     delete rndf_;
00138   }
00139 
00141   bool setup(ros::NodeHandle node)
00142   {   
00143     // no delay: we always want the most recent data
00144     ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00145     static int qDepth = 1;
00146     nav_state_topic_ = node.subscribe("navigator/state", qDepth,
00147                                       &CommanderNode::processNavState, this,
00148                                       noDelay);
00149     nav_cmd_pub_ = 
00150       node.advertise<art_msgs::NavigatorCommand>("navigator/cmd", qDepth);
00151     return true;
00152   }
00153 
00155   void processNavState(const art_msgs::NavigatorState::ConstPtr &nst)
00156   {
00157     ROS_DEBUG("navigator state message received");
00158     navState_ = *nst;
00159   }
00160 
00162   bool parse_args(int argc, char** argv)
00163   {
00164     // set the flags
00165     const char* optflags = "rv?";
00166     int ch;
00167 
00168     // use getopt to parse the flags
00169     while(-1 != (ch = getopt(argc, argv, optflags)))
00170       {
00171         switch(ch)
00172           {
00173           case 'r':                     // start run immediately
00174             startrun_ = true;
00175               break;
00176           case 'v':                     // extra verbosity
00177             verbose_++;
00178               break;
00179           case '?': // help
00180           default:  // unknown
00181             print_usage(argc, argv);
00182             return false;
00183           }
00184       }
00185 
00186     if (optind < argc)
00187       {
00188         std::cerr << "ERROR: invalid extra parameter: " 
00189                   << argv[optind] << std::endl;
00190         print_usage(argc, argv);
00191         return false;
00192       }
00193 
00194     return true;
00195   }
00196 
00198   bool build_graph()
00199   {
00200     if (!rndf_->is_valid)
00201       {
00202         ROS_FATAL("RNDF not valid");
00203         return false;;
00204       }
00205 
00206     graph_ = new Graph();
00207     rndf_->populate_graph(*graph_);
00208     graph_->find_mapxy();
00209     graph_->find_implicit_edges();
00210     //zones_ = ZoneOps::build_zone_list_from_rndf(*rndf_, *graph_);    
00211 
00212     // Fill in mission data
00213     if (!mdf_->is_valid)
00214       {
00215         ROS_FATAL("MDF not valid");
00216         return false;;
00217       }
00218 
00219     mdf_->add_speed_limits(*graph_);
00220     mission_ = new Mission(*mdf_);
00221 
00222     if (load_mission_) 
00223       {
00224         // Load state of previously started mission
00225         mission_->clear();
00226         if (!mission_->load(mission_file_.c_str(), *graph_))
00227           {
00228             ROS_FATAL_STREAM("Unable to load stored mission file, "
00229                              <<mission_file_<<" is missing or corrupt");
00230             return false;
00231           }
00232         ROS_INFO_STREAM("Loaded stored mission from "<<mission_file_);
00233       }
00234     else
00235       {
00236         // No started mission
00237         if (!mission_->populate_elementid(*graph_))
00238           {
00239             ROS_FATAL("Mission IDs not same size as Element IDs");
00240             return false;
00241           }
00242         ROS_INFO("Running full mission from MDF");
00243       }
00244     
00245     if (mission_->remaining_points() < 1)
00246       {
00247         ROS_FATAL("No checkpoints left");
00248         return false;
00249       }
00250 
00251     return true;
00252   }
00253 
00254   
00256   void putOrder(const art_msgs::Order &order)
00257   {
00258     art_msgs::NavigatorCommand cmd;
00259     cmd.header.stamp = ros::Time::now();
00260     cmd.header.frame_id = frame_id_;
00261     cmd.order = order;
00262     ROS_DEBUG_STREAM("sending behavior "
00263                      << NavBehavior(cmd.order.behavior).Name());
00264     nav_cmd_pub_.publish(cmd);
00265   }
00266 
00268   bool spin()
00269   {
00270     if (startrun_)                      // -r option specified?
00271       {
00272         ROS_INFO("ordering navigator to Run");
00273         art_msgs::Order run_order;
00274         run_order.behavior.value = NavBehavior::Run;
00275         putOrder(run_order);
00276       }
00277 
00278     // initialize Commander class
00279     Commander commander(verbose_, speed_limit_, graph_, mission_, zones_);
00280 
00281     // loop until end of mission
00282     ROS_INFO("begin mission");
00283     ros::Rate cycle(art_msgs::ArtHertz::COMMANDER);
00284     while(ros::ok())
00285       {
00286         ros::spinOnce();                  // handle incoming messages
00287 
00288         ROS_DEBUG_STREAM("navstate = "
00289                          << NavEstopState(navState_.estop).Name()
00290                          << ", " << NavRoadState(navState_.road).Name()
00291                          << ", last_waypt = "
00292                          << ElementID(navState_.last_waypt).name().str
00293                          << ", replan_waypt = "
00294                          << ElementID(navState_.replan_waypt).name().str
00295                          << ", L" << (bool) navState_.lane_blocked
00296                          << " R" << (bool) navState_.road_blocked
00297                          << " S" << (bool) navState_.stopped
00298                          << " Z" << (bool) navState_.have_zones);
00299 
00300         // exit loop when Navigator has shut down
00301         if (navState_.estop.state == NavEstopState::Done)
00302           {
00303             ROS_INFO("Estop Done. Stopping.");
00304             break;
00305           }
00306 
00307         // generate navigator order for this cycle
00308         art_msgs::Order next_order = commander.command(navState_);
00309         
00310         // send next order to Navigator, if any
00311         if (next_order.behavior.value != NavBehavior::None)
00312           putOrder(next_order);
00313 
00314         cycle.sleep();                  // sleep until next cycle
00315 
00316       } //end of mission while loop
00317 
00318     ROS_INFO("Robot shut down.");
00319     return true;
00320   };
00321 
00323   void print_usage(int argc, char** argv)
00324   {
00325     std::cerr << "usage:  rosrun art_nav commander [options]"
00326               << std::endl << std::endl;
00327     std::cerr << "Options:" << std::endl;
00328     std::cerr << "  -r             start running robot immediately"
00329               << std::endl;
00330     std::cerr << "  -v             verbose messages (-vv for more)"
00331               << std::endl;
00332     std::cerr << "  -?             print this help message"
00333               << std::endl;
00334   }
00335 
00337   bool wait_for_input()
00338   {
00339     ROS_INFO("Waiting for navigator input");
00340     ros::Rate cycle(art_msgs::ArtHertz::COMMANDER);
00341     while(ros::ok())
00342       {
00343         ros::spinOnce();                // handle incoming messages
00344         if (navState_.header.stamp != ros::Time())
00345           {
00346             ROS_INFO("Navigator input received");
00347             return true;                // navigator running
00348           }
00349         cycle.sleep();
00350       }
00351     return false;                       // node shut down
00352   }
00353   
00354 private:
00355 
00356   // parameters
00357   std::string mission_file_;
00358   bool load_mission_;
00359   bool startrun_; 
00360   double speed_limit_;
00361   std::string rndf_name_;
00362   std::string mdf_name_;
00363   int verbose_;
00364   std::string frame_id_;        
00365 
00366   // topics and messages
00367   ros::Subscriber nav_state_topic_;       // navigator state topic
00368   ros::Publisher nav_cmd_pub_;            // navigator command topic
00369   art_msgs::NavigatorState navState_;     // last received
00370 
00371   RNDF *rndf_;
00372   MDF *mdf_;
00373   Graph* graph_;
00374   Mission* mission_;
00375   ZonePerimeterList zones_;
00376 };
00377 
00379 int main(int argc, char **argv)
00380 {
00381   ros::init(argc, argv, "commander");
00382   ros::NodeHandle node;
00383 
00384   ROS_INFO("commander starting");
00385 
00386   CommanderNode cmdr;
00387   
00388   if (!cmdr.parse_args(argc,argv))
00389     {
00390       std::cerr<<"\n";
00391       return 1;    
00392     }
00393 
00394   // set up ROS topics
00395   if (!cmdr.setup(node))
00396     return 2;
00397 
00398   // build the road map graph
00399   if (!cmdr.build_graph())
00400     return 3;
00401 
00402   // wait for input topics
00403   if (!cmdr.wait_for_input())
00404     return 4;
00405 
00406   // keep invoking commander until mission completed
00407   if (!cmdr.spin())
00408     return 5;
00409 
00410   return 0;
00411 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:42:11