course.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*-
00002  *
00003  *  Navigator course planning class
00004  *
00005  *  Copyright (C) 2007, 2010, Austin Robot Technology
00006  *
00007  *  License: Modified BSD Software License Agreement
00008  *
00009  *  $Id: course.h 1856 2011-11-18 00:39:09Z jack.oquin $
00010  */
00011 
00012 #ifndef _COURSE_HH_
00013 #define _COURSE_HH_
00014 
00015 #include <vector>
00016 
00017 #include <art/infinity.h>
00018 #include <art_msgs/ArtLanes.h>
00019 #include <art_map/coordinates.h>
00020 #include <art_map/zones.h>
00021 
00022 #include "Controller.h"
00023 
00025 class Course
00026 {
00027  public:
00028 
00029   // intersection crossing directions
00030   typedef enum {
00031     Right = -1,
00032     Straight = 0,
00033     Left = 1
00034   } direction_t;
00035 
00037   Course(Navigator *_nav, int _verbose);
00038 
00040   ~Course() {};
00041 
00043   void begin_run_cycle(void);
00044 
00046   void configure();
00047 
00049   void desired_heading(pilot_command_t &pcmd, float offset_ratio = 0.0);
00050 
00052   float distance_in_plan(const MapPose &from,
00053                          const WayPointNode &wp) const;
00054 
00056   float distance_in_plan(const MapPose &from,
00057                          const MapPose &to) const;
00058 
00060   float distance_in_plan(const MapPose &from,
00061                          const MapXY &to) const;
00062 
00064   void end_run_cycle(void);
00065 
00069   int find_aim_polygon(poly_list_t &lane);
00070 
00072   bool find_passing_lane(void);
00073 
00075   void find_travel_lane(bool rejoin);
00076 
00078   bool in_lane(const MapPose &pose) const
00079   {
00080     return in_poly_list(plan, pose);
00081   }
00082 
00084   bool in_poly_list(const poly_list_t &polys, const MapPose &pose) const
00085   {
00086     return (pops->getContainingPoly(polys, pose) >= 0);
00087   }
00088 
00090   direction_t intersection_direction(void);
00091 
00093   direction_t lane_change_direction(void);
00094 
00096   bool lane_waypoint_reached(void);
00097 
00106   void lanes_message(const art_msgs::ArtLanes &lanes);
00107 
00109   void log(const char *str, const poly_list_t &polys);
00110 
00112   void new_waypoint_reached(ElementID new_way)
00113   {
00114     waypoint_checked = true;
00115     navdata->last_waypt = new_way.toMapID();
00116     ROS_DEBUG_STREAM("reached waypoint %s" << new_way.name().str);
00117   };
00118 
00121   bool new_waypts(void);
00122 
00124   void no_waypoint_reached(void)
00125   {
00126     waypoint_checked = true;
00127   };
00128 
00134   bool plan_valid(void)
00135   {
00136     // check that in the current order waypts match
00137     for (int i = 0; i < Order::N_WAYPTS; ++i)
00138       {
00139         if (plan_waypt[i] != order->waypt[i].id)
00140           return false;                 // need a new plan
00141       }
00142     return (!plan.empty()               // valid if plan exists
00143             && !new_plan_lanes);        //   and no new lane polygons
00144   }
00145 
00152   ElementID replan_roadblock(void);
00153 
00155   void reset(void);
00156 
00162   bool same_lane(ElementID id1, ElementID id2)
00163   {
00164     return (id1.same_lane(id2) && id1.pt <= id2.pt);
00165   }
00166 
00171   float stop_waypt_distance(bool same_lane);
00172 
00174   void signal_pass(void)
00175   {
00176     turn_signal_on(passing_left);
00177   }
00178 
00180   void signal_pass_return(void)
00181   {
00182     turn_signal_on(!passing_left);
00183   }
00184 
00190   bool special_waypt(unsigned windex)
00191   {
00192     return (order->waypt[windex].is_stop
00193             || uturn_waypt(windex));
00194   }
00195 
00197   void signal_for_direction(direction_t direction)
00198   {
00199     if (direction == Straight)
00200       {
00201         turn_signals_off();
00202       }
00203     else
00204       {
00205         // turning: true if Left
00206         turn_signal_on(direction == Left);
00207       }
00208   }
00209 
00211   bool switch_to_passing_lane();
00212 
00214   void turn_signals_both_on(void)
00215   {
00216     if (navdata->signal_left || navdata->signal_right)
00217       {
00218         navdata->signal_left = true;
00219         navdata->signal_right = true;
00220         ROS_DEBUG("setting both turn signals on");
00221       }
00222   }
00223 
00225   void turn_signals_off(void)
00226   {
00227     if (navdata->signal_left || navdata->signal_right)
00228       {
00229         navdata->signal_left = false;
00230         navdata->signal_right = false;
00231         ROS_DEBUG("setting turn signals off");
00232       }
00233   }
00234 
00236   void turn_signal_on(bool direction)
00237   {
00238     if (navdata->signal_left != direction
00239         || navdata->signal_right != !direction)
00240       {
00241         navdata->signal_left = direction;
00242         navdata->signal_right = !direction;
00243         ROS_DEBUG("signalling %s", (direction? "left": "right"));
00244       }
00245   }
00246 
00251   float uturn_distance(void);
00252 
00254   int uturn_order_index(void);
00255 
00258   bool uturn_waypt(unsigned windex);
00259 
00261   bool zone_waypoint_reached(void);
00262   bool zone_perimeter_reached(void);
00263   bool spot_waypoint_reached(void);
00264 
00265   // public class data
00266   poly_list_t polygons;                 //< all polygons for local area
00267   poly_list_t plan;                     //< planned course
00268 
00269   poly_list_t passed_lane;              //< original lane being passed
00270   bool passing_left;                    //< when passing, true if to left
00271   MapPose start_pass_location;          //< pose where passing started
00272 
00273   WayPointNode stop_waypt;              //< coming stop or U-turn way-point
00274   poly stop_poly;                       //< polygon containing stop waypt
00275   poly aim_poly;                        //< aim polygon for rejoining plan
00276                                         //  (none if its poly_id = -1)
00277   ZonePerimeterList zones;
00278 
00279   
00280   float max_speed_for_slow_down(const float& final_speed,
00281                                 const float& distance,
00282                                 const float& max, 
00283                                 const float& max_deceleration);
00284   
00285   float max_speed_for_change_in_heading(const float& dheading,
00286                                         const float& distance,
00287                                         const float& max,
00288                                         const float& max_yaw_rate);
00289 
00290   float get_yaw_spring_system(const Polar& aim_polar, int poly_id,
00291                               float poly_heading,
00292                               float max_yaw, float curr_velocity,
00293                               float offset_ratio = 0.0);
00294 
00295   bool spot_ahead();
00296   mapxy_list_t calculate_spot_points(const std::vector<WayPointNode>
00297                                      &new_waypts);
00298   mapxy_list_t calculate_spot_points();
00299   mapxy_list_t calculate_zone_barrier_points();
00300   bool curr_spot();
00301 
00302   bool nqe_special(int i, int j);
00303 
00304  private:
00305 
00306   // Internal state.  Some of these vectors are class variables to
00307   // minimize dynamic memory allocation, instead of making them
00308   // automatic.
00309   ElementID plan_waypt[art_msgs::Order::N_WAYPTS]; //< waypts in the plan
00310   bool new_plan_lanes;                  //< new lanes since plan made
00311   bool waypoint_checked;
00312   int poly_index;                       // index in polygons of odom pose
00313 
00314   // Passing lane selection data.
00315   ElementID adj_lane[2];                // adjacent lane IDs
00316   poly_list_t adj_polys[2];             // adjacent lanes in segment
00317   int passing_lane;                     // index of passing lane (or -1)
00318 
00319   // saved order way-points for road block
00320   ElementID saved_waypt_id[art_msgs::Order::N_WAYPTS];
00321   int saved_replan_num;
00322 
00323   // .cfg variables
00324   double heading_change_ratio;
00325   double k_error;
00326   double k_int;
00327   double k_theta;
00328   double lane_change_secs;
00329   //double lane_steer_time;
00330   double last_error;
00331   double max_speed_for_sharp;
00332   double max_yaw_rate;
00333   //double min_lane_change_dist;
00334   double min_lane_steer_dist;
00335   double spot_waypoint_radius;
00336   double spring_lookahead;
00337   double turning_latency;
00338   //double yaw_ratio;
00339   //double zone_perimeter_radius;
00340   double zone_waypoint_radius;
00341 
00342   // constructor parameters
00343   int verbose;                          // message verbosity level
00344   Navigator *nav;                       // internal navigator class
00345 
00346   // convenience pointers to Navigator class data
00347   PolyOps* pops;                        // polygon operations class
00348   art_msgs::Order *order;               // current commander order
00349   art_msgs::NavigatorState *navdata;   // current navigator state data
00350   nav_msgs::Odometry *odom;             // current odometry position
00351   nav_msgs::Odometry *estimate;         // estimated control position
00352   const Config *config_;
00353 
00355   Polar head_for_waypt(float target_dist);
00356 
00358   void set_plan_waypts(void)
00359   {
00360     for (int i = 0; i < Order::N_WAYPTS; ++i)
00361       plan_waypt[i] = order->waypt[i].id;
00362   }
00363 
00364 
00365 };
00366 
00367 #endif // _COURSE_HH_
 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