Skip to content

Commit

Permalink
Merge pull request dji-sdk#74 from lanyusea/3.2
Browse files Browse the repository at this point in the history
ENH: update python code: add activation/datatransfer API
  • Loading branch information
amenonDJI authored Mar 15, 2017
2 parents 71556e0 + bd0ef77 commit 72ff974
Show file tree
Hide file tree
Showing 2 changed files with 34 additions and 16 deletions.
27 changes: 21 additions & 6 deletions dji_sdk/src/dji_sdk/dji_drone.py
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
#!/usr/bin/env python
#encoding = utf-8
import rospy
import std_msgs.msg
import dji_sdk.msg
import nav_msgs.msg
import dji_sdk.srv
import std_msgs.msg
import dji_sdk.msg
import nav_msgs.msg
import dji_sdk.srv
import math
import time
import actionlib
Expand Down Expand Up @@ -66,6 +66,9 @@ def odometry_subscriber_callback(self, odometry):
def time_stamp_subscriber_callback(self, time_stamp):
self.time_stamp = time_stamp

def mobile_data_callback(self, data):
self.latest_mobile_data = data

def init_subscribers(self):
self.acceleration_subscriber = rospy.Subscriber("dji_sdk/acceleration", dji_sdk.msg.Acceleration, self.acceleration_subscriber_callback)
self.attitude_quaternion_subscriber = rospy.Subscriber("dji_sdk/attitude_quaternion", dji_sdk.msg.AttitudeQuaternion, self.attitude_quaternion_subscriber_callback)
Expand All @@ -81,8 +84,10 @@ def init_subscribers(self):
self.activation_subscriber = rospy.Subscriber("dji_sdk/activation", std_msgs.msg.UInt8, self.activation_subscriber_callback)
self.odometry_subscriber = rospy.Subscriber("dji_sdk/odometry", nav_msgs.msg.Odometry, self.odometry_subscriber_callback)
self.time_stamp_subscriber = rospy.Subscriber("dji_sdk/time_stamp", dji_sdk.msg.TimeStamp, self.time_stamp_subscriber_callback)
self.mobile_data_callback = rospy.Subscriber("dji_sdk/data_received_from_remote_device", dji_sdk.msg.TransparentTransmissionData, self.mobile_data_callback)

def init_services(self):
rospy.wait_for_service("dji_sdk/activation")
rospy.wait_for_service("dji_sdk/attitude_control")
rospy.wait_for_service("dji_sdk/camera_action_control")
rospy.wait_for_service("dji_sdk/drone_task_control")
Expand All @@ -96,8 +101,10 @@ def init_services(self):
rospy.wait_for_service("dji_sdk/virtual_rc_enable_control")
rospy.wait_for_service("dji_sdk/virtual_rc_data_control")
rospy.wait_for_service("dji_sdk/sync_flag_control")
rospy.wait_for_service("dji_sdk/send_data_to_remote_device")

self.attitude_control_service = rospy.ServiceProxy("dji_sdk/attitude_control", dji_sdk.srv.AttitudeControl)
self.activation_service = rospy.ServiceProxy("dji_sdk/activation", dji_sdk.srv.Activation)
self.attitude_control_service = rospy.ServiceProxy("dji_sdk/attitude_conrol", dji_sdk.srv.AttitudeControl)
self.camera_action_control_service = rospy.ServiceProxy("dji_sdk/camera_action_control", dji_sdk.srv.CameraActionControl)
self.drone_task_control_service = rospy.ServiceProxy("dji_sdk/drone_task_control", dji_sdk.srv.DroneTaskControl)
self.gimbal_angle_control_service = rospy.ServiceProxy("dji_sdk/gimbal_angle_control", dji_sdk.srv.GimbalAngleControl)
Expand All @@ -110,6 +117,7 @@ def init_services(self):
self.drone_vrc_enable_service = rospy.ServiceProxy("dji_sdk/virtual_rc_enable_control", dji_sdk.srv.VirtualRCEnableControl)
self.drone_vrc_data_service = rospy.ServiceProxy("dji_sdk/virtual_rc_data_control", dji_sdk.srv.VirtualRCDataControl)
self.sync_timestamp_service = rospy.ServiceProxy("dji_sdk/sync_flag_control", dji_sdk.srv.SyncFlagControl)
self.send_data_to_remote_device = rospy.ServiceProxy("dji_sdk/send_data_to_remote_device", dji_sdk.srv.SendDataToRemoteDevice)

def init_actions(self):
self.local_position_navigation_action_client = actionlib.SimpleActionClient("dji_sdk/local_position_navigation_action", dji_sdk.msg.LocalPositionNavigationAction)
Expand All @@ -119,6 +127,9 @@ def init_actions(self):
self.waypoint_navigation_action_client = actionlib.SimpleActionClient("dji_sdk/waypoint_navigation_action", dji_sdk.msg.WaypointNavigationAction)
self.waypoint_navigation_action_client.wait_for_server()

def activation(self):
self.activation_service()

def local_position_navigation_send_request(self, x, y, z):
goal = dji_sdk.msg.LocalPositionNavigationGoal(x = x, y = y, z = z)
self.local_position_navigation_action_client.send_goal(goal)
Expand Down Expand Up @@ -199,6 +210,9 @@ def lookat(self, x, y, z, duration):
self.gimbal_angle_control_service(flag = flag, yaw = yaw, roll = 0, pitch = 0, duration = duration)
self.local_position_control_service(x = x, y = y, z = z, yaw = yaw)

def send_data_to_remote_device(self, data):
self.send_data_to_remote_device(data)

def __init__(self, namespace=''):
rospy.init_node('dji_sdk_connector')
self.namespace = namespace
Expand All @@ -214,8 +228,9 @@ def __init__(self, namespace=''):
self.local_position = dji_sdk.msg.LocalPosition()
self.local_position_ref = dji_sdk.msg.LocalPosition()
self.power_status = dji_sdk.msg.PowerStatus()
self.rc_channels = dji_sdk.msg.RCChannels()
self.rc_channels = dji_sdk.msg.RCChannels()
self.time_stamp = dji_sdk.msg.TimeStamp()
self.latest_mobile_data = dji_sdk.msg.TransparentTransmissionData()
self.velocity = dji_sdk.msg.Velocity()
self.odometry = nav_msgs.msg.Odometry()

Expand Down
23 changes: 13 additions & 10 deletions dji_sdk_demo/script/client.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#!/usr/bin/env python

from dji_sdk.dji_drone import DJIDrone
import dji_sdk.msg
import dji_sdk.msg
import time
import sys
import math
Expand All @@ -27,7 +27,8 @@ def display_main_menu():
print "[q] Disarm Test"
print "[r] Vrc Test"
print "[s] Sync Test"
print "[t] Exit"
print "[t] Send Data to Mobile"
print "[u] Exit"
print "\ninput a/b/c etc..then press enter key"
print "\nuse `rostopic echo` to query drone status"
print "----------------------------------------"
Expand Down Expand Up @@ -158,11 +159,11 @@ def main():
elif main_operate_code == 'h':
R = 2
V = 2
# start to draw circle
# start to draw circle
for i in range(300):
vx = V * math.sin((V/R)*i/50.0)
vy = V * math.cos((V/R)*i/50.0)

drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, vx, vy, 0, 0)
time.sleep(0.02)
elif main_operate_code == 'i':
Expand All @@ -189,13 +190,13 @@ def main():
# stop video
drone.stop_video()
elif main_operate_code == 'm':
# Local Navi Test
# Local Navi Test
drone.local_position_navigation_send_request(-100, -100, 100)
elif main_operate_code == 'n':
# GPS Navi Test
# GPS Navi Test
drone.global_position_navigation_send_request(22.535, 113.95, 100)
elif main_operate_code == 'o':
# Waypoint List Navi Test
# Waypoint List Navi Test
newWaypointList = [
dji_sdk.msg.Waypoint(latitude = 22.535, longitude = 113.95, altitude = 100, staytime = 5, heading = 0),
dji_sdk.msg.Waypoint(latitude = 22.535, longitude = 113.96, altitude = 100, staytime = 0, heading = 90),
Expand All @@ -211,17 +212,19 @@ def main():
drone.vrc_start();
for i in range(100):
drone.vrc_control(1024-660, 1024-660, 1024-660, 1024+660)
time.sleep(0.02)
time.sleep(0.02)
for i in range(1000):
drone.vrc_control(1024, 1024, 1024+660, 1024+660)
time.sleep(0.02)
time.sleep(0.02)
for i in range(1000):
drone.vrc_control(1024-660, 1024-660)
time.sleep(0.02)
time.sleep(0.02)
drone.vrc_stop()
elif main_operate_code == 's':
drone.sync_timestamp(50)
elif main_operate_code == 't':
drone.send_data_to_remote_device([1, 2, 3])
elif main_operate_code == 'u':
return
else:
display_main_menu()
Expand Down

0 comments on commit 72ff974

Please sign in to comment.