stop.cc

Go to the documentation of this file.

/*
 *  Navigator stop controller
 *
 *  Copyright (C) 2007, 2010, Austin Robot Technology
 *  License: Modified BSD Software License Agreement
 *
 *  $Id: stop.cc 872 2010-11-28 13:31:07Z jack.oquin $
 */

#include <art_msgs/ArtVehicle.h>
using art_msgs::ArtVehicle;

#include "navigator_internal.h"
#include "Controller.h"
#include "stop.h"

Stop::Stop(Navigator *navptr, int _verbose):
  Controller(navptr, _verbose)
{
  reset();
};

Stop::~Stop() {};

Controller::result_t Stop::control(pilot_command_t &pcmd)
{
  ART_ERROR("Stop::control() requires a float distance parameter,");
  ART_ERROR("                regular control() interface not supported.");
  return NotApplicable;
}

Controller::result_t Stop::control(pilot_command_t &pcmd,
                                   float distance,
                                   float threshold,
                                   float topspeed)
{
  result_t result = OK;

  // stop_latency compensates for latency in the braking system.
  float abs_speed = fabsf(estimate->twist.twist.linear.x);
  float latencydist = abs_speed * config_->stop_latency;
  float D = distance - latencydist;

  // According to the model, deceleration should be constant, but in
  // the real world latency will cause it to vary, so apply feedback
  // and recompute the model every cycle.
  if (abs_speed < Epsilon::speed)
    abs_speed = 0.0;

  float V = -abs_speed;
  float A = V*V/(2.0*D);

  // see if it is time to begin stopping
  if ((D <= config_->min_stop_distance || A >= config_->stop_deceleration)
      && !stopping)
    {
      stopping = true;
      initial_speed = fmaxf(topspeed,abs_speed);
      if (verbose)
        ART_MSG(2, "begin stopping while %.3fm distant", distance);
    }

  // Once stopping is initiated, keep doing it until reset(), no
  // matter how V changes.
  if (stopping)
    {

      if (D <= threshold)
        {
          pcmd.velocity = 0.0;          // halt immediately.

          // report Finished when stopped within stop polygon
          if (abs_speed < Epsilon::speed)
            result = Finished;
        }
      else if (!creeping && abs_speed >= Epsilon::speed)
        {
          // Not there yet.  Do not let the requested speed increase
          // above initial speed when stopping began, even when the
          // car was accelerating.
          pcmd.velocity = fminf(pcmd.velocity, abs_speed - A);
          pcmd.velocity = fminf(pcmd.velocity, initial_speed);
          if (pcmd.velocity < 0.0)
            pcmd.velocity = 0.0;        // do not change direction
        }
      else if (D <= config_->max_creep_distance)
        {
          if (!creeping && verbose)
            ART_MSG(2, "begin creeping while %.3fm distant", distance);
          creeping = true;
          // stopped too soon, keep creeping forward
          pcmd.velocity = fminf(pcmd.velocity, config_->stop_creep_speed);
        }

      if (verbose >= 2)
        {
          ART_MSG(5, "current, desired speed %.3f m/s, %.3f, decel %.3f m/s/s",
                  abs_speed, pcmd.velocity, A);
          ART_MSG(5, "distance %.3f m, threshold %.3f, latency %.3f",
                  distance, threshold, latencydist);
        }
    }

  trace("stop controller", pcmd, result);
  return result;
};

// reset controller
void Stop::reset(void)
{
  stopping = false;
  creeping = false;
  initial_speed = 0.0;
};