simul takeoff

Onboard-SDK-ROS/CMakeLists.txt

@@ -193,6 +193,7 @@ add_service_files(
   StereoVGASubscription.srv
   StereoDepthSubscription.srv
   SetupCameraStream.srv
+  Overwatch.srv
 )
 
 ## Generate actions in the 'action' folder

Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h

@@ -37,6 +37,7 @@
 // DJI SDK includes
 #include <dji_osdk_ros/MissionWpAction.h>
 #include <dji_osdk_ros/MissionWpUpload.h>
+#include <dji_osdk_ros/Overwatch.h>
 #include <dji_osdk_ros/vehicle_wrapper.h>
 #include <dji_osdk_ros/dji_vehicle_node.h>
 
@@ -70,13 +71,14 @@ typedef struct ServiceAck
 
 
 
-bool runWaypointMission(int responseTimeout);
+bool setupWaypointMission(int responseTimeout);
+bool runWaypointMission();
 
 void setWaypointDefaults(DJI::OSDK::WayPointSettings* wp);
 
 void setWaypointInitDefaults(dji_osdk_ros::MissionWaypointTask& waypointTask);
 
-std::vector<DJI::OSDK::WayPointSettings> createWaypoints();
+// std::vector<DJI::OSDK::WayPointSettings> createWaypoints();
 
 std::vector<DJI::OSDK::WayPointSettings> generateWaypointsPolygon(
   DJI::OSDK::WayPointSettings* start_data, DJI::OSDK::float64_t increment,
@@ -101,6 +103,8 @@ bool takeoff();
 
 bool land();
 
+bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res);
+
 void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
 //void waypointEventCallback(Vehicle *vehicle, RecvContainer recvFrame, UserData userData);
 

Onboard-SDK-ROS/launch/local_test.launch

@@ -13,7 +13,7 @@
     </node>
     <param name="base_speed" type="double" value="0.5"/>
     <param name="drone_id" type="int" value="0"/>
-    <param name="camera_type" type="int" value="0"/>
-    <!-- <node pkg="dji_osdk_ros" type="bus_driver" name="bus_driver" output="screen"/> -->
+    <param name="camera_type" type="int" value="1"/>
+    <node pkg="dji_osdk_ros" type="bus_driver" name="bus_driver" output="screen"/>
     <!-- <node pkg="dji_osdk_ros" type="image_saver.py" name="image_saver" output="screen"/> -->
 </launch>

Onboard-SDK-ROS/scripts/image_activation.py

@@ -22,9 +22,9 @@ def main():
     try:
         camera_service_proxy = rospy.ServiceProxy('setup_camera_stream', SetupCameraStream)
         response = camera_service_proxy(rospy.get_param('camera_type'), 1)
-        print(response)
+        rospy.loginfo(response)
     except:
-        print("image service call failed")
+        rospy.loginfo("image service call failed")
 
 if __name__ == '__main__':
     main()

Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp

@@ -28,6 +28,7 @@
 #include <dji_osdk_ros/bus_driver.h>
 #include <dji_osdk_ros/FlightTaskControl.h>
 #include <dji_osdk_ros/ObtainControlAuthority.h>
+#include <dji_osdk_ros/Overwatch.h>
 #include <string>
 #include <iostream>
 #include <fstream>
@@ -191,7 +192,8 @@ void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
 
 }
 
-bool runWaypointMission(int responseTimeout)
+
+bool setupWaypointMission(int responseTimeout)
 {
   ros::spinOnce();
 
@@ -200,8 +202,6 @@ bool runWaypointMission(int responseTimeout)
   setWaypointInitDefaults(waypointTask);
 
   // Waypoint Mission: Create Waypoints
-  float64_t increment = 0.000001 / C_PI * 180;
-  float32_t start_alt = 5;
   ROS_INFO("Creating Waypoints..\n");
   generatedWaypts = loadWaypoints();
   // generatedWaypts = createWaypoints();
@@ -222,10 +222,15 @@ bool runWaypointMission(int responseTimeout)
     return false;
   }
 
+  return true;
+}
+
+bool runWaypointMission()
+{
+
   // Waypoint Mission: Start
   if (missionAction(DJI_MISSION_TYPE::WAYPOINT,
-                    MISSION_ACTION::START)
-        .result)
+                    MISSION_ACTION::START).result)
   {
     ROS_INFO("Mission start command sent successfully");
   }
@@ -241,7 +246,7 @@ bool runWaypointMission(int responseTimeout)
 void setWaypointDefaults(WayPointSettings* wp)
 {
   wp->damping         = 0.25;
-  wp->yaw             = 0;
+  wp->yaw             = 0.0;
   wp->gimbalPitch     = 0;
   wp->turnMode        = 0;
   wp->hasAction       = 0;
@@ -267,14 +272,14 @@ void setWaypointInitDefaults(dji_osdk_ros::MissionWaypointTask& waypointTask)
   waypointTask.gimbal_pitch_mode  = dji_osdk_ros::MissionWaypointTask::GIMBAL_PITCH_FREE;
 }
 
-std::vector<DJI::OSDK::WayPointSettings> createWaypoints()
-{
-
-  std::vector<DJI::OSDK::WayPointSettings> wpVector =
-    loadWaypoints();
-  return wpVector;
-
-}
+// std::vector<DJI::OSDK::WayPointSettings> createWaypoints()
+// {
+//
+//   std::vector<DJI::OSDK::WayPointSettings> wpVector =
+//     loadWaypoints();
+//   return wpVector;
+//
+// }
 
 std::vector<DJI::OSDK::WayPointSettings> loadWaypoints()
 {
@@ -289,13 +294,15 @@ std::vector<DJI::OSDK::WayPointSettings> loadWaypoints()
   {
     ros::param::get("/robot_list/robot_"+to_string(droneId)+"/waypoint_" + to_string(i), wpt);
     i++; // iterate to next parameter
+    ROS_INFO("Waypoint read as (Ind, Lat, Lon, Alt, Yaw): %f %f %f %f %f\n ", wpt[0], wpt[1], wpt[2], wpt[3], wpt[4]);
     WayPointSettings wp;
     setWaypointDefaults(&wp);
     wp.index     = i;
     wp.latitude = wpt[1];
     wp.longitude = wpt[2];
     wp.altitude = wpt[3];
-    wp.yaw = wpt[4];
+    // wp.yaw = static_cast<int>(wpt[4]);
+    wp.yaw = 0;
     wp_list.push_back(wp);
   }
 
@@ -322,7 +329,7 @@ void uploadWaypoints(std::vector<DJI::OSDK::WayPointSettings>& wp_list,
     waypoint.turn_mode           = wp->turnMode;
     waypoint.has_action          = 0;
     waypointTask.mission_waypoint.push_back(waypoint);
-    ROS_INFO("Waypoint created at (Lat, Lon, Alt, Dmp, Yaw): %f %f %f %f %f\n ", waypoint.latitude, waypoint.longitude, waypoint.altitude, waypoint.damping_distance, target_yaw);
+    ROS_INFO("Waypoint created at (Lat, Lon, Alt, Dmp, Yaw): %f %f %f %f %f\n ", waypoint.latitude, waypoint.longitude, waypoint.altitude, waypoint.damping_distance, waypoint.target_yaw);
   }
 }
 
@@ -385,17 +392,29 @@ bool land()
   return flightTaskControl.response.result;
 }
 
-int main(int argc, char** argv)
+// bool add(beginner_tutorials::AddTwoInts::Request &req, beginner_tutorials::AddTwoInts::Response &res)
+bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res)
 {
+  ROS_INFO("takeoff command received");
+  // ros::Duration(7.5).sleep();
 
-  //ros::Duration(7.5).sleep(); // sleep while sdk starts up// should wait for topic here
+  dji_osdk_ros::ObtainControlAuthority obtainCtrlAuthority;
+  obtainCtrlAuthority.request.enable_obtain = true;
+  obtain_ctrl_authority_client.call(obtainCtrlAuthority);
 
+  // count number of waypoints
+  runWaypointMission();
+
+  res.result = true;
+  return res.result;
+}
+
+int main(int argc, char** argv)
+{
 
   ros::init(argc, argv, "mission_node");
   ros::NodeHandle nh;
 
-  ros::Duration(7.5).sleep();
-
   ros::param::get("base_speed", baseSpeedParam);
   ros::param::get("drone_id", droneId);
   ROS_INFO("base speed set to %f", baseSpeedParam);
@@ -408,25 +427,27 @@ int main(int argc, char** argv)
     "dji_osdk_ros/mission_waypoint_upload");
   waypoint_action_client = nh.serviceClient<dji_osdk_ros::MissionWpAction>(
     "dji_osdk_ros/mission_waypoint_action");
-
-  dji_osdk_ros::ObtainControlAuthority obtainCtrlAuthority;
-  obtainCtrlAuthority.request.enable_obtain = true;
-  obtain_ctrl_authority_client.call(obtainCtrlAuthority);
-
+  flight_control_client = nh.serviceClient<dji_osdk_ros::FlightTaskControl>(
+    "flight_task_control");
   syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 10);
-
-  flight_control_client = nh.serviceClient<dji_osdk_ros::FlightTaskControl>("flight_task_control");
   gps_pos_subscriber = nh.subscribe<sensor_msgs::NavSatFix>(
     "dji_osdk_ros/gps_position", 10, &gpsPosCallback);
 
 
-  // Setup variables for use
-  int responseTimeout = 1;
+  ros::ServiceServer overwatchService = nh.advertiseService("Overwatch", overwatchFunction);
 
-  // count number of waypoints
-  waypointNumber = countWaypoints();
-  runWaypointMission(responseTimeout);
+  // wait for waypoints to be written
+  // ros::Duration(6.0).sleep();
+  // wait for services to be up
+  obtain_ctrl_authority_client.waitForExistence();
+  waypoint_upload_client.waitForExistence();
+  waypoint_action_client.waitForExistence();
+  flight_control_client.waitForExistence();
+
+  int responseTimeout = 1;
+  setupWaypointMission(responseTimeout);
 
+  ROS_INFO("bus driver ready to go!");
 
   ros::spin();
 

Onboard-SDK-ROS/srv/Overwatch.srv

+bool start
+---
+bool result