{"id":437,"date":"2022-06-03T01:07:42","date_gmt":"2022-06-03T01:07:42","guid":{"rendered":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/?page_id=437"},"modified":"2022-09-25T03:45:03","modified_gmt":"2022-09-25T03:45:03","slug":"software","status":"publish","type":"page","link":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/software\/","title":{"rendered":"Software"},"content":{"rendered":"\n<pre class=\"wp-block-code\"><code>\/*\nTeam J: Project DAIR\nTeam Members: Matthew, Udit, Hardik, Taruna, Nanditha\n\nROS 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. \n*\/\n  \n#include \"comm_pipeline\/planner.h\"\n\nPlanner::Planner(ros::NodeHandle *nodehandle) : nh_(*nodehandle),\n                                                changed_to_guided_(false),\n                                                delivery_waypoint_reached_(false)\n{\n  initializeSubscribers();\n  initializeServices();\n\n  ros::NodeHandle param_nh(\"~\");\n\n  param_nh.param(\"delivery_waypoint_number\", delivery_waypoint_number_, 2);\n  param_nh.param(\"wp_threshold\", wp_threshold_, 0.25f);\n}\n\nvoid Planner::initializeSubscribers()\n{\n  state_sub_ = nh_.subscribe&lt;mavros_msgs::State&gt;(\"mavros\/state\", 10, &amp;Planner::stateCallback, this);\n  nav_output_sub_ = nh_.subscribe&lt;mavros_msgs::NavControllerOutput&gt;(\"mavros\/nav_controller_output\", 10, &amp;Planner::navOutputCallback, this);\n  mission_sub_ = nh_.subscribe&lt;mavros_msgs::WaypointReached&gt;(\"mavros\/mission\/reached\", 10, &amp;Planner::missionCallback, this);\n}\n\nvoid Planner::initializeServices()\n{\n  set_mode_client_ = nh_.serviceClient&lt;mavros_msgs::SetMode&gt;(\"mavros\/set_mode\");\n  activate_stag_client_ = nh_.serviceClient&lt;comm_pipeline::ActivateStag&gt;(\"marker\/activate_stag\");\n\n  ros::service::waitForService(\"mavros\/set_mode\");\n  ros::service::waitForService(\"marker\/activate_stag\");\n  ROS_DEBUG(\"All service clients connceted succesfully\");\n\n  found_marker_server_ = nh_.advertiseService(\"planner\/found_marker\",\n                                              &amp;Planner::foundMarkerCallback, this);\n}\n\nvoid Planner::activateStag()\n{\n  comm_pipeline::ActivateStag srv;\n\n  if (activate_stag_client_.call(srv))\n  {\n    if (srv.response.activated)\n    {\n      ROS_INFO(\"Marker detector activated succesfully\");\n    }\n    else\n    {\n      ROS_WARN(\"Marker detector did not activate\");\n    }\n  }\n  else\n  {\n    ROS_ERROR(\"Call to ActivateStag service failed\");\n  }\n}\n\nbool Planner::foundMarkerCallback(comm_pipeline::FoundMarker::Request &amp;req, comm_pipeline::FoundMarker::Response &amp;res)\n{\n  ROS_INFO(\"Found marker service called\");\n\n  if (phase_manager_.isInitialized() &amp;&amp; phase_manager_.getCurrentPhaseType() == PhaseType::Scan)\n  {\n    phase_manager_.setDetectedPosition(req.position.point.x, req.position.point.y, req.position.point.z);\n    phase_manager_.changePhase(PhaseType::Detected);\n    res.success = true;\n  }\n  else\n  {\n    ROS_WARN(\"Found Marker called whilst not in Scan\");\n    res.success = false;\n  }\n\n  return true;\n}\n\nvoid Planner::stateCallback(const mavros_msgs::State::ConstPtr &amp;msg)\n{\n  current_state_ = *msg;\n\n  if (!current_state_.connected)\n  {\n    return;\n  }\n\n  \/*\n  Change to guided only when:\n    1. Not previously changed into GUIDED\n    2. Delivery Waypoint is reached\n    3. Not already in GUIDED\n  *\/\n  if (!changed_to_guided_ &amp;&amp; delivery_waypoint_reached_ &amp;&amp; current_state_.mode != \"GUIDED\")\n  {\n    mavros_msgs::SetMode mode_change;\n    mode_change.request.custom_mode = \"GUIDED\";\n\n    if (set_mode_client_.call(mode_change))\n    {\n\n      \/\/ Make sure GUIDED mode was sent to the FCU\n      if (!mode_change.response.mode_sent)\n      {\n        ROS_ERROR(\"GUIDED mode was not set by MAVROS\");\n        return;\n      }\n\n      ROS_INFO(\"Guided mode enabled...\");\n      changed_to_guided_ = true;\n\n      \/\/ Send STag Activation\n      activateStag();\n\n      \/\/ Setup the phase manager\n      phase_manager_.initialize();\n    }\n    else\n    {\n      ROS_ERROR(\"Call to SetMode service failed\");\n    }\n  }\n}\n\nvoid Planner::navOutputCallback(const mavros_msgs::NavControllerOutput::ConstPtr &amp;msg)\n{\n  nav_output_ = *msg;\n\n  if (current_state_.mode != \"GUIDED\" || !phase_manager_.isInitialized())\n  {\n    return;\n  }\n\n  \/\/ nav output returns in cm so convert to metres\n  float xy_distance_to_wp = nav_output_.wp_dist \/ 100.0f;\n  float total_dist_to_wp = pow(xy_distance_to_wp, 2) + pow(nav_output_.alt_error, 2);\n\n  \/\/ If we havent arrived at the waypoint then return\n  if (total_dist_to_wp &gt; pow(wp_threshold_, 2))\n  {\n    return;\n  }\n\n  phase_manager_.runCurrentPhase();\n}\n\nvoid Planner::missionCallback(const mavros_msgs::WaypointReached::ConstPtr &amp;msg)\n{\n  if (msg-&gt;wp_seq &gt;= delivery_waypoint_number_)\n  {\n    delivery_waypoint_reached_ = true;\n    ROS_INFO(\"Delivery Waypoint Reached\");\n  }\n}<\/code><\/pre>\n","protected":false},"excerpt":{"rendered":"","protected":false},"author":297,"featured_media":0,"parent":0,"menu_order":0,"comment_status":"closed","ping_status":"closed","template":"page-templates\/page_fullwidth.php","meta":{"footnotes":""},"class_list":["post-437","page","type-page","status-publish","hentry","clearfix"],"_links":{"self":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/wp-json\/wp\/v2\/pages\/437","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/wp-json\/wp\/v2\/pages"}],"about":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/wp-json\/wp\/v2\/types\/page"}],"author":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/wp-json\/wp\/v2\/users\/297"}],"replies":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/wp-json\/wp\/v2\/comments?post=437"}],"version-history":[{"count":2,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/wp-json\/wp\/v2\/pages\/437\/revisions"}],"predecessor-version":[{"id":441,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/wp-json\/wp\/v2\/pages\/437\/revisions\/441"}],"wp:attachment":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2022teamj\/wp-json\/wp\/v2\/media?parent=437"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}