obstacle.h

Go to the documentation of this file.

/* -*- mode: C++ -*-
 *
 *  Navigator obstacle class
 *
 *  Copyright (C) 2007, 2010, Austin Robot Technology
 *  License: Modified BSD Software License Agreement
 *
 *  $Id: obstacle.h 1638 2011-08-11 06:41:43Z jack.oquin $
 */

#ifndef _OBSTACLE_HH_
#define _OBSTACLE_HH_

#include <vector>
#include "ntimer.h"

#include <art_msgs/ObservationArray.h>

class Obstacle
{
 public:

  Obstacle(Navigator *_nav, int _verbose);

  ~Obstacle()
  {
    delete blockage_timer;
  };

  bool blockage_timeout(void)
  {
    return blockage_timer->Check();
  }

  void blocked(void)
  {
    if (verbose >= 5)
      ART_MSG(3, "Starting blockage timer");
    blockage_timer->Start(config_->blockage_timeout_secs);
  }

  bool car_approaching();

  void closest_in_lane(const poly_list_t &lane, float &ahead, float &behind);

  bool initialized(void)
  {
    //return lasers->have_ranges;
    return true;
  };

  float maximum_range(void) {return max_range;}

  art_msgs::Observation observation(art_msgs::Observation::_oid_type oid)
  {
    return obstate.obs[oid];
  }

  bool observer_clear(art_msgs::Observation::_oid_type oid)
  {
    bool clear = obstate.obs[oid].clear && obstate.obs[oid].applicable;

    // if waiting on observers, reset the blockage_timer
    if (!clear)
      blocked();
    return clear;
  }

  void observers_message(const art_msgs::ObservationArrayConstPtr obs_msg);

  bool passing_lane_clear(void);

  void reset(void);

  void unblocked(void)
  {
    if (verbose >= 5)
      ART_MSG(3, "Canceling blockage timer");
    blockage_timer->Cancel();
  }
  
  void update_blockage_state(void)
  {
    if (navdata->stopped != was_stopped ||
        !navdata->stopped)
      {
        if (navdata->stopped)
          blocked();
        else
          unblocked();
        if (verbose >= 4 &&
            navdata->stopped != was_stopped)
          ART_MSG(8, "vehicle %s moving",
                  (navdata->stopped? "stopped": "started"));
        was_stopped = navdata->stopped;
      }
  }

 private:

  // parameters
  float max_range;                      //< maximum scan range

  ros::Subscriber obs_sub_;             //< observations subscription

  // observers data
  art_msgs::ObservationArray obstate;   //< current observers state

  // blockage timer
  NavTimer *blockage_timer;
  bool was_stopped;                     // previous cycle's stop state

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

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

  // returns true if obstacle is within the specified lane
  bool in_lane(MapXY location, const poly_list_t &lane, int start_index);

#if 0 // ignoring obstacles at the moment
  // conversions between laser scan indices and bearings
  float laser_scan_bearing(unsigned index, const player_laser_data_t &scan)
  {
    return scan.min_angle + index * scan.resolution;
  }
  unsigned laser_scan_index(float bearing, const player_laser_data_t &scan)
  {
    unsigned index =
      (unsigned) rintf((bearing - scan.min_angle) / scan.resolution);
    if (index < 0)
      index = 0;
    else if (index > scan.ranges_count)
      index = scan.ranges_count;
    return index;
  }
#endif // ignoring obstacles at the moment
};

#endif // _OBSTACLE_HH_