homer_navigation_node.cpp

#include "homer_navigation/homer_navigation_node.h"

HomerNavigationNode::HomerNavigationNode() {
  ros::NodeHandle nh;

  // subscribers
  m_map_sub = nh.subscribe<nav_msgs::OccupancyGrid>(
      "/map", 1, &HomerNavigationNode::mapCallback, this);
  m_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>(
      "/pose", 1, &HomerNavigationNode::poseCallback, this);
  m_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
      "/scan", 1, &HomerNavigationNode::laserDataCallback, this);
  m_down_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
      "/front_scan", 1, &HomerNavigationNode::downlaserDataCallback, this);
  m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>(
      "/homer_navigation/start_navigation", 1,
      &HomerNavigationNode::startNavigationCallback, this);
  m_move_base_simple_goal_sub = nh.subscribe<geometry_msgs::PoseStamped>(
      "/move_base_simple/goal", 1,
      &HomerNavigationNode::moveBaseSimpleGoalCallback,
      this);  // for RVIZ usage
  m_stop_navigation_sub = nh.subscribe<std_msgs::Empty>(
      "/homer_navigation/stop_navigation", 1,
      &HomerNavigationNode::stopNavigationCallback, this);
  m_navigate_to_poi_sub = nh.subscribe<homer_mapnav_msgs::NavigateToPOI>(
      "/homer_navigation/navigate_to_POI", 1,
      &HomerNavigationNode::navigateToPOICallback, this);
  m_unknown_threshold_sub = nh.subscribe<std_msgs::Int8>(
      "/homer_navigation/unknown_threshold", 1,
      &HomerNavigationNode::unknownThresholdCallback, this);
  m_refresh_param_sub = nh.subscribe<std_msgs::Empty>(
      "/homer_navigation/refresh_params", 1,
      &HomerNavigationNode::refreshParamsCallback, this);
  m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>(
      "/homer_navigation/max_depth_move_distance", 1,
      &HomerNavigationNode::maxDepthMoveDistanceCallback, this);

  m_cmd_vel_pub =
      nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 1);
  m_target_reached_string_pub =
      nh.advertise<std_msgs::String>("/homer_navigation/target_reached", 1);
  // m_target_reached_empty_pub 	=
  // nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 1);
  m_target_unreachable_pub = nh.advertise<homer_mapnav_msgs::TargetUnreachable>(
      "/homer_navigation/target_unreachable", 1);
  m_path_pub = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 1);
  m_ptu_center_world_point_pub = nh.advertise<homer_ptu_msgs::CenterWorldPoint>(
      "/ptu/center_world_point", 1);
  m_set_pan_tilt_pub =
      nh.advertise<homer_ptu_msgs::SetPanTilt>("/ptu/set_pan_tilt", 1);
  m_debug_pub = nh.advertise<std_msgs::String>("/homer_navigation/debug", 1);

  m_get_POIs_client = nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>(
      "/map_manager/get_pois");

  m_MainMachine.setName("HomerNavigation Main");
  ADD_MACHINE_STATE(m_MainMachine, IDLE);
  ADD_MACHINE_STATE(m_MainMachine, AWAITING_PATHPLANNING_MAP);
  ADD_MACHINE_STATE(m_MainMachine, FOLLOWING_PATH);
  ADD_MACHINE_STATE(m_MainMachine, AVOIDING_COLLISION);
  ADD_MACHINE_STATE(m_MainMachine, FINAL_TURN);

  init();
}

void HomerNavigationNode::loadParameters() {
  // Explorer constructor
  ros::param::param("/homer_mapping/resolution", m_resolution, (double)0.05);
  ros::param::param("/homer_navigation/allowed_obstacle_distance/min",
                    m_AllowedObstacleDistance.first, (float)0.3);
  ros::param::param("/homer_navigation/allowed_obstacle_distance/max",
                    m_AllowedObstacleDistance.second, (float)5.0);
  ros::param::param("/homer_navigation/safe_obstacle_distance/min",
                    m_SafeObstacleDistance.first, (float)0.7);
  ros::param::param("/homer_navigation/safe_obstacle_distance/max",
                    m_SafeObstacleDistance.second, (float)1.5);
  ros::param::param("/homer_navigation/frontier_safeness_factor",
                    m_FrontierSafenessFactor, (float)1.4);
  ros::param::param("/homer_navigation/safe_path_weight", m_SafePathWeight,
                    (double)1.2);
  ros::param::param("/homer_navigation/waypoint_sampling_threshold",
                    m_waypoint_sampling_threshold, (float)1.5);
  m_AllowedObstacleDistance.first /= m_resolution;
  m_AllowedObstacleDistance.second /= m_resolution;
  m_SafeObstacleDistance.first /= m_resolution;
  m_SafeObstacleDistance.second /= m_resolution;

  // check path
  ros::param::param("/homer_navigation/check_path", m_check_path, (bool)true);
  ros::param::param("/homer_navigation/check_path_max_distance",
                    m_check_path_max_distance, (float)2.0);

  // collision
  ros::param::param("/homer_navigation/collision_distance",
                    m_collision_distance, (float)0.3);
  ros::param::param("/homer_navigation/collision_distance_near_target",
                    m_collision_distance_near_target, (float)0.2);
  ros::param::param("/homer_navigation/backward_collision_distance",
                    m_backward_collision_distance, (float)0.5);

  // cmd_vel config values
  ros::param::param("/homer_navigation/min_turn_angle", m_min_turn_angle,
                    (float)0.15);
  ros::param::param("/homer_navigation/max_turn_speed", m_max_turn_speed,
                    (float)0.6);
  ros::param::param("/homer_navigation/min_turn_speed", m_min_turn_speed,
                    (float)0.3);
  ros::param::param("/homer_navigation/max_move_speed", m_max_move_speed,
                    (float)0.4);
  ros::param::param("/homer_navigation/max_drive_angle", m_max_drive_angle,
                    (float)0.6);

  // caution factors
  ros::param::param("/homer_navigation/map_speed_factor", m_map_speed_factor,
                    (float)1.0);
  ros::param::param("/homer_navigation/waypoint_speed_factor",
                    m_waypoint_speed_factor, (float)1.0);
  ros::param::param("/homer_navigation/obstacle_speed_factor",
                    m_obstacle_speed_factor, (float)1.0);
  ros::param::param("/homer_navigation/target_distance_speed_factor",
                    m_target_distance_speed_factor, (float)0.4);

  // robot dimensions
  ros::param::param("/homer_navigation/min_x", m_min_x, (float)0.3);
  ros::param::param("/homer_navigation/min_y", m_min_y, (float)0.27);

  // error durations
  ros::param::param("/homer_navigation/callback_error_duration",
                    m_callback_error_duration, (float)0.3);

  ros::param::param("/homer_navigation/use_ptu", m_use_ptu, (bool)false);

  ros::param::param("/homer_navigation/unknown_threshold", m_unknown_threshold,
                    (int)50);
  ros::param::param("/homer_navigation/waypoint_radius_factor",
                    m_waypoint_radius_factor, (float)0.25);
}

void HomerNavigationNode::init() {
  m_max_move_sick = 40.0;
  m_max_move_down = 40.0;
  m_max_move_depth = 40.0;
  m_robot_pose.position.x = 0.0;
  m_robot_pose.position.y = 0.0;
  m_robot_pose.position.z = 0.0;
  m_robot_pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
  m_robot_last_pose = m_robot_pose;
  m_avoided_collision = false;
  m_act_speed = 0;
  m_angular_avoidance = 0;
  m_last_calculations_failed = 0;
  m_last_check_path_res = false;
  m_new_target = true;

  loadParameters();

  m_explorer = new Explorer(
      m_AllowedObstacleDistance.first, m_AllowedObstacleDistance.second,
      m_SafeObstacleDistance.first, m_SafeObstacleDistance.second,
      m_SafePathWeight, m_FrontierSafenessFactor, m_unknown_threshold);
  m_last_map_data = new std::vector<int8_t>(0);

  m_MainMachine.setState(IDLE);
}

HomerNavigationNode::~HomerNavigationNode() {
  if (m_explorer) {
    delete m_explorer;
  }
  if (m_last_map_data) {
    delete m_last_map_data;
  }
}

void HomerNavigationNode::stopRobot() {
  m_act_speed = 0;
  geometry_msgs::Twist cmd_vel_msg;
  cmd_vel_msg.linear.x = 0;
  cmd_vel_msg.linear.y = 0;
  cmd_vel_msg.linear.z = 0;
  cmd_vel_msg.angular.x = 0;
  cmd_vel_msg.angular.y = 0;
  cmd_vel_msg.angular.z = 0;
  m_cmd_vel_pub.publish(cmd_vel_msg);
  ros::Duration(0.1).sleep();
  m_cmd_vel_pub.publish(cmd_vel_msg);
  ros::Duration(0.1).sleep();
  m_cmd_vel_pub.publish(cmd_vel_msg);
}

void HomerNavigationNode::idleProcess() {
  if (m_MainMachine.state() == FOLLOWING_PATH) {
    if ((ros::Time::now() - m_last_laser_time) >
        ros::Duration(m_callback_error_duration)) {
      ROS_ERROR_STREAM("Laser data timeout!\n");
      stopRobot();
    }
    if ((ros::Time::now() - m_last_pose_time) >
        ros::Duration(m_callback_error_duration)) {
      ROS_ERROR_STREAM("Pose timeout!\n");
      stopRobot();
    }
  }
}

void HomerNavigationNode::calculatePath() {
  if (m_distance_to_target < m_desired_distance && !m_new_target) {
    m_path_reaches_target = true;
    return;
  }
  m_explorer->setOccupancyMap(m_width, m_height, m_origin,
                              &(*m_last_map_data)[0]);
  m_explorer->setStart(
      map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));

  bool success;
  m_pixel_path = m_explorer->getPath(success);
  if (!success) {
    ROS_WARN_STREAM("No path found for navigation");
    m_last_calculations_failed++;
    ROS_INFO_STREAM("last_calculation_failed: " << m_last_calculations_failed);
    if (m_last_calculations_failed > 8) {
      sendTargetUnreachableMsg(
          homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
    }
  } else {
    m_last_calculations_failed = 0;
    std::vector<Eigen::Vector2i> waypoint_pixels =
        m_explorer->sampleWaypointsFromPath(m_pixel_path,
                                            m_waypoint_sampling_threshold);
    m_waypoints.clear();
    ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: "
                    << waypoint_pixels.size());
    if (waypoint_pixels.size() > 0) {
      for (std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin();
           it != waypoint_pixels.end(); ++it) {
        geometry_msgs::PoseStamped poseStamped;
        poseStamped.header.frame_id = "/map";
        poseStamped.pose.position =
            map_tools::fromMapCoords(*it, m_origin, m_resolution);
        poseStamped.pose.orientation.x = 0.0;
        poseStamped.pose.orientation.y = 0.0;
        poseStamped.pose.orientation.z = 0.0;
        poseStamped.pose.orientation.w = 1.0;
        m_waypoints.push_back(poseStamped);
      }
      sendPathData();
    } else {
      sendTargetUnreachableMsg(
          homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
    }

    m_last_laser_time = ros::Time::now();
    m_last_pose_time = ros::Time::now();
  }
}

void HomerNavigationNode::startNavigation() {
  if (m_distance_to_target < m_desired_distance && !m_new_target) {
    ROS_INFO_STREAM(
        "Will not (re-)plan path: Target position already reached.");
    m_path_reaches_target = true;
    targetPositionReached();
    return;
  }
  ROS_INFO_STREAM("Distance to target still too large ("
                  << m_distance_to_target
                  << "m; requested: " << m_desired_distance << "m)");

  if (m_fast_path_planning) {
    maskMap();
  }

  m_explorer->setOccupancyMap(m_width, m_height, m_origin,
                              &(*m_last_map_data)[0]);

  // check if there still exists a path to the original target
  if (m_avoided_collision && m_initial_path_reaches_target &&
      m_stop_before_obstacle) {
    m_explorer->setStart(
        map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));

    bool success;
    m_pixel_path = m_explorer->getPath(success);
    if (!success) {
      ROS_INFO_STREAM(
          "Initial path would have reached target, new path does not. "
          << "Sending target unreachable.");
      sendTargetUnreachableMsg(
          homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
      return;
    }
  }

  m_explorer->setStart(
      map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
  Eigen::Vector2i new_target_approx = m_explorer->getNearestAccessibleTarget(
      map_tools::toMapCoords(m_target_point, m_origin, m_resolution));

  geometry_msgs::Point new_target_approx_world =
      map_tools::fromMapCoords(new_target_approx, m_origin, m_resolution);
  ROS_INFO_STREAM(
      "start Navigation: Approx target: " << new_target_approx_world);

  m_path_reaches_target =
      (map_tools::distance(new_target_approx_world, m_target_point) <
       m_desired_distance);
  m_initial_path_reaches_target = m_path_reaches_target;

  bool new_approx_is_better =
      (map_tools::distance(m_robot_pose.position, m_target_point) -
       map_tools::distance(new_target_approx_world, m_target_point)) >
      2 * m_desired_distance;
  if (!new_approx_is_better && !m_path_reaches_target) {
    ROS_WARN_STREAM(
        "No better way to target found, turning and then reporting as "
        "unreachable."
        << std::endl
        << "Distance to target: " << m_distance_to_target
        << "m; requested: " << m_desired_distance << "m");
    m_MainMachine.setState(FINAL_TURN);
  } else {
    m_explorer->setTarget(new_target_approx);
    m_MainMachine.setState(FOLLOWING_PATH);
    calculatePath();
  }
}

void HomerNavigationNode::sendPathData() {
  nav_msgs::Path msg;
  msg.header.frame_id = "/map";
  msg.header.stamp = ros::Time::now();
  if (m_waypoints.size() > 0) {
    msg.poses = m_waypoints;
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.pose = m_robot_pose;
    pose_stamped.header.frame_id = "/map";
    pose_stamped.header.stamp = ros::Time::now();
    msg.poses.insert(msg.poses.begin(), pose_stamped);
  }
  m_path_pub.publish(msg);
}

void HomerNavigationNode::sendTargetReachedMsg() {
  stopRobot();
  m_MainMachine.setState(IDLE);
  std_msgs::String reached_string_msg;
  reached_string_msg.data = m_target_name;
  m_target_reached_string_pub.publish(reached_string_msg);
  m_waypoints.clear();
  nav_msgs::Path empty_path_msg;
  empty_path_msg.poses = m_waypoints;
  m_path_pub.publish(empty_path_msg);
  ROS_INFO_STREAM("=== Reached Target " << m_target_name << " ===");
}

void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason) {
  stopRobot();
  m_MainMachine.setState(IDLE);
  homer_mapnav_msgs::TargetUnreachable unreachable_msg;
  unreachable_msg.reason = reason;
  m_target_unreachable_pub.publish(unreachable_msg);
  m_waypoints.clear();
  nav_msgs::Path empty_path_msg;
  empty_path_msg.poses = m_waypoints;
  m_path_pub.publish(empty_path_msg);
  ROS_INFO_STREAM("=== TargetUnreachableMsg ===");
}

void HomerNavigationNode::targetPositionReached() {
  ROS_INFO_STREAM("Target position reached. Distance to target: "
                  << m_distance_to_target
                  << "m. Desired distance:" << m_desired_distance << "m");
  stopRobot();
  m_waypoints.clear();
  sendPathData();
  m_MainMachine.setState(FINAL_TURN);
  ROS_INFO_STREAM("Turning to look-at point");
}

bool HomerNavigationNode::checkPath() {
  if (m_pixel_path.size() != 0) {
    for (unsigned i = 0; i < m_pixel_path.size() - 1; i++) {
      geometry_msgs::Point p =
          map_tools::fromMapCoords(m_pixel_path.at(i), m_origin, m_resolution);
      if (map_tools::distance(m_robot_pose.position, p) >
          m_check_path_max_distance) {
        return true;
      }
      for (int a = 0; a < 5; a++) {
        if (map_tools::findValue(
                m_last_map_data, m_width, m_height,
                m_pixel_path[i].x() +
                    (m_pixel_path[i + 1].x() - m_pixel_path[i].x()) * a / 4,
                m_pixel_path[i].y() +
                    (m_pixel_path[i + 1].y() - m_pixel_path[i].y()) * a / 4,
                90, 2)) {
          ROS_WARN_STREAM("Obstacle detected in current path recalculating");
          return false;
        }
      }
    }
  }
}

void HomerNavigationNode::handleCollision() {
  if (m_MainMachine.state() == FOLLOWING_PATH) {
    stopRobot();
    m_MainMachine.setState(AVOIDING_COLLISION);
    ROS_WARN_STREAM("Collision detected while following path!");
  }
}

void HomerNavigationNode::performNextMove() {
  if (m_MainMachine.state() == FOLLOWING_PATH) {
    if (m_distance_to_target < m_desired_distance && !m_new_target) {
      ROS_INFO_STREAM("Desired distance to target was reached.");
      targetPositionReached();
      return;
    }
    if (m_waypoints.size() == 0) {
      ROS_WARN_STREAM(
          "No waypoints but trying to perform next move! Skipping.");
      return;
    }
    // if we have accidentaly skipped waypoints, recalculate path
    float minDistance = FLT_MAX;
    float distance;
    unsigned nearestWaypoint = 0;
    for (unsigned i = 0; i < m_waypoints.size(); i++) {
      distance = map_tools::distance(m_robot_pose.position,
                                     m_waypoints[i].pose.position);
      if (distance < minDistance) {
        nearestWaypoint = i;
        minDistance = distance;
      }
    }
    if (nearestWaypoint != 0) {
      // if the target is nearer than the waypoint don't recalculate
      if (m_waypoints.size() != 2) {
        ROS_WARN_STREAM("Waypoints skipped. Recalculating path!");
        calculatePath();
        if (m_MainMachine.state() != FOLLOWING_PATH) {
          return;
        }
      } else {
        m_waypoints.erase(m_waypoints.begin());
      }
    }

    Eigen::Vector2i waypointPixel = map_tools::toMapCoords(
        m_waypoints[0].pose.position, m_origin, m_resolution);
    float obstacleDistanceMap = m_explorer->getObstacleTransform()->getValue(
                                    waypointPixel.x(), waypointPixel.y()) *
                                m_resolution;
    float waypointRadius = m_waypoint_radius_factor * obstacleDistanceMap;

    if ((waypointRadius < m_resolution) || (m_waypoints.size() == 1)) {
      waypointRadius = m_resolution;
    }

    if (m_waypoints.size() != 0) {
      // calculate distance between last_pose->current_pose and waypoint
      Eigen::Vector2f line;  // direction of the line
      line[0] = m_robot_pose.position.x - m_robot_last_pose.position.x;
      line[1] = m_robot_pose.position.y - m_robot_last_pose.position.y;
      Eigen::Vector2f
          point_to_start;  // vector from the point to the start of the line
      point_to_start[0] =
          m_robot_last_pose.position.x - m_waypoints[0].pose.position.x;
      point_to_start[1] =
          m_robot_last_pose.position.y - m_waypoints[0].pose.position.y;
      float dot =
          point_to_start[0] * line[0] +
          point_to_start[1] * line[1];  // dot product of point_to_start * line
      Eigen::Vector2f distance;  // shortest distance vector from point to line
      distance[0] = point_to_start[0] - dot * line[0];
      distance[1] = point_to_start[1] - dot * line[1];
      float distance_to_waypoint =
          sqrt((double)(distance[0] * distance[0] + distance[1] * distance[1]));

      // check if current waypoint has been reached
      if ((distance_to_waypoint < waypointRadius && m_waypoints.size() > 1) ||
          (m_distance_to_target < waypointRadius)) {
        m_waypoints.erase(m_waypoints.begin());
      }
    }

    sendPathData();
    // last wayoint reached
    if (m_waypoints.size() == 0) {
      ROS_INFO_STREAM("Last waypoint reached");
      currentPathFinished();
      return;
    }

    if (m_use_ptu) {
      ROS_INFO_STREAM("Moving PTU to center next Waypoint");
      homer_ptu_msgs::CenterWorldPoint ptu_msg;
      ptu_msg.point.x = m_waypoints[0].pose.position.x;
      ptu_msg.point.y = m_waypoints[0].pose.position.y;
      ptu_msg.point.z = 0;
      ptu_msg.permanent = true;
      m_ptu_center_world_point_pub.publish(ptu_msg);
    }

    float distanceToWaypoint = map_tools::distance(
        m_robot_pose.position, m_waypoints[0].pose.position);
    float angleToWaypoint = angleToPointDeg(m_waypoints[0].pose.position);
    if (angleToWaypoint < -180) {
      angleToWaypoint += 360;
    }
    float angle = deg2Rad(angleToWaypoint);

    // linear speed calculation
    if (m_avoided_collision) {
      if (std::abs(angleToWaypoint) < 10) {
        m_avoided_collision = false;
      }
    } else if (abs(angle) > m_max_drive_angle) {
      m_act_speed = 0.0;
    } else {
      float obstacleMapDistance = 1;
      for (int wpi = -1; wpi < std::min((int)m_waypoints.size(), (int)2);
           wpi++) {
        Eigen::Vector2i robotPixel;
        if (wpi == -1) {
          robotPixel = map_tools::toMapCoords(m_robot_pose.position, m_origin,
                                              m_resolution);
        } else {
          robotPixel = map_tools::toMapCoords(m_waypoints[wpi].pose.position,
                                              m_origin, m_resolution);
        }
        obstacleMapDistance =
            std::min((float)obstacleMapDistance,
                     (float)(m_explorer->getObstacleTransform()->getValue(
                                 robotPixel.x(), robotPixel.y()) *
                             m_resolution));
        if (obstacleMapDistance <= 0.00001) {
          ROS_ERROR_STREAM(
              "obstacleMapDistance is below threshold to 0 setting to 1");
          obstacleMapDistance = 1;
        }
      }

      float max_move_distance_speed =
          m_max_move_speed * m_max_move_distance * m_obstacle_speed_factor;
      float max_map_distance_speed =
          m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
      m_act_speed = std::min(
          std::max((float)0.1,
                   m_distance_to_target * m_target_distance_speed_factor),
          std::min(std::min(m_max_move_speed, max_move_distance_speed),
                   std::min(max_map_distance_speed,
                            distanceToWaypoint * m_waypoint_speed_factor)));
      std_msgs::String tmp;
      std::stringstream str;
      str << "m_obstacle_speed " << max_move_distance_speed
          << " max_map_distance_speed " << max_map_distance_speed;
      tmp.data = str.str();
      m_debug_pub.publish(tmp);
    }

    // angular speed calculation
    if (angle < 0) {
      angle = std::max(angle * (float)0.8, -m_max_turn_speed);
      m_act_speed = m_act_speed + angle / 2.0;
      if (m_act_speed < 0) {
        m_act_speed = 0;
      }
    } else {
      angle = std::min(angle * (float)0.8, m_max_turn_speed);
      m_act_speed = m_act_speed - angle / 2.0;
      if (m_act_speed < 0) {
        m_act_speed = 0;
      }
    }
    geometry_msgs::Twist cmd_vel_msg;
    cmd_vel_msg.linear.x = m_act_speed;
    cmd_vel_msg.angular.z = angle;
    m_cmd_vel_pub.publish(cmd_vel_msg);

    ROS_DEBUG_STREAM("Driving & turning"
                     << std::endl
                     << "linear: " << m_act_speed << " angular: " << angle
                     << std::endl
                     << "distanceToWaypoint:" << distanceToWaypoint
                     << "angleToWaypoint: " << angleToWaypoint << std::endl);
  } else if (m_MainMachine.state() == AVOIDING_COLLISION) {
    if (m_distance_to_target < m_desired_distance && !m_new_target) {
      ROS_INFO_STREAM("Collision detected near target. Switch to final turn.");
      targetPositionReached();
    } else if (m_max_move_distance <= m_collision_distance &&
                   m_waypoints.size() > 1 ||
               m_max_move_distance <= m_collision_distance_near_target) {
      ROS_WARN_STREAM("Maximum driving distance too short ("
                      << m_max_move_distance << "m)! Moving back.");
      geometry_msgs::Twist cmd_vel_msg;
      if (!HomerNavigationNode::backwardObstacle()) {
        cmd_vel_msg.linear.x = -0.2;
      } else {
        if (m_angular_avoidance == 0) {
          float angleToWaypoint = angleToPointDeg(m_waypoints[0].pose.position);
          if (angleToWaypoint < -180) {
            angleToWaypoint += 360;
          }
          if (angleToWaypoint < 0) {
            m_angular_avoidance = -0.4;
          } else {
            m_angular_avoidance = 0.4;
          }
        }
        cmd_vel_msg.angular.z = m_angular_avoidance;
      }
      m_cmd_vel_pub.publish(cmd_vel_msg);
    } else {
      m_angular_avoidance = 0;
      m_avoided_collision = true;
      ROS_WARN_STREAM("Collision avoided. Updating path.");
      currentPathFinished();
    }
  } else if (m_MainMachine.state() == FINAL_TURN) {
    if (m_use_ptu) {
      // reset PTU
      homer_ptu_msgs::SetPanTilt msg;
      msg.absolute = true;
      msg.panAngle = 0;
      msg.tiltAngle = 0;
      m_set_pan_tilt_pub.publish(msg);
    }

    if (m_skip_final_turn) {
      ROS_INFO_STREAM("Final turn skipped. Target reached.");
      if (m_path_reaches_target) {
        sendTargetReachedMsg();
      } else {
        sendTargetUnreachableMsg(
            homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
      }
      return;
    }

    float turnAngle = minTurnAngle(tf::getYaw(m_robot_pose.orientation),
                                   m_target_orientation);
    ROS_DEBUG_STREAM(
        "homer_navigation::PerformNextMove:: Final Turn. Robot orientation: "
        << rad2Deg(tf::getYaw(m_robot_pose.orientation))
        << ". Target orientation: " << rad2Deg(m_target_orientation)
        << "homer_navigation::PerformNextMove:: turnAngle: "
        << rad2Deg(turnAngle));

    if (std::fabs(turnAngle) < m_min_turn_angle) {
      ROS_INFO_STREAM(":::::::NEAREST WALKABLE TARGET REACHED BECAUSE lower "
                      << m_min_turn_angle);
      ROS_INFO_STREAM("target angle = " << m_target_orientation);
      ROS_INFO_STREAM("is angle = " << tf::getYaw(m_robot_pose.orientation));
      ROS_INFO_STREAM("m_distance_to_target = " << m_distance_to_target);
      ROS_INFO_STREAM("m_desired_distance = " << m_desired_distance);
      if (m_path_reaches_target) {
        sendTargetReachedMsg();
      } else {
        sendTargetUnreachableMsg(
            homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
      }
      return;
    } else {
      if (turnAngle < 0) {
        turnAngle =
            std::max(std::min(turnAngle, -m_min_turn_speed), -m_max_turn_speed);
      } else {
        turnAngle =
            std::min(std::max(turnAngle, m_min_turn_speed), m_max_turn_speed);
      }
      geometry_msgs::Twist cmd_vel_msg;
      cmd_vel_msg.angular.z = turnAngle;
      m_cmd_vel_pub.publish(cmd_vel_msg);
    }
  }
}

void HomerNavigationNode::currentPathFinished() {
  ROS_INFO_STREAM("Current path was finished, initiating recalculation.");
  m_waypoints.clear();
  stopRobot();
  m_MainMachine.setState(AWAITING_PATHPLANNING_MAP);
}

int HomerNavigationNode::angleToPointDeg(geometry_msgs::Point target) {
  double cx = m_robot_pose.position.x;
  double cy = m_robot_pose.position.y;
  int targetAngle = rad2Deg(atan2(target.y - cy, target.x - cx));
  int currentAngle = rad2Deg(tf::getYaw(m_robot_pose.orientation));

  int angleDiff = targetAngle - currentAngle;
  angleDiff = (angleDiff + 180) % 360 - 180;
  return angleDiff;
}

bool HomerNavigationNode::drawPolygon(
    std::vector<geometry_msgs::Point> vertices) {
  if (vertices.size() == 0) {
    ROS_INFO_STREAM("No vertices given!");
    return false;
  }
  // make temp. map
  std::vector<int> data(m_width * m_height);
  for (int i = 0; i < data.size(); i++) {
    data[i] = 0;
  }

  // draw the lines surrounding the polygon
  for (unsigned int i = 0; i < vertices.size(); i++) {
    int i2 = (i + 1) % vertices.size();
    drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y,
             30);
  }
  // calculate a point in the middle of the polygon
  float midX = 0;
  float midY = 0;
  for (unsigned int i = 0; i < vertices.size(); i++) {
    midX += vertices[i].x;
    midY += vertices[i].y;
  }
  midX /= vertices.size();
  midY /= vertices.size();
  // fill polygon
  return fillPolygon(data, (int)midX, (int)midY, 30);
}

void HomerNavigationNode::drawLine(std::vector<int>& data, int startX,
                                   int startY, int endX, int endY, int value) {
  // bresenham algorithm
  int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
  // compute distances
  dx = endX - startX;
  dy = endY - startY;

  // compute increment
  if (dx < 0) {
    incx = -1;
    dx = -dx;
  } else {
    incx = dx ? 1 : 0;
  }

  if (dy < 0) {
    incy = -1;
    dy = -dy;
  } else {
    incy = dy ? 1 : 0;
  }

  // which distance is greater?
  dist = (dx > dy) ? dx : dy;
  // initializing
  x = startX;
  y = startY;
  xerr = dx;
  yerr = dy;

  // compute cells
  for (t = 0; t < dist; t++) {
    int index = x + m_width * y;
    if (index < 0 || index > data.size()) {
      continue;
    }
    data[index] = value;

    xerr += dx;
    yerr += dy;
    if (xerr > dist) {
      xerr -= dist;
      x += incx;
    }
    if (yerr > dist) {
      yerr -= dist;
      y += incy;
    }
  }
}

bool HomerNavigationNode::fillPolygon(std::vector<int>& data, int x, int y,
                                      int value) {
  int index = x + m_width * y;
  bool tmp = false;

  if ((int)m_last_map_data->at(index) > 90) {
    tmp = true;
  }
  if (data[index] != value) {
    data[index] = value;
    if (fillPolygon(data, x + 1, y, value)) {
      tmp = true;
    }
    if (fillPolygon(data, x - 1, y, value)) {
      tmp = true;
    }
    if (fillPolygon(data, x, y + 1, value)) {
      tmp = true;
    }
    if (fillPolygon(data, x, y - 1, value)) {
      tmp = true;
    }
  }
  return tmp;
}

bool HomerNavigationNode::backwardObstacle() {
  std::vector<geometry_msgs::Point> vertices;
  geometry_msgs::Point base_link_point;
  geometry_msgs::Point map_point;
  Eigen::Vector2i map_coord;

  std::vector<float> x;
  std::vector<float> y;

  x.push_back(-m_min_x - m_backward_collision_distance);
  y.push_back(m_min_y);

  x.push_back(-m_min_x - m_backward_collision_distance);
  y.push_back(-m_min_y);

  x.push_back(-0.1);
  y.push_back(-m_min_y);

  x.push_back(-0.1);
  y.push_back(m_min_y);

  for (int i = 0; i < x.size(); i++) {
    base_link_point.x = x[i];
    base_link_point.y = y[i];
    map_coord = map_tools::toMapCoords(
        map_tools::transformPoint(base_link_point, m_transform_listener,
                                  "/base_link", "/map"),
        m_origin, m_resolution);
    map_point.x = map_coord.x();
    map_point.y = map_coord.y();
    vertices.push_back(map_point);
  }

  return drawPolygon(vertices);
}

void HomerNavigationNode::maskMap() {
  // generate bounding box
  ROS_INFO_STREAM("Calculating Bounding box for fast planning");
  Eigen::Vector2i pose_pixel =
      map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution);
  Eigen::Vector2i target_pixel =
      map_tools::toMapCoords(m_target_point, m_origin, m_resolution);
  Eigen::Vector2i safe_pixel_distance(m_AllowedObstacleDistance.first * 4,
                                      m_AllowedObstacleDistance.first * 4);
  Eigen::AlignedBox2i planning_box;
  planning_box.extend(pose_pixel);
  planning_box.extend(target_pixel);
  ROS_INFO_STREAM("Bounding Box: (" << planning_box.min() << " "
                                    << planning_box.max());
  Eigen::AlignedBox2i safe_planning_box(
      planning_box.min() - safe_pixel_distance,
      planning_box.max() + safe_pixel_distance);
  ROS_INFO_STREAM("safe Bounding Box: (" << safe_planning_box.min() << " "
                                         << safe_planning_box.max());
  ROS_INFO_STREAM("min in m: " << map_tools::fromMapCoords(
                      safe_planning_box.min(), m_origin, m_resolution));
  ROS_INFO_STREAM("max in m: " << map_tools::fromMapCoords(
                      safe_planning_box.max(), m_origin, m_resolution));
  for (size_t x = 0; x < m_width; x++) {
    for (size_t y = 0; y < m_width; y++) {
      if (!safe_planning_box.contains(Eigen::Vector2i(x, y))) {
        m_last_map_data->at(y * m_width + x) = -1;
      }
    }
  }
}

// convenience math functions
float HomerNavigationNode::minTurnAngle(float angle1, float angle2) {
  angle1 *= 180.0 / M_PI;
  angle2 *= 180.0 / M_PI;

  int diff = angle2 - angle1;
  diff = (diff + 180) % 360 - 180;
  if (diff < -180) {
    diff += 360;
  }

  float ret = static_cast<double>(diff) * M_PI / 180.0;
  return ret;
}

void HomerNavigationNode::refreshParamsCallback(
    const std_msgs::Empty::ConstPtr& msg) {
  ROS_INFO_STREAM("Refreshing Parameters");
  loadParameters();
}

void HomerNavigationNode::mapCallback(
    const nav_msgs::OccupancyGrid::ConstPtr& msg) {
  if (m_last_map_data) {
    delete m_last_map_data;
  }

  m_last_map_data = new std::vector<int8_t>(msg->data);
  m_origin = msg->info.origin;
  m_width = msg->info.width;
  m_height = msg->info.height;
  m_resolution = msg->info.resolution;

  switch (m_MainMachine.state()) {
    case AWAITING_PATHPLANNING_MAP:
      startNavigation();
      break;
    case FOLLOWING_PATH: {
      if (m_check_path) {
        if (!checkPath()) {
          if (!m_last_check_path_res) {
            calculatePath();
          }
          m_last_check_path_res = false;
        } else {
          m_last_check_path_res = true;
        }
      }
      break;
    }
  }
}

void HomerNavigationNode::poseCallback(
    const geometry_msgs::PoseStamped::ConstPtr& msg) {
  m_robot_last_pose = m_robot_pose;
  m_robot_pose = msg->pose;
  m_last_pose_time = ros::Time::now();
  m_distance_to_target =
      map_tools::distance(m_robot_pose.position, m_target_point);
  m_new_target = false;
  performNextMove();
}

void HomerNavigationNode::calcMaxMoveDist() {
  m_max_move_distance =
      std::min(m_max_move_sick, std::min(m_max_move_down, m_max_move_depth));
  if (m_max_move_distance <= m_collision_distance &&
          std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1 ||
      m_max_move_distance <= m_collision_distance_near_target &&
          std::fabs(m_act_speed) > 0.1 && m_waypoints.size() == 1 ||
      m_max_move_distance <= 0.1) {
    handleCollision();
  }
}
void HomerNavigationNode::maxDepthMoveDistanceCallback(
    const std_msgs::Float32::ConstPtr& msg) {
  m_max_move_depth = msg->data;
  calcMaxMoveDist();
}

void HomerNavigationNode::laserDataCallback(
    const sensor_msgs::LaserScan::ConstPtr& msg) {
  m_last_laser_time = ros::Time::now();
  m_max_move_sick = map_tools::get_max_move_distance(
      map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min,
                                        msg->angle_increment, msg->range_min,
                                        msg->range_max, m_transform_listener,
                                        msg->header.frame_id, "/base_link"),
      m_min_x, m_min_y);
  calcMaxMoveDist();
}

void HomerNavigationNode::downlaserDataCallback(
    const sensor_msgs::LaserScan::ConstPtr& msg) {
  m_max_move_down = map_tools::get_max_move_distance(
      map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min,
                                        msg->angle_increment, msg->range_min,
                                        msg->range_max, m_transform_listener,
                                        msg->header.frame_id, "/base_link"),
      m_min_x, m_min_y);
  calcMaxMoveDist();
}

void HomerNavigationNode::startNavigationCallback(
    const homer_mapnav_msgs::StartNavigation::ConstPtr& msg) {
  ROS_INFO_STREAM("This is MY node.");
  m_avoided_collision = false;
  m_target_point = msg->goal.position;
  m_target_orientation = tf::getYaw(msg->goal.orientation);
  m_desired_distance =
      msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
  m_skip_final_turn = msg->skip_final_turn;
  m_fast_path_planning = msg->fast_planning;
  m_new_target = true;
  m_target_name = "";
  m_initial_path_reaches_target = false;

  ROS_INFO_STREAM("Navigating to target "
                  << m_target_point.x << ", " << m_target_point.y
                  << "\nTarget orientation: " << m_target_orientation
                  << "Desired distance to target: " << m_desired_distance);

  m_MainMachine.setState(AWAITING_PATHPLANNING_MAP);
}