homer_navigation_node.cpp

#include <vector>
#include <iostream>
#include <sstream>
#include <cmath>

#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#include <ros/package.h>

#include "homer_navigation/homer_navigation_node.h"

//messages
#include <std_msgs/Empty.h>
#include <homer_mapnav_msgs/TargetUnreachable.h>
#include <homer_mapnav_msgs/GetPointsOfInterest.h>
#include <homer_mapnav_msgs/ModifyMap.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int8.h>
#include <tools/tools.h>
#include <tools/loadRosConfig.h>

//nav_libs
#include "Explorer/Explorer.h"
#include "SpeedControl/SpeedControl.h"

using namespace std;


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

	srand(time(NULL));// Initialize random numbers

    //subscribers
    map_sub_ 				= nh.subscribe<nav_msgs::OccupancyGrid>("/map", 1, &HomerNavigationNode::mapCallback, this);
    pose_sub_				= nh.subscribe<geometry_msgs::PoseStamped>("/pose", 1, &HomerNavigationNode::poseCallback, this);
    laser_data_sub_			= nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &HomerNavigationNode::laserDataCallback, this);
    laser_back_data_sub_    = nh.subscribe<sensor_msgs::LaserScan>("/back_scan", 1, &HomerNavigationNode::backLaserCallback, this);
    start_navigation_sub_ 	= nh.subscribe<homer_mapnav_msgs::StartNavigation>("/homer_navigation/start_navigation", 1, &HomerNavigationNode::startNavigationCallback, this);
    stop_navigation_sub_ 	= nh.subscribe<homer_mapnav_msgs::StopNavigation>("/homer_navigation/stop_navigation", 1, &HomerNavigationNode::stopNavigationCallback, this);
    navigate_to_poi_sub_	= nh.subscribe<homer_mapnav_msgs::NavigateToPOI>("/homer_navigation/navigate_to_POI", 1, &HomerNavigationNode::navigateToPOICallback, this);
    unknown_threshold_sub_ 	= nh.subscribe<std_msgs::Int8>("/homer_navigation/unknown_threshold", 1, &HomerNavigationNode::unknownThresholdCallback, this);
    refresh_param_sub_ 		= nh.subscribe<std_msgs::Empty>("/homer_navigation/refresh_params", 1, &HomerNavigationNode::refreshParamsCallback, this);

	cmd_vel_pub_			= nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 1);
    target_reached_pub_ 	= nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 1);
    target_unreachable_pub_ = nh.advertise<homer_mapnav_msgs::TargetUnreachable>("/homer_navigation/target_unreachable", 1);
    path_pub_ 				= nh.advertise<nav_msgs::Path>("/homer_navigation/path", 1);
    get_POIs_client_ 		= nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>("/map_manager/get_pois");


	m_move_base_simple_goal_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &HomerNavigationNode::moveBaseSimpleGoalCallback, this); // for RVIZ usage



    m_MapTypeMachine.setName( "HomerNavigation Node" );
	ADD_MACHINE_STATE( m_MapTypeMachine, SLAM_MAP );
	ADD_MACHINE_STATE( m_MapTypeMachine, NAVIGATION_MAP );
	m_MapTypeMachine.setState( SLAM_MAP );

    m_MainMachine.setName( "HomerNavigation Main" );
	ADD_MACHINE_STATE( m_MainMachine, IDLE );
	ADD_MACHINE_STATE( m_MainMachine, AWAITING_EXPLORATION_MAP );
	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 );
    ADD_MACHINE_STATE( m_MainMachine, TARGET_REACHED );
	
	m_avoided_collision = false; 
	 
    init();
    m_act_speed = 0;
    m_act_angle = 0; 
}

void HomerNavigationNode::loadParameters()
{
    SpeedControl::loadDimensions();
    
	//Explorer constructor 
    loadConfigValue("/homer_navigation/safe_path_weight", 				m_SafePathWeight);
    loadConfigValue("/homer_mapping/resolution", 						resolution_);
    loadConfigValue("/homer_navigation/allowed_obstacle_distance/min", 	m_AllowedObstacleDistance.first);
    loadConfigValue("/homer_navigation/allowed_obstacle_distance/max", 	m_AllowedObstacleDistance.second);
    m_AllowedObstacleDistance.first /= resolution_;
    m_AllowedObstacleDistance.second/= resolution_;
    loadConfigValue("/homer_navigation/safe_obstacle_distance/min", 	m_SafeObstacleDistance.first);
    loadConfigValue("/homer_navigation/safe_obstacle_distance/max", 	m_SafeObstacleDistance.second);
    m_SafeObstacleDistance.first /= resolution_;
    m_SafeObstacleDistance.second /= resolution_;
    loadConfigValue("/homer_navigation/frontier_safeness_factor", 		m_FrontierSafenessFactor);
	
	//Explorer     
    loadConfigValue("/homer_navigation/waypoint_sampling_threshold", 	waypoint_sampling_threshold_);    
    
    //check path 
    loadConfigValue("/homer_navigation/check_path", 					check_path_);
    loadConfigValue("/homer_navigation/check_path_max_errors", 			check_path_max_errors_);
    loadConfigValue("/homer_navigation/check_path_max_distance", 		check_path_max_distance_);
    
    //collision 
    loadConfigValue("/homer_navigation/collision_distance", 			collision_distance_);
    loadConfigValue("/homer_navigation/collision_distance_near_target", collision_distance_near_target_);

	//moveRobot config values
	loadConfigValue("/homer_navigation/backward_distance", 							 backward_distance_);
	loadConfigValue("/homer_navigation/max_trans_vel", 								 m_MaxTransVel);
    loadConfigValue("/homer_navigation/max_rot_vel", 								 m_MaxRotVel);
    loadConfigValue("/homer_navigation/speed_control/last_speedfactor_count", 		 m_SpeedFactorMeanFilterSize);
    loadConfigValue("/homer_navigation/speed_control/min_move_speedfactor", 		 m_MinMoveSpeedFactor);
    loadConfigValue("/homer_navigation/speed_control/min_turn_speedfactor_moving", 	 m_MinTurnSpeedFactorMoving);
    loadConfigValue("/homer_navigation/speed_control/min_turn_speedfactor_standing", m_MinTurnSpeedFactorStanding);
    loadConfigValue("/homer_navigation/turn_threshold_angle", 						 turn_threshold_angle_);

	//cmd_vel config values
    loadConfigValue("/homer_navigation/use_cmd_vel", 					m_use_cmd_vel_);
    loadConfigValue("/homer_navigation/min_turn_angle", 				m_min_turn_angle);
    loadConfigValue("/homer_navigation/max_turn_speed", 				m_max_turn_speed);
    loadConfigValue("/homer_navigation/max_move_speed", 				m_max_move_speed);
    loadConfigValue("/homer_navigation/max_drive_angle",				m_max_drive_angle);

}


void HomerNavigationNode::init()
{
    last_map_timestamp_ = ros::Time(0);

	loadParameters();

    robot_pose_.position.x = 0.0;
    robot_pose_.position.y = 0.0;
    robot_pose_.position.z = 0.0;
    robot_pose_.orientation = tf::createQuaternionMsgFromYaw(0.0);

    last_laser_time_ = ros::Time::now();

    explorer_ = new Explorer ( m_AllowedObstacleDistance.first, m_AllowedObstacleDistance.second,
								m_SafeObstacleDistance.first, m_SafeObstacleDistance.second,
								m_SafePathWeight, m_FrontierSafenessFactor );

    m_MainMachine.setState ( IDLE );
}

HomerNavigationNode::~HomerNavigationNode()
{
    if ( explorer_ )
	{
        delete explorer_;
	}
    if(last_map_data_)
    {
        delete last_map_data_;
    }
}

void HomerNavigationNode::sendStopRobot()
{
    m_act_speed = 0;
    m_act_angle = 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;
	cmd_vel_pub_.publish(cmd_vel_msg);
}

void HomerNavigationNode::idleProcess()
{
	if ( m_MainMachine.state() == FOLLOWING_PATH )
	{
	    if ( (ros::Time::now() - last_laser_time_) > ros::Duration(2.0) )
	    {
	        ROS_ERROR_STREAM( "Laser data timeout!\n");
			sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::LASER_TIMEOUT);
		}
	}
}


void HomerNavigationNode::calculatePath()
{
    float desired_distance_2 = desired_distance_ - 0.05;
    if ( desired_distance_2 < resolution_ )
	{
      desired_distance_2 = resolution_;
	}
    explorer_->setStart ( map_tools::toMapCoords( robot_pose_.position,  origin_, resolution_) );

    if ( map_tools::distance( map_tools::fromMapCoords( target_approx_, origin_, resolution_ ), target_point_ ) <= desired_distance_ )
	{
      ROS_INFO_STREAM( "There is a way to the target circle. Planning direct path." );
      explorer_->setTarget ( map_tools::toMapCoords( target_point_, origin_, resolution_ ), desired_distance_2 / resolution_ );
	}
	else
	{
      ROS_INFO_STREAM( "The target area is not reachable. Trying to go as close as possible." );
      explorer_->setTarget ( target_approx_, 0 );
	}

	bool success;
    pixel_path_ = explorer_->getPath( success );
    if ( !success )
	{
        ROS_WARN_STREAM("No path found for navigation, reporting as unreachable." );
        sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
	}
	else
	{
		ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << pixel_path_.size());
		std::vector<Eigen::Vector2i> waypoint_pixels = explorer_->sampleWaypointsFromPath ( pixel_path_, waypoint_sampling_threshold_ );
		waypoints_.clear();
		for(std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin(); it != waypoint_pixels.end(); ++it)
		{
		    geometry_msgs::PoseStamped poseStamped;
		    poseStamped.pose.position = map_tools::fromMapCoords(*it, origin_, 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;
		    waypoints_.push_back(poseStamped);
		}

		ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << waypoints_.size());

		sendPathData();
		//make sure no timeout occurs after long calculation time
		last_laser_time_ = ros::Time::now();
	}
}


void HomerNavigationNode::startExploration()
{
    ROS_INFO_STREAM( "Starting exploration." );
/*
	m_Explorer->resetExploration();
	m_Explorer->setOccupancyMap ( gridSize, gridSize, mapData, m_ExploredRegion );
	m_Explorer->setFrontierSafenessFactor( m_FrontierSafenessFactor );
	m_Explorer->setStart ( m_CoordinateConverter->worldToMap ( m_RobotPose ) );

	bool success;
	m_PixelPath= m_Explorer->getExplorationTransformPath( success );

	if ( success ) {
		m_Target = m_CoordinateConverter->mapToWorld( m_PixelPath.back() );
		m_TargetApprox = m_PixelPath.back();
	}

	vector<Pixel> waypointPixels = m_Explorer->sampleWaypointsFromPath ( m_PixelPath, m_WaypointSamplingThreshold );
	m_Waypoints = m_CoordinateConverter->mapToWorld ( waypointPixels );
*/

    if ( pixel_path_.size() == 0 )
	{
        ROS_WARN_STREAM("No path found for exploration, sending NoTargetM." );
        //sendMessage ( new NoTargetM() );
		m_MainMachine.setState( IDLE );
	}
	else
	{
		m_MainMachine.setState ( FOLLOWING_PATH );
		sendPathData();
	}
  //make sure no timeout occurs after long calculation time
    last_laser_time_ = ros::Time::now();
}


void HomerNavigationNode::startNavigation()
{
    ROS_INFO_STREAM("Starting navigation to " << target_point_.x << "," << target_point_.y);

    if ( distanceTo(target_point_) < desired_distance_ )
	{
        ROS_INFO_STREAM( "Will not (re-)plan path: Target position already reached." );
		targetPositionReached();
		return;
	}
    ROS_INFO_STREAM( "Distance to target still too large (" << distanceTo( target_point_ ) << "m; requested: " << desired_distance_ << "m)" );

    ROS_DEBUG_STREAM("homer_navigation_node::startNavigation: target point " << target_point_.x << " " << target_point_.y);
    Eigen::Vector2i new_target = map_tools::toMapCoords(target_point_, origin_, resolution_);
    ROS_DEBUG_STREAM("homer_navigation_node::startNavigation: target cell " << new_target);
    Eigen::Vector2i new_target_approx;

	switch ( m_MapTypeMachine.state() )
	{
		case SLAM_MAP:
        {
            ROS_INFO_STREAM( "Resetting occupancy map." );

            if(fast_path_planning_)
            {
                maskMap();
            }
            explorer_->setOccupancyMap(width_, width_, origin_, &(*last_map_data_)[0]);
            explorer_->setStart(map_tools::toMapCoords(robot_pose_.position, origin_, resolution_));
            new_target_approx = explorer_->getNearestAccessibleTarget(new_target);
			break;
        }
		case NAVIGATION_MAP:
            ROS_INFO_STREAM( "Updating obstacles in externally assigned navigation map." );
            explorer_->updateObstacles( width_, width_, origin_, &(*last_map_data_)[0] );
            explorer_->setStart(map_tools::toMapCoords(robot_pose_.position, origin_, resolution_));
            new_target_approx = new_target;
			break;
	}

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

    bool new_approx_is_better = ( map_tools::distance( robot_pose_.position, target_point_ ) - map_tools::distance( new_target_approx_world, target_point_ ) ) > 0.2;
    bool new_approx_reaches_target = ( map_tools::distance(new_target_approx_world, target_point_ ) < desired_distance_ );
    m_path_reaches_target = new_approx_reaches_target;
    if ( !new_approx_is_better && !new_approx_reaches_target )
	{
      ROS_WARN_STREAM( "No better way to target found, turning and then reporting as unreachable."
          <<     endl << "Distance to target: " << distanceTo( target_point_ ) << "m; requested: " << desired_distance_ << "m" );
	  m_MainMachine.setState( FINAL_TURN );
	}
	else
	{
      target_approx_ = new_target_approx;
      m_MainMachine.setState ( FOLLOWING_PATH );
      calculatePath();
	}
  }
  
void HomerNavigationNode::sendPathData()
{
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.pose = robot_pose_;
    pose_stamped.header.frame_id = "/map";
	pose_stamped.header.stamp = ros::Time::now();
	
    nav_msgs::Path msg;
    msg.poses = waypoints_;
    if(waypoints_.size() > 0)
        msg.poses.insert(msg.poses.begin(), pose_stamped);
    path_pub_.publish(msg);
 }

void HomerNavigationNode::sendTargetReachedMsg() {
	sendStopRobot();
    m_MainMachine.setState( IDLE );
    std_msgs::Empty reached_msg;
    target_reached_pub_.publish(reached_msg);
    waypoints_.clear();
    nav_msgs::Path empty_path_msg;
    empty_path_msg.poses = waypoints_;
    path_pub_.publish(empty_path_msg);
    ROS_INFO_STREAM("TargetReachedMsg");
}

void HomerNavigationNode::sendTargetUnreachableMsg( int8_t reason ) {
	sendStopRobot();
	m_MainMachine.setState( IDLE );
    homer_mapnav_msgs::TargetUnreachable unreachable_msg;
    unreachable_msg.reason = reason;
    target_unreachable_pub_.publish(unreachable_msg);
    waypoints_.clear();
    nav_msgs::Path empty_path_msg;
    empty_path_msg.poses = waypoints_;
    path_pub_.publish(empty_path_msg);
    ROS_INFO_STREAM("TargetUnreachableMsg");
}

void HomerNavigationNode::targetPositionReached()
{
  //we're as close s we can get, target reached
    ROS_INFO_STREAM( "Target position reached. Distance to target: " << distanceTo( target_point_ ) << "m. Desired distance:" << desired_distance_ << "m" );
	sendStopRobot();
   // usleep( 30000 );
    waypoints_.clear();
    sendPathData();
	m_MainMachine.setState( FINAL_TURN );
    ROS_INFO_STREAM("Turning to look-at point");
}


void HomerNavigationNode::checkPath()
{
    invalid_path_count_=0;
    for ( unsigned i=0; i<pixel_path_.size(); i++ )
	{
        geometry_msgs::Point p = map_tools::fromMapCoords(pixel_path_.at(i), origin_, resolution_);
        if ( distanceTo(p) > check_path_max_distance_ )
		{
			continue;
		}
        if (map_tools::findValue( last_map_data_, width_, height_, pixel_path_[i].x(), pixel_path_[i].y(), 90, m_AllowedObstacleDistance.first ) )
		{
            invalid_path_count_++;
            ROS_WARN_STREAM("Obstacle detected in current path " << invalid_path_count_ << " of " << check_path_max_errors_ << " times.");

            if ( invalid_path_count_ >= check_path_max_errors_ )
            {
                ROS_WARN_STREAM( "Replanning path." );
                currentPathFinished();
			}
			return;
		}
	}
}

void HomerNavigationNode::handleCollision ()
{
	if ( m_MainMachine.state()== FOLLOWING_PATH )
	{
        sendStopRobot();
        if( distanceTo( target_point_) < collision_distance_near_target_ )
        {
            ROS_INFO_STREAM("Collision detected near target. Switch to final turn.");
            targetPositionReached();
        }
        else
        {
            m_MainMachine.setState( AVOIDING_COLLISION );
            ROS_WARN_STREAM( "Collision detected while following path!" );
        }
    }
}

float HomerNavigationNode::calcSpeedFactor()
{
    float speedFactor = SpeedControl::getSpeedFactor(laser_points_, m_MinMoveSpeedFactor, 1.0 );

	m_LastSpeedFactors.push_back( speedFactor );
	if ( m_LastSpeedFactors.size() > m_SpeedFactorMeanFilterSize ) { m_LastSpeedFactors.pop_front(); }

    float speedFactorMean = mean( m_LastSpeedFactors );

	return speedFactorMean;
}


void HomerNavigationNode::performNextMove()
{
    float maxMoveDistance = SpeedControl::getMaxMoveDistance ( laser_points_ );
	float speedFactor = calcSpeedFactor();

	switch ( m_MainMachine.state() )
	{
		case FOLLOWING_PATH:
		{
            if ( distanceTo( target_point_ ) < desired_distance_ )
			{
                ROS_INFO_STREAM( "Desired distance to target was reached." );
				targetPositionReached();
				return;
            }

			float waypointRadiusLaser = 0.125 * maxMoveDistance;

            Eigen::Vector2i waypointPixel = map_tools::toMapCoords(waypoints_[0].pose.position, origin_, resolution_);
            float obstacleDistanceMap = explorer_->getObstacleTransform()->getValue( waypointPixel.x(), waypointPixel.y() );
            float waypointRadiusMap = 0.125 * obstacleDistanceMap * resolution_;

			float waypointRadius = waypointRadiusLaser + waypointRadiusMap;
            if ( ( waypointRadius < resolution_ ) || ( waypoints_.size() == 1 ) )
			{
                waypointRadius = resolution_;
			}

            //if we have accidentaly skipped waypoints, recalculate path
			float minDistance=FLT_MAX;
			unsigned nearestWaypoint=0;
            for ( unsigned i=0; i<waypoints_.size(); i++ )
			{
                if ( distanceTo( waypoints_[i].pose.position ) < minDistance ) {
					nearestWaypoint = i;
                    minDistance = distanceTo( waypoints_[i].pose.position );
				}
			}
			if ( nearestWaypoint != 0 )
			{
                ROS_WARN_STREAM("Waypoints skipped. Recalculating path!");
				calculatePath();
				if ( m_MainMachine.state() != FOLLOWING_PATH ) { break; }
			}

            ROS_DEBUG_STREAM("NextMove DEBUG VALUES: maxMoveDistance="<< maxMoveDistance <<"\n"
                            << "\tWaypointRadiusLaser=" << waypointRadiusLaser << "\n\t"
                            << "obstacleDistanceMap=" << obstacleDistanceMap << "\n\t"
                            << "");

            //check if current waypoint has been reached
            if ( (waypoints_.size() != 0) && ( distanceTo( waypoints_[0].pose.position ) < waypointRadius ) )
            {
                waypoints_.erase ( waypoints_.begin() );
                ROS_DEBUG_STREAM("homer_navigation::performNextMove(): Current waypoint has been reached! Still " << waypoints_.size() << " to reach!");
            }

			sendPathData();

			ROS_ERROR_STREAM("waypointRadius: " << waypointRadius);
            //last wayoint reached
            if ( waypoints_.size() == 0 )
			{
                ROS_INFO_STREAM("Last waypoint reached");
				currentPathFinished();
				return;
            }

            geometry_msgs::Point currentWaypoint = waypoints_[0].pose.position;

			double distanceToWaypoint = distanceTo ( currentWaypoint );
			double angleToWaypoint = angleToPointDeg ( currentWaypoint );

            ROS_DEBUG_STREAM("homer_navigation::performNextMove(): Distance to waypoint: "<<distanceToWaypoint<<" Angle to waypoint: "<< (angleToWaypoint) << " Waypoint: " << currentWaypoint << "Robot Pose: " << robot_pose_.position << " Robot orientation " << tf::getYaw(robot_pose_.orientation)) ;

			ostringstream stream;
			stream.precision(2);

			if (!m_use_cmd_vel_) 
			{
				ROS_ERROR_STREAM("Not supported");
			}
				//move to next waypoint if heading in right direction
			else // else use cmd_vel
			{
				if (angleToWaypoint < -180)
				{
					angleToWaypoint += 360;
				}	

				//linear speed calculation
				double speed = distanceToWaypoint; 
				if (speed < 0 )
				{
					speed = max(speed,-m_max_move_speed);
				}
				else
				{
					speed = min(speed,m_max_move_speed);
				}
				if(m_avoided_collision)
				{
					if( std::abs(angleToWaypoint) < 10)
					{
						m_avoided_collision = false;
					}
					else
					{
						speed = 0;
					}
				}
				//linear speed calculation end
				//angular speed calculation
				double angle = angleToWaypoint*3.14/180.0;
				if (abs(angle) < m_min_turn_angle)
				{
					angle = 0.0;
				}
				else
				{
					if (abs(angle) > m_max_drive_angle)
					{
						speed = 0.0;
					}
					if (angle < 0 )
					{
						angle = max(angle,-m_max_turn_speed);
					}
					else
					{
						angle = min(angle,m_max_turn_speed);
					}
					if (distanceToWaypoint < 1.0)
					{
					angle *= distanceToWaypoint;
					}
					// min speed for angles because under 0.35 the machine is really really slow
					if (speed < 0.07)
					{
						if ( angle < 0 ) 
						{
							angle = min(angle,-0.45);
						}
						else
						{
							angle = max(angle,0.45);
						}
					}
				}
				//angular speed calculation end			

				m_act_speed = speed;
    			m_act_angle = angle; 
				geometry_msgs::Twist cmd_vel_msg;
				cmd_vel_msg.linear.x = speed;			
				cmd_vel_msg.angular.z = angle;
				cmd_vel_pub_.publish(cmd_vel_msg);

				stream << "Driving & turning" << endl;
				stream << "linear: " << speed << " angular: " << angle << endl;
				stream << "distanceToWaypoint:" << distanceToWaypoint << "angleToWaypoint: " << angleToWaypoint << endl;
			}
			ROS_INFO_STREAM( stream.str() );
			break;
		}

		case AVOIDING_COLLISION:
		{
            if( distanceTo( target_point_) < collision_distance_near_target_ )
            {
                ROS_INFO_STREAM("Collision detected near target. Switch to final turn.");
                targetPositionReached();
            } 
			else if ( maxMoveDistance <= collision_distance_ )
			{
				ostringstream stream;
                stream << "Maximum driving distance too short (" << maxMoveDistance << "m)! Moving back.";
                ROS_WARN_STREAM( stream.str() );
				if (!m_use_cmd_vel_)
				{
					ROS_ERROR_STREAM("Not supported");
				}
				else // cmd_vel
				{
                    geometry_msgs::Twist cmd_vel_msg;
                    if(HomerNavigationNode::obstacleBackwardDistance() > 0.4)
                    {
                        cmd_vel_msg.linear.x = -0.3;
                    }
                    else
                    {
                        cmd_vel_msg.angular.z = -0.45;
                    }
                    cmd_vel_pub_.publish(cmd_vel_msg);
				}
			} 
			else
			{
				m_avoided_collision = true;
                ROS_WARN_STREAM( "Collision avoided. Updating path." );
                currentPathFinished();
			}
			break;
		}
		case FINAL_TURN:
		{
			if ( 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);
        		}
				break;
			}

			double turnAngle = minTurnAngle( tf::getYaw(robot_pose_.orientation), target_orientation_ );
			ROS_INFO_STREAM("homer_navigation::PerformNextMove:: Final Turn. Robot orientation: " << rad2Deg(tf::getYaw(robot_pose_.orientation)) << ". Target orientation: " << rad2Deg(target_orientation_) );
			ROS_DEBUG_STREAM ( "homer_navigation::PerformNextMove:: turnAngle: " << rad2Deg(turnAngle));
			if (m_use_cmd_vel_ ) // final turn with cmd_vel
			{
				if (turnAngle< 0 )
				{
					turnAngle= max(turnAngle,-m_max_turn_speed);
				}
				else
				{
					turnAngle = min(turnAngle,m_max_turn_speed);
				}
				
				if (abs(turnAngle) < m_min_turn_angle) 
				{
					ROS_INFO_STREAM(":::::::TARGET REACHED BECAUSE lower "<<m_min_turn_angle);
					if( m_path_reaches_target	)
		            {
		    			sendTargetReachedMsg();
		    		}
		    		else
		    		{
		    			sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
		    		}
				}
				else
				{
					geometry_msgs::Twist cmd_vel_msg;
					cmd_vel_msg.angular.z = turnAngle;
					cmd_vel_pub_.publish(cmd_vel_msg);				
				}
			}
			break;
		}

		case AWAITING_EXPLORATION_MAP:
		case AWAITING_PATHPLANNING_MAP:
        {
      //make sure that the robot doesn't move
            ROS_INFO_STREAM("Awaiting pathplanning map");
			sendStopRobot();
			break;
        }
        case IDLE:
			break;
    }
}

void HomerNavigationNode::currentPathFinished() // also used for replanning
{
    ROS_INFO_STREAM( "Current path was finished, initiating recalculation.");
	waypoints_.clear();
	sendStopRobot();
	m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );

}

// returns angle to target point in degrees(!)
int HomerNavigationNode::angleToPointDeg ( geometry_msgs::Point target )
{
    double cx = robot_pose_.position.x;
    double cy = robot_pose_.position.y;
    int targetAngle = rad2Deg( atan2 ( target.y - cy, target.x - cx ) );
    int currentAngle = rad2Deg( tf::getYaw(robot_pose_.orientation) );

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


double HomerNavigationNode::distanceTo ( geometry_msgs::Point target )
{
    double cx = robot_pose_.position.x;
    double cy = robot_pose_.position.y;
    double distance_to_target_2 = ( cx - target.x ) * ( cx - target.x ) + ( cy - target.y ) * ( cy - target.y );
    return sqrt ( distance_to_target_2 );
}

float HomerNavigationNode::obstacleBackwardDistance()
{
    float min_y = -0.27; 
    float max_y =  0.27;

    float back_distance = 4;

    for(float depth = -0.3 ; depth > -1.0; depth -= 0.01) 
    {
        for(float y = min_y ; y <= max_y ; y += 0.1) 
        {
            geometry_msgs::Point base_link_point;
            base_link_point.x = depth;
            base_link_point.y = y;
            geometry_msgs::Point map_point = map_tools::transformPoint(base_link_point, transform_listener_ ,"/base_link", "/map");
            int i = map_tools::map_index(map_point, origin_ ,width_,resolution_);
            if(last_map_data_->at(i) > 90)
            {
              if(back_distance > HomerNavigationNode::distanceTo(map_point))
              {
                  back_distance = HomerNavigationNode::distanceTo(map_point);
              }
            }
        }
        if(back_distance != 4)
        {
            break;
        }
    }
    ROS_ERROR_STREAM("Back Distance: "<< back_distance);
    return back_distance;
}

void HomerNavigationNode::actualizeStatusInfo()
{
	ostringstream stream;
	stream << m_MapTypeMachine.stateString() << '\n'
           << m_MainMachine.stateString();
    ROS_DEBUG_STREAM( stream.str() );
}

void HomerNavigationNode::maskMap()
{
    //generate bounding box
    ROS_INFO_STREAM("Calculating Bounding box for fast planning");
    Eigen::Vector2i pose_pixel =  map_tools::toMapCoords( robot_pose_.position,  origin_, resolution_);
    Eigen::Vector2i target_pixel =  map_tools::toMapCoords( target_point_,  origin_, 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(), origin_, resolution_));
    ROS_INFO_STREAM("max in m: "<<map_tools::fromMapCoords(safe_planning_box.max(), origin_, resolution_));
    for(size_t x = 0; x < width_; x++)
  	{
        for(size_t y = 0; y < width_; y++)
        {
            if(!safe_planning_box.contains(Eigen::Vector2i(x, y)))
            {
                last_map_data_->at(y * 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;

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

template<class ContainerT>
double HomerNavigationNode::mean ( const ContainerT& values )
{
  typename ContainerT::const_iterator it;
  it = values.begin();
  double sum = 0;
  while ( it != values.end() )
  {
    sum += *it;
    it++;
  }
  return sum / double ( values.size() );
}

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(msg->info.height != msg->info.width)
    {
        ROS_ERROR_STREAM("Incoming Map not quadratic. No map update!");
        return;
    }
    if(msg->header.stamp != last_map_timestamp_)
    {
        last_map_timestamp_ = msg->header.stamp;
        last_map_data_ = new std::vector<int8_t>(msg->data);
        origin_ = msg->info.origin;
        width_ = msg->info.width;
        height_ = msg->info.height;
        resolution_ = msg->info.resolution;
        
        Eigen::Vector2i map_point;
		if(m_laser_points_map.size() > 0)
		{
			for(int i = 0; i < m_laser_points_map.size(); i++)
			{
				geometry_msgs::Point& point = m_laser_points_map[i];
				map_point = map_tools::toMapCoords(point, origin_, resolution_);
				int k = map_point.y() * width_ + map_point.x();
				if(k < 0 || k > width_*height_)
				{
					continue;
				}
				last_map_data_->at(k) = homer_mapnav_msgs::ModifyMap::BLOCKED;
			}
		}
		if(m_back_laser.size() > 0 )
		{
			for(int i = 0; i < m_back_laser.size(); i++)
			{
				geometry_msgs::Point& point = m_back_laser[i];
				map_point = map_tools::toMapCoords(point, origin_, resolution_);
				int k = map_point.y() * width_ + map_point.x();
				if(k < 0 || k > width_*height_)
				{
					continue;
				}
				last_map_data_->at(k) = homer_mapnav_msgs::ModifyMap::BLOCKED;
			}
		}
		
        switch ( m_MainMachine.state() )
        {
            case AWAITING_EXPLORATION_MAP:
                if ( m_MapTypeMachine.state() ==  SLAM_MAP )
                {
                    startExploration();
                }
                break;

            case AWAITING_PATHPLANNING_MAP:
                startNavigation();
                break;
            case FOLLOWING_PATH:
            {
                if ( check_path_)
                {
                    checkPath();
                }
                
                break;
			}
            default:
                break;
        }
    }
}

void HomerNavigationNode::poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    robot_pose_ = msg->pose;
	performNextMove();
}

void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    float frontal_obstacle_distance;
    bool collision_detected;
    
    last_laser_time_ = ros::Time::now();
    laser_points_ =  map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, transform_listener_, msg->header.frame_id, "/base_link");                                       
    m_laser_points_map =  map_tools::laser_ranges_to_points( msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, transform_listener_, msg->header.frame_id, "/map");                

    if(m_act_speed == 0) 
    {
    	return;
    }
    frontal_obstacle_distance = SpeedControl::getMaxMoveDistance ( laser_points_ );
    collision_detected = frontal_obstacle_distance <= collision_distance_;
    if(collision_detected)
    {
    	handleCollision();
    }
}

void HomerNavigationNode::backLaserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
	m_back_laser = map_tools::laser_ranges_to_points( msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, transform_listener_, msg->header.frame_id, "/map");                
}

void HomerNavigationNode::startNavigationCallback(const homer_mapnav_msgs::StartNavigation::ConstPtr& msg)
{
    ROS_INFO_STREAM("Start navigating to (" << msg->goal.position.x << ", " << msg->goal.position.y << ")");
    if (m_MainMachine.state() != IDLE) {
        ROS_WARN_STREAM( "Aborting current operation and starting navigation.\n");
    }

    m_MapTypeMachine.setState(SLAM_MAP);

    target_point_ = msg->goal.position;
    target_orientation_ = tf::getYaw(msg->goal.orientation);
    desired_distance_ = msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
	skip_final_turn_ = msg->skip_final_turn;
    fast_path_planning_ = msg->fast_planning;

    m_LastSpeedFactors.clear();

    ROS_INFO_STREAM("Navigating to target " << target_point_.x << ", " << target_point_.y
                    << "\nTarget orientation: " << target_orientation_
                    << "Desired distance to target: " << desired_distance_);

    m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
}


void HomerNavigationNode::moveBaseSimpleGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	m_avoided_collision 	= false;
    target_point_ 			= msg->pose.position;
    target_orientation_ 	= tf::getYaw(msg->pose.orientation);
	desired_distance_ 		= 0.1;// msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
	skip_final_turn_ 		= false;
    fast_path_planning_ 	= false;

    ROS_INFO_STREAM("Navigating to target via Move Base Simple x: " << target_point_.x << ", y: " << target_point_.y
                    << "\nTarget orientation: " << target_orientation_
                    << "Desired distance to target: " << desired_distance_ );

    m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
}

void HomerNavigationNode::navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr &msg)
{
	m_avoided_collision = false;
    std::string name = msg->poi_name;
    homer_mapnav_msgs::GetPointsOfInterest srv;
    get_POIs_client_.call(srv);