homer_navigation_node.cpp

	{
		m_target_point 			= msg->pose.position;
		m_target_orientation 	= tf::getYaw(msg->pose.orientation);
	}
	m_avoided_collision 	= false;
	m_desired_distance 		= 0.1;
	m_skip_final_turn 		= false;
	m_fast_path_planning 	= false;
	m_new_target 		    = true;

	ROS_INFO_STREAM("Navigating to target via Move Base Simple x: " << m_target_point.x << ", y: " << m_target_point.y
			<< "\nTarget orientation: " << m_target_orientation
			<< " Desired distance to target: " << m_desired_distance
			<< "\nframe_id: " << msg->header.frame_id);

	m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
}

void HomerNavigationNode::navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr &msg)
{
	homer_mapnav_msgs::GetPointsOfInterest srv;
	m_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 == msg->poi_name)
		{
			m_avoided_collision  = false;	
			m_target_point 		 = it->pose.position;
			m_target_orientation = tf::getYaw(it->pose.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 = false;
			m_new_target 		 = true;
			m_target_name        = msg->poi_name;

			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 );
			return;
		}
	}

	ROS_ERROR_STREAM("No point of interest with name '" << msg->poi_name << "' found in current poi list");
	sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::UNKNOWN);
}

void HomerNavigationNode::stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg)
{
	ROS_INFO_STREAM("Stopping navigation." );
	m_MainMachine.setState( IDLE );
	m_avoided_collision = false;
	stopRobot();

	m_waypoints.clear();
	nav_msgs::Path empty_path_msg;
	empty_path_msg.poses = m_waypoints;
	m_path_pub.publish(empty_path_msg);
}

void HomerNavigationNode::unknownThresholdCallback(const std_msgs::Int8::ConstPtr &msg)
{
	m_explorer->setUnknownThreshold(static_cast<int>(msg->data));
}

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

	HomerNavigationNode node;

	ros::Rate rate(50);

	while(ros::ok())
	{
		ros::spinOnce();
		node.idleProcess();
		rate.sleep();
	}

	return 0;
}