course.h

Go to the documentation of this file.

/* -*- mode: C++ -*-
 *
 *  Navigator course planning class
 *
 *  Copyright (C) 2007, 2010, Austin Robot Technology
 *
 *  License: Modified BSD Software License Agreement
 *
 *  $Id: course.h 1856 2011-11-18 00:39:09Z jack.oquin $
 */

#ifndef _COURSE_HH_
#define _COURSE_HH_

#include <vector>

#include <art/infinity.h>
#include <art_msgs/ArtLanes.h>
#include <art_map/coordinates.h>
#include <art_map/zones.h>

#include "Controller.h"

class Course
{
 public:

  // intersection crossing directions
  typedef enum {
    Right = -1,
    Straight = 0,
    Left = 1
  } direction_t;

  Course(Navigator *_nav, int _verbose);

  ~Course() {};

  void begin_run_cycle(void);

  void configure();

  void desired_heading(pilot_command_t &pcmd, float offset_ratio = 0.0);

  float distance_in_plan(const MapPose &from,
                         const WayPointNode &wp) const;

  float distance_in_plan(const MapPose &from,
                         const MapPose &to) const;

  float distance_in_plan(const MapPose &from,
                         const MapXY &to) const;

  void end_run_cycle(void);

  int find_aim_polygon(poly_list_t &lane);

  bool find_passing_lane(void);

  void find_travel_lane(bool rejoin);

  bool in_lane(const MapPose &pose) const
  {
    return in_poly_list(plan, pose);
  }

  bool in_poly_list(const poly_list_t &polys, const MapPose &pose) const
  {
    return (pops->getContainingPoly(polys, pose) >= 0);
  }

  direction_t intersection_direction(void);

  direction_t lane_change_direction(void);

  bool lane_waypoint_reached(void);

  void lanes_message(const art_msgs::ArtLanes &lanes);

  void log(const char *str, const poly_list_t &polys);

  void new_waypoint_reached(ElementID new_way)
  {
    waypoint_checked = true;
    navdata->last_waypt = new_way.toMapID();
    ROS_DEBUG_STREAM("reached waypoint %s" << new_way.name().str);
  };

  bool new_waypts(void);

  void no_waypoint_reached(void)
  {
    waypoint_checked = true;
  };

  bool plan_valid(void)
  {
    // check that in the current order waypts match
    for (int i = 0; i < Order::N_WAYPTS; ++i)
      {
        if (plan_waypt[i] != order->waypt[i].id)
          return false;                 // need a new plan
      }
    return (!plan.empty()               // valid if plan exists
            && !new_plan_lanes);        //   and no new lane polygons
  }

  ElementID replan_roadblock(void);

  void reset(void);

  bool same_lane(ElementID id1, ElementID id2)
  {
    return (id1.same_lane(id2) && id1.pt <= id2.pt);
  }

  float stop_waypt_distance(bool same_lane);

  void signal_pass(void)
  {
    turn_signal_on(passing_left);
  }

  void signal_pass_return(void)
  {
    turn_signal_on(!passing_left);
  }

  bool special_waypt(unsigned windex)
  {
    return (order->waypt[windex].is_stop
            || uturn_waypt(windex));
  }

  void signal_for_direction(direction_t direction)
  {
    if (direction == Straight)
      {
        turn_signals_off();
      }
    else
      {
        // turning: true if Left
        turn_signal_on(direction == Left);
      }
  }

  bool switch_to_passing_lane();

  void turn_signals_both_on(void)
  {
    if (navdata->signal_left || navdata->signal_right)
      {
        navdata->signal_left = true;
        navdata->signal_right = true;
        ROS_DEBUG("setting both turn signals on");
      }
  }

  void turn_signals_off(void)
  {
    if (navdata->signal_left || navdata->signal_right)
      {
        navdata->signal_left = false;
        navdata->signal_right = false;
        ROS_DEBUG("setting turn signals off");
      }
  }

  void turn_signal_on(bool direction)
  {
    if (navdata->signal_left != direction
        || navdata->signal_right != !direction)
      {
        navdata->signal_left = direction;
        navdata->signal_right = !direction;
        ROS_DEBUG("signalling %s", (direction? "left": "right"));
      }
  }

  float uturn_distance(void);

  int uturn_order_index(void);

  bool uturn_waypt(unsigned windex);

  bool zone_waypoint_reached(void);
  bool zone_perimeter_reached(void);
  bool spot_waypoint_reached(void);

  // public class data
  poly_list_t polygons;                 //< all polygons for local area
  poly_list_t plan;                     //< planned course

  poly_list_t passed_lane;              //< original lane being passed
  bool passing_left;                    //< when passing, true if to left
  MapPose start_pass_location;          //< pose where passing started

  WayPointNode stop_waypt;              //< coming stop or U-turn way-point
  poly stop_poly;                       //< polygon containing stop waypt
  poly aim_poly;                        //< aim polygon for rejoining plan
                                        //  (none if its poly_id = -1)
  ZonePerimeterList zones;

  
  float max_speed_for_slow_down(const float& final_speed,
                                const float& distance,
                                const float& max, 
                                const float& max_deceleration);
  
  float max_speed_for_change_in_heading(const float& dheading,
                                        const float& distance,
                                        const float& max,
                                        const float& max_yaw_rate);

  float get_yaw_spring_system(const Polar& aim_polar, int poly_id,
                              float poly_heading,
                              float max_yaw, float curr_velocity,
                              float offset_ratio = 0.0);

  bool spot_ahead();
  mapxy_list_t calculate_spot_points(const std::vector<WayPointNode>
                                     &new_waypts);
  mapxy_list_t calculate_spot_points();
  mapxy_list_t calculate_zone_barrier_points();
  bool curr_spot();

  bool nqe_special(int i, int j);

 private:

  // Internal state.  Some of these vectors are class variables to
  // minimize dynamic memory allocation, instead of making them
  // automatic.
  ElementID plan_waypt[art_msgs::Order::N_WAYPTS]; //< waypts in the plan
  bool new_plan_lanes;                  //< new lanes since plan made
  bool waypoint_checked;
  int poly_index;                       // index in polygons of odom pose

  // Passing lane selection data.
  ElementID adj_lane[2];                // adjacent lane IDs
  poly_list_t adj_polys[2];             // adjacent lanes in segment
  int passing_lane;                     // index of passing lane (or -1)

  // saved order way-points for road block
  ElementID saved_waypt_id[art_msgs::Order::N_WAYPTS];
  int saved_replan_num;

  // .cfg variables
  double heading_change_ratio;
  double k_error;
  double k_int;
  double k_theta;
  double lane_change_secs;
  //double lane_steer_time;
  double last_error;
  double max_speed_for_sharp;
  double max_yaw_rate;
  //double min_lane_change_dist;
  double min_lane_steer_dist;
  double spot_waypoint_radius;
  double spring_lookahead;
  double turning_latency;
  //double yaw_ratio;
  //double zone_perimeter_radius;
  double zone_waypoint_radius;

  // constructor parameters
  int verbose;                          // message verbosity level
  Navigator *nav;                       // internal navigator class

  // convenience pointers to Navigator class data
  PolyOps* pops;                        // polygon operations class
  art_msgs::Order *order;               // current commander order
  art_msgs::NavigatorState *navdata;   // current navigator state data
  nav_msgs::Odometry *odom;             // current odometry position
  nav_msgs::Odometry *estimate;         // estimated control position
  const Config *config_;

  Polar head_for_waypt(float target_dist);

  void set_plan_waypts(void)
  {
    for (int i = 0; i < Order::N_WAYPTS; ++i)
      plan_waypt[i] = order->waypt[i].id;
  }


};

#endif // _COURSE_HH_