Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
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
00084 ros::NodeHandle nh("~");
00085
00086
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
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
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
00165 const char* optflags = "rv?";
00166 int ch;
00167
00168
00169 while(-1 != (ch = getopt(argc, argv, optflags)))
00170 {
00171 switch(ch)
00172 {
00173 case 'r':
00174 startrun_ = true;
00175 break;
00176 case 'v':
00177 verbose_++;
00178 break;
00179 case '?':
00180 default:
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
00211
00212
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
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
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_)
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
00279 Commander commander(verbose_, speed_limit_, graph_, mission_, zones_);
00280
00281
00282 ROS_INFO("begin mission");
00283 ros::Rate cycle(art_msgs::ArtHertz::COMMANDER);
00284 while(ros::ok())
00285 {
00286 ros::spinOnce();
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
00301 if (navState_.estop.state == NavEstopState::Done)
00302 {
00303 ROS_INFO("Estop Done. Stopping.");
00304 break;
00305 }
00306
00307
00308 art_msgs::Order next_order = commander.command(navState_);
00309
00310
00311 if (next_order.behavior.value != NavBehavior::None)
00312 putOrder(next_order);
00313
00314 cycle.sleep();
00315
00316 }
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();
00344 if (navState_.header.stamp != ros::Time())
00345 {
00346 ROS_INFO("Navigator input received");
00347 return true;
00348 }
00349 cycle.sleep();
00350 }
00351 return false;
00352 }
00353
00354 private:
00355
00356
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
00367 ros::Subscriber nav_state_topic_;
00368 ros::Publisher nav_cmd_pub_;
00369 art_msgs::NavigatorState navState_;
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
00395 if (!cmdr.setup(node))
00396 return 2;
00397
00398
00399 if (!cmdr.build_graph())
00400 return 3;
00401
00402
00403 if (!cmdr.wait_for_input())
00404 return 4;
00405
00406
00407 if (!cmdr.spin())
00408 return 5;
00409
00410 return 0;
00411 }