#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);
std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
for(it = srv.response.poi_list.pois.begin(); it != srv.response.poi_list.pois.end(); ++it)
{
if(it->name == name)
{
ROS_INFO_STREAM("Start navigating to (" << it->pose.position.x << ", " << it->pose.position.y << ")");
if (m_MainMachine.state() != IDLE) {
ROS_WARN_STREAM( "Aborting current operation and starting navigation.\n");
}
m_MapTypeMachine.setState(SLAM_MAP);
sendStopRobot();
target_point_ = it->pose.position;
target_orientation_ = tf::getYaw(it->pose.orientation);
desired_distance_ = msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
skip_final_turn_ = msg->skip_final_turn;
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 );
return;
}
}
ROS_ERROR_STREAM("No point of interest with name '" << msg->poi_name << "' found in current poi list");
}
void HomerNavigationNode::stopNavigationCallback(const homer_mapnav_msgs::StopNavigation::ConstPtr& msg)
{
ROS_INFO_STREAM("Stopping navigation." );
// stop exploring
m_MainMachine.setState( IDLE );
m_avoided_collision = false;
sendStopRobot();
waypoints_.clear();
nav_msgs::Path empty_path_msg;
empty_path_msg.poses = waypoints_;
path_pub_.publish(empty_path_msg);
}
void HomerNavigationNode::unknownThresholdCallback(const std_msgs::Int8::ConstPtr &msg)
{
explorer_->setUnknownThreshold(static_cast<int>(msg->data));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "homer_navigation");
HomerNavigationNode node;
ros::Rate rate(12);
while(ros::ok())
{
ros::spinOnce();
node.idleProcess();
rate.sleep();
}
return 0;
}