homer_navigation_node.cpp

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