Software

/*
Team J: Project DAIR
Team Members: Matthew, Udit, Hardik, Taruna, Nanditha

ROS node that initiates onboard communication. The Planner Class creates a node handler, intializers subscribers and the necessary call back functions. Initializes phase manager that handles the various phases during autonomous flight. 
*/
  
#include "comm_pipeline/planner.h"

Planner::Planner(ros::NodeHandle *nodehandle) : nh_(*nodehandle),
                                                changed_to_guided_(false),
                                                delivery_waypoint_reached_(false)
{
  initializeSubscribers();
  initializeServices();

  ros::NodeHandle param_nh("~");

  param_nh.param("delivery_waypoint_number", delivery_waypoint_number_, 2);
  param_nh.param("wp_threshold", wp_threshold_, 0.25f);
}

void Planner::initializeSubscribers()
{
  state_sub_ = nh_.subscribe<mavros_msgs::State>("mavros/state", 10, &Planner::stateCallback, this);
  nav_output_sub_ = nh_.subscribe<mavros_msgs::NavControllerOutput>("mavros/nav_controller_output", 10, &Planner::navOutputCallback, this);
  mission_sub_ = nh_.subscribe<mavros_msgs::WaypointReached>("mavros/mission/reached", 10, &Planner::missionCallback, this);
}

void Planner::initializeServices()
{
  set_mode_client_ = nh_.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
  activate_stag_client_ = nh_.serviceClient<comm_pipeline::ActivateStag>("marker/activate_stag");

  ros::service::waitForService("mavros/set_mode");
  ros::service::waitForService("marker/activate_stag");
  ROS_DEBUG("All service clients connceted succesfully");

  found_marker_server_ = nh_.advertiseService("planner/found_marker",
                                              &Planner::foundMarkerCallback, this);
}

void Planner::activateStag()
{
  comm_pipeline::ActivateStag srv;

  if (activate_stag_client_.call(srv))
  {
    if (srv.response.activated)
    {
      ROS_INFO("Marker detector activated succesfully");
    }
    else
    {
      ROS_WARN("Marker detector did not activate");
    }
  }
  else
  {
    ROS_ERROR("Call to ActivateStag service failed");
  }
}

bool Planner::foundMarkerCallback(comm_pipeline::FoundMarker::Request &req, comm_pipeline::FoundMarker::Response &res)
{
  ROS_INFO("Found marker service called");

  if (phase_manager_.isInitialized() && phase_manager_.getCurrentPhaseType() == PhaseType::Scan)
  {
    phase_manager_.setDetectedPosition(req.position.point.x, req.position.point.y, req.position.point.z);
    phase_manager_.changePhase(PhaseType::Detected);
    res.success = true;
  }
  else
  {
    ROS_WARN("Found Marker called whilst not in Scan");
    res.success = false;
  }

  return true;
}

void Planner::stateCallback(const mavros_msgs::State::ConstPtr &msg)
{
  current_state_ = *msg;

  if (!current_state_.connected)
  {
    return;
  }

  /*
  Change to guided only when:
    1. Not previously changed into GUIDED
    2. Delivery Waypoint is reached
    3. Not already in GUIDED
  */
  if (!changed_to_guided_ && delivery_waypoint_reached_ && current_state_.mode != "GUIDED")
  {
    mavros_msgs::SetMode mode_change;
    mode_change.request.custom_mode = "GUIDED";

    if (set_mode_client_.call(mode_change))
    {

      // Make sure GUIDED mode was sent to the FCU
      if (!mode_change.response.mode_sent)
      {
        ROS_ERROR("GUIDED mode was not set by MAVROS");
        return;
      }

      ROS_INFO("Guided mode enabled...");
      changed_to_guided_ = true;

      // Send STag Activation
      activateStag();

      // Setup the phase manager
      phase_manager_.initialize();
    }
    else
    {
      ROS_ERROR("Call to SetMode service failed");
    }
  }
}

void Planner::navOutputCallback(const mavros_msgs::NavControllerOutput::ConstPtr &msg)
{
  nav_output_ = *msg;

  if (current_state_.mode != "GUIDED" || !phase_manager_.isInitialized())
  {
    return;
  }

  // nav output returns in cm so convert to metres
  float xy_distance_to_wp = nav_output_.wp_dist / 100.0f;
  float total_dist_to_wp = pow(xy_distance_to_wp, 2) + pow(nav_output_.alt_error, 2);

  // If we havent arrived at the waypoint then return
  if (total_dist_to_wp > pow(wp_threshold_, 2))
  {
    return;
  }

  phase_manager_.runCurrentPhase();
}

void Planner::missionCallback(const mavros_msgs::WaypointReached::ConstPtr &msg)
{
  if (msg->wp_seq >= delivery_waypoint_number_)
  {
    delivery_waypoint_reached_ = true;
    ROS_INFO("Delivery Waypoint Reached");
  }
}