keyNavSafety.cpp

Go to the documentation of this file.

#include <youbot_overhead_localization/keyNavSafety.h>

keyNavSafety::keyNavSafety()
{
  moveInitialized = false;
  localizationInitialized = false;
  currentCost.cost = 0;

  std::string moveCmdTopicName;

  if (!n.getParam("/key_nav_safety/timeBetweenStopCmds", timeBetweenStopCmds))
    timeBetweenStopCmds = 5;

  //subscriber and publisher initialization
  if (!n.getParam("/key_nav_safety/moveCommandTopic", moveCmdTopicName))
    baseCommandPublisher = n.advertise<geometry_msgs::Twist>("/cmd_vel", -1);
  else
    baseCommandPublisher = n.advertise<geometry_msgs::Twist>(moveCmdTopicName, -1);

  baseCommandSubscriber = n.subscribe("/key_nav_safety/vel", -1, &keyNavSafety::baseCommandCallback, this);
  costSubscriber = n.subscribe("/base_localizer/cost", 1, &keyNavSafety::costCallback, this);
  localizationSubscriber = n.subscribe("/base_localizer/pose_2d", 1, &keyNavSafety::localizationCallback, this);
  costClient = n.serviceClient<youbot_overhead_localization::GetCost>("/path_planner/get_cost");
  stopCommandFound = true;
  lastStopCmdTime = 0;
}

void keyNavSafety::costCallback(const youbot_overhead_localization::CostmapCost& newCost)
{
  currentCost.cost = newCost.cost;
  //adjust speed based on cost
  if (moveInitialized && localizationInitialized)
  {
    geometry_msgs::Twist modifiedCommand;
    modifiedCommand.linear.x = moveCommand.linear.x;
    modifiedCommand.linear.y = moveCommand.linear.y;
    modifiedCommand.angular.z = moveCommand.angular.z;

    if (currentCost.cost > 0)
    {
      float speedReduction = .000444 * pow(currentCost.cost - 254, 2) + .1;
      if (fabs(moveCommand.linear.x) > .1 || fabs(moveCommand.linear.y) > .1)
      {
        //Determine if robot is moving away from an obstacle
        float alpha = atan2(-moveCommand.linear.x, moveCommand.linear.y) + PI / 2.0 + robotPose.theta;
        while (alpha > PI)
          alpha -= 2 * PI;
        while (alpha < -PI)
          alpha += 2 * PI;

        youbot_overhead_localization::GetCost srv;
        srv.request.x = robotPose.x
            + .2 * sqrt(pow(moveCommand.linear.x, 2) + pow(moveCommand.linear.y, 2)) * cos(alpha);
        srv.request.y = robotPose.y
            + .2 * sqrt(pow(moveCommand.linear.x, 2) + pow(moveCommand.linear.y, 2)) * sin(alpha);
        //ROS_INFO("Current: (%f,%f); Predicted: (%f,%f)", robotPose.x, robotPose.y, srv.request.x, srv.request.y);
        //ROS_INFO("Movement angle: %f; Robot angle: %f", alpha, robotPose.theta);
        if (costClient.call(srv))
        {
          float futureCost = srv.response.cost;
          if (futureCost < currentCost.cost)
          {
            //ROS_INFO("Detected movement away from obstacles");
            speedReduction = .75;
          }
        }
      }

      if (speedReduction < .1)
        speedReduction = .1;
      modifiedCommand.linear.x *= speedReduction;
      modifiedCommand.linear.y *= speedReduction;
      modifiedCommand.angular.z *= speedReduction;

      //ROS_INFO("Speed adjusted by %f for safety", speedReduction);
    }
    else
    {
      //ROS_INFO("Speed adjusted by 1.0 for safety");
    }

    //Checking to see if the the command is a stop command.  If it is

    if (stopCommandFound)
    {
      time_t currentT = time(NULL);
      long timeDiff = currentT - lastStopCmdTime;

      if (timeDiff > timeBetweenStopCmds)
      {
        baseCommandPublisher.publish(modifiedCommand);
        lastStopCmdTime = currentT;
      }
    }
    else
      baseCommandPublisher.publish(modifiedCommand);
  }
}

void keyNavSafety::baseCommandCallback(const geometry_msgs::Twist& requestedCommand)
{
  moveCommand = requestedCommand;
  if (!moveInitialized)
    moveInitialized = true;

  if (moveCommand.linear.x == 0 && moveCommand.linear.y == 0 && moveCommand.angular.z == 0)
  {
    stopCommandFound = true;
  }
  else
  {
    stopCommandFound = false;
    lastStopCmdTime = 0;
  }
}

void keyNavSafety::localizationCallback(const youbot_overhead_localization::Pose2d& pose)
{
  robotPose = pose;
  if (!localizationInitialized)
    localizationInitialized = true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "key_nav_safety");

  keyNavSafety navSafety;

  ros::spin();

  return 0;
}