├── README.md
└── multiDrone-ROS
    ├── px4ros
        ├── CMakeLists.txt
        ├── CMakeLists.txt~
        ├── msg
        │   ├── gpsPosition.msg
        │   └── gpsPosition.msg~
        ├── package.xml
        ├── package.xml~
        ├── scripts
        │   ├── follower_control_decode.py
        │   ├── follower_control_decode.py~
        │   ├── leader_control_decode.py
        │   ├── leader_control_decode.py~
        │   ├── leader_controller.py~
        │   ├── main_control.py
        │   ├── main_control.py~
        │   ├── serial_ros.py
        │   └── serial_ros.py~
        └── src
        │   ├── px4_getpoint_node.cpp
        │   ├── px4_getpoint_node.cpp~
        │   └── px4_getpoint_node_bp.cpp
    └── spiri_go
        ├── CMakeLists.txt
        ├── README.md
        ├── action
            ├── LandHere.action
            └── Takeoff.action
        ├── include
            ├── spiri_data_classes.h
            ├── spiri_go.h
            └── spiri_go.h~
        ├── initialize
            └── apm_sim
        ├── launch
            ├── apm_pluginlists.yaml
            ├── collision_avoidance.launch
            ├── collision_avoidance.launch~
            ├── jetson.launch
            ├── kbteleop.launch
            ├── leader-sitl.launch
            ├── leader-sitl.launch~
            ├── leader.launch
            ├── leader.launch~
            ├── sitl.launch
            ├── spiri_go.launch
            ├── takeoff_and_land.launch
            └── takeoff_and_land.launch~
        ├── package.xml
        ├── scripts
            └── mavkbteleop.py
        ├── setup.py
        ├── spiripy
            ├── __init__.py
            ├── api.py
            └── spiri_except.py
        ├── src
            ├── arm_and_takeoff.cpp~
            ├── collision_avoidance.cpp
            ├── collision_avoidance.cpp~
            ├── follower.cpp
            ├── follower.cpp~
            ├── leader.cpp
            ├── leader.cpp~
            ├── leader_control.cpp
            ├── leader_control.cpp~
            ├── leader_go.cpp~
            ├── read me.txt
            ├── read me.txt~
            ├── spiri_data_classes.cpp
            ├── spiri_go.cpp
            ├── spiri_go.cpp~
            ├── spiri_go_client.cpp~
            ├── takeoff_and_land.cpp
            ├── takeoff_and_land.cpp~
            └── test
            │   ├── spiri_go_test.cpp
            │   ├── test_com.cpp
            │   └── test_com.cpp~
        ├── srv
            ├── LastState.srv
            └── LocalPosition.srv
        └── test
            ├── api.py
            ├── sim.py
            └── sitl.test


/README.md:
--------------------------------------------------------------------------------
 1 | # multiDrone-ROS
 2 | 本工程基于开源飞控程序Ardupilot，以及Mavros，实现多个飞行器的编队控制。 </br>
 3 | Package: </br>
 4 | px4ros: 负责数据通信，从串口/网口读写数据，解码接收到的数据 </br>
 5 | spiri_go: 主程序，程序完整流程和循环控制
 6 | 
 7 | # leader参考代码
 8 | px4ros/scripts/seral_ros.py   </br>
 9 | px4rod/scripts/leader_control_decode.py   </br>
10 | spiri_go/src/spiri_go.cpp    </br>
11 | spiri_go/src/leader.cpp  
12 | 
13 | # follower参考代码类似
14 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(px4ros)
 3 | 
 4 | ## Find catkin macros and libraries
 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 6 | ## is used, also find other catkin packages
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |   roscpp
 9 |   rospy
10 |   std_msgs
11 |   mavros
12 |   mavros_extras
13 |   geometry_msgs
14 |   angles
15 | 
16 |   message_generation
17 | )
18 | 
19 | add_message_files(FILES gpsPosition.msg)
20 | 
21 | generate_messages()
22 | 
23 | 
24 | catkin_package(
25 |   CATKIN_DEPENDS message_runtime
26 | )
27 | 
28 | 
29 | include_directories(
30 |   ${catkin_INCLUDE_DIRS}
31 | )
32 | 
33 | 
34 | add_executable(px4ros src/px4_getpoint_node.cpp)
35 | 
36 | 
37 | target_link_libraries(px4ros
38 |    ${catkin_LIBRARIES}
39 |  )
40 | 
41 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/CMakeLists.txt~:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(px4ros)
 3 | 
 4 | ## Find catkin macros and libraries
 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 6 | ## is used, also find other catkin packages
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |   roscpp
 9 |   rospy
10 |   std_msgs
11 |   mavros
12 |   mavros_extras
13 |   geometry_msgs
14 |   angles
15 | 
16 |   message_generation
17 | )
18 | 
19 | add_message_files(FILES gpsPosition.msg)
20 | generate_messages()
21 | 
22 | 
23 | catkin_package(
24 |   CATKIN_DEPENDS message_runtime
25 | )
26 | 
27 | 
28 | include_directories(
29 |   ${catkin_INCLUDE_DIRS}
30 | )
31 | 
32 | 
33 | add_executable(px4ros src/px4_getpoint_node.cpp)
34 | 
35 | 
36 | target_link_libraries(px4ros
37 |    ${catkin_LIBRARIES}
38 |  )
39 | 
40 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/msg/gpsPosition.msg:
--------------------------------------------------------------------------------
1 | float64 lat
2 | float64 lon
3 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/msg/gpsPosition.msg~:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PenguinCheng/multiDrone-ROS/73df4bb83e1091565331f9467f779e4a3ad9bd95/multiDrone-ROS/px4ros/msg/gpsPosition.msg~


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>px4ros</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The px4ros package</description>
 6 | 
 7 | 
 8 |   <maintainer email="odroid@todo.todo">odroid</maintainer>
 9 |   <license>TODO</license>
10 | 
11 |   <buildtool_depend>catkin</buildtool_depend>
12 |   <build_depend>roscpp</build_depend>
13 |   <build_depend>rospy</build_depend>
14 |   <build_depend>std_msgs</build_depend>
15 | 
16 |   <build_depend>message_generation</build_depend>
17 |   <run_depend>roscpp</run_depend>
18 |   <run_depend>rospy</run_depend>
19 |   <run_depend>std_msgs</run_depend>
20 | 
21 |   <run_depend>message_runtime</run_depend>
22 | 
23 | 
24 |   <export>
25 | 
26 |   </export>
27 | </package>
28 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/package.xml~:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>px4ros</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The px4ros package</description>
 6 | 
 7 | 
 8 |   <maintainer email="odroid@todo.todo">odroid</maintainer>
 9 |   <license>TODO</license>
10 | 
11 |   <buildtool_depend>catkin</buildtool_depend>
12 |   <build_depend>roscpp</build_depend>
13 |   <build_depend>rospy</build_depend>
14 |   <build_depend>std_msgs</build_depend>
15 | 
16 |   <build_depend>message_generation</build_depend>
17 |   <run_depend>roscpp</run_depend>
18 |   <run_depend>rospy</run_depend>
19 |   <run_depend>std_msgs</run_depend>
20 | 
21 |   <run_depend>message_runtime</run_depend>
22 | 
23 | 
24 |   <export>
25 | 
26 |   </export>
27 | </package>
28 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/follower_control_decode.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | import rospy
  3 | from std_msgs.msg import String
  4 | from px4ros.msg import gpsPosition
  5 | 
  6 | def decode_node():
  7 | 	rospy.init_node('px4ros_leader_control_decode', anonymous=True)
  8 | 	pub = rospy.Publisher('neighbour_position', gpsPosition, queue_size=20)
  9 | 	pub2 = rospy.Publisher('neighbour_position_2', gpsPosition, queue_size=20)
 10 | 	rate = rospy.Rate(10) # 10hz
 11 | 
 12 | 	rospy.Subscriber("serial_data_received", String, decodeInfo)
 13 | 	
 14 | 	position = gpsPosition()	
 15 | 	position2 = gpsPosition()
 16 | 	while not rospy.is_shutdown():
 17 | 		position.lat = neighbour_one_lat
 18 | 		position.lon = neighbour_one_lon
 19 | 		pub.publish(position)
 20 | 		rospy.loginfo("1:%s",position)
 21 | 
 22 | 		position2.lat = neighbour_two_lat
 23 | 		position2.lon = neighbour_two_lon
 24 | 		pub.publish(position2)
 25 | 		rospy.loginfo("2:%s",position2)
 26 | 
 27 | 		rate.sleep()
 28 | 
 29 | '''callback function'''
 30 | def decodeInfo(data):
 31 | 	global neighbour_one_lon
 32 | 	global neighbour_one_lat
 33 | 	global last_neighbour_one_lon
 34 | 	global last_neighbour_one_lat
 35 | 	global neighbour_two_lon
 36 | 	global neighbour_two_lat
 37 | 	global last_neighbour_two_lon
 38 | 	global last_neighbour_two_lat
 39 | 
 40 | 	if data.data:
 41 | 		latStr = ""
 42 | 		lonStr = ""
 43 | 		latStr2 = ""
 44 | 		lonStr2 = ""
 45 | 		i = 3
 46 | 		if data.data[0:3] == "x1o":        #receive the data of leader 1 and leader 2
 47 | 			while i<len(data.data):
 48 | 				if data.data[i] == "a":
 49 | 					break
 50 | 				lonStr += data.data[i]
 51 | 				i += 1
 52 | 			i += 1
 53 | 			while i<len(data.data):
 54 | 				if data.data[i] == "x":
 55 | 					break
 56 | 				latStr += data.data[i]
 57 | 				i += 1
 58 | 			#i +=1
 59 | 			if data.data[i:i+3] == "x2o":
 60 | 				i += 3  #skip x2o
 61 | 				while i<len(data.data):
 62 | 					if data.data[i] == "a":
 63 | 						break
 64 | 					lonStr2 += data.data[i]
 65 | 					i += 1
 66 | 				i += 1
 67 | 				while i<len(data.data):
 68 | 					if data.data[i] == "\n":
 69 | 						break
 70 | 					latStr2 += data.data[i]
 71 | 					i += 1
 72 | 
 73 | 			try:
 74 | 				neighbour_one_lon = float(lonStr)
 75 | 				neighbour_one_lat = float(latStr)
 76 | 
 77 | 				last_neighbour_one_lon = neighbour_one_lon
 78 | 				last_neighbour_one_lat = neighbour_one_lat
 79 | 
 80 | 				neighbour_two_lon = float(lonStr2)
 81 | 				neighbour_two_lat = float(latStr2)
 82 | 
 83 | 				last_neighbour_two_lon = neighbour_two_lon
 84 | 				last_neighbour_two_lat = neighbour_two_lat
 85 | 			except Exception, e:
 86 | 				print "decode exception:", e
 87 | 				neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
 88 | 				neighbour_one_lat = last_neighbour_one_lat
 89 | 				neighbour_two_lon = last_neighbour_two_lon    #maybe 0.0
 90 | 				neighbour_two_lat = last_neighbour_two_lat
 91 | 		elif data.data[0:3] == "x2o":        #receive the data of leader 1 and leader 2
 92 | 			while i<len(data.data):
 93 | 				if data.data[i] == "a":
 94 | 					break
 95 | 				lonStr += data.data[i]
 96 | 				i += 1
 97 | 			i += 1
 98 | 			while i<len(data.data):
 99 | 				if data.data[i] == "x":
100 | 					break
101 | 				latStr += data.data[i]
102 | 				i += 1
103 | 			i +=1
104 | 			if data.data[i:i+3] == "x1o":
105 | 				i += 3  #skip x2o
106 | 				while i<len(data.data):
107 | 					if data.data[i] == "a":
108 | 						break
109 | 					lonStr2 += data.data[i]
110 | 					i += 1
111 | 				i += 1
112 | 				while i<len(data.data):
113 | 					if data.data[i] == "\n":
114 | 						break
115 | 					latStr2 += data.data[i]
116 | 					i += 1
117 | 
118 | 			try:
119 | 				neighbour_one_lon = float(lonStr)
120 | 				neighbour_one_lat = float(latStr)
121 | 
122 | 				last_neighbour_one_lon = neighbour_one_lon
123 | 				last_neighbour_one_lat = neighbour_one_lat
124 | 
125 | 				neighbour_two_lon = float(lonStr2)
126 | 				neighbour_two_lat = float(latStr2)
127 | 
128 | 				last_neighbour_two_lon = neighbour_two_lon
129 | 				last_neighbour_two_lat = neighbour_two_lat
130 | 			except Exception, e:
131 | 				print "decode exception:", e
132 | 				neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
133 | 				neighbour_one_lat = last_neighbour_one_lat
134 | 				neighbour_two_lon = last_neighbour_two_lon    #maybe 0.0
135 | 				neighbour_two_lat = last_neighbour_two_lat
136 | 	
137 | 	else:
138 | 		neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
139 | 		neighbour_one_lat = last_neighbour_one_lat
140 | 		neighbour_two_lon = last_neighbour_two_lon    #maybe 0.0
141 | 		neighbour_two_lat = last_neighbour_two_lat
142 | 		print "not received neighbour information!"
143 | 	
144 | if __name__ == '__main__':
145 | 	try:
146 | 		neighbour_one_lat = 0.0
147 | 		neighbour_one_lon = 0.0
148 | 		last_neighbour_one_lon = 0.0
149 | 		last_neighbour_one_lat = 0.0
150 | 	
151 | 		neighbour_two_lat = 0.0
152 | 		neighbour_two_lon = 0.0
153 | 		last_neighbour_two_lon = 0.0
154 | 		last_neighbour_two_lat = 0.0
155 | 	
156 | 		decode_node()
157 | 	except rospy.ROSInterruptException:
158 | 		pass
159 | 
160 | 
161 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/follower_control_decode.py~:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | import rospy
  3 | from std_msgs.msg import String
  4 | from px4ros.msg import gpsPosition
  5 | 
  6 | def decode_node():
  7 | 	rospy.init_node('px4ros_leader_control_decode', anonymous=True)
  8 | 	pub = rospy.Publisher('neighbour_position', gpsPosition, queue_size=20)
  9 | 	pub2 = rospy.Publisher('neighbour_position_2', gpsPosition2, queue_size=20)
 10 | 	rate = rospy.Rate(10) # 10hz
 11 | 
 12 | 	rospy.Subscriber("serial_data_received", String, decodeInfo)
 13 | 	
 14 | 	position = gpsPosition()	
 15 | 	position2 = gpsPosition()
 16 | 	while not rospy.is_shutdown():
 17 | 		position.lat = neighbour_one_lat
 18 | 		position.lon = neighbour_one_lon
 19 | 		pub.publish(position)
 20 | 		rospy.loginfo("%s",position)
 21 | 
 22 | 		position1.lat = neighbour_two_lat
 23 | 		position1.lon = neighbour_two_lon
 24 | 		pub.publish(position2)
 25 | 		rospy.loginfo("%s",position1)
 26 | 
 27 | 		rate.sleep()
 28 | 
 29 | '''callback function'''
 30 | def decodeInfo(data):
 31 | 	global neighbour_one_lon
 32 | 	global neighbour_one_lat
 33 | 	global last_neighbour_one_lon
 34 | 	global last_neighbour_one_lat
 35 | 	global neighbour_two_lon
 36 | 	global neighbour_two_lat
 37 | 	global last_neighbour_two_lon
 38 | 	global last_neighbour_two_lat
 39 | 
 40 | 	if data.data:
 41 | 		latStr = ""
 42 | 		lonStr = ""
 43 | 		i = 3
 44 | 		if data.data[0:2] == "01o":        #receive the data of leader 1
 45 | 			while i<len(data.data):
 46 | 				if data.data[i] == "a":
 47 | 					break
 48 | 				lonStr += data.data[i]
 49 | 				i += 1
 50 | 			i += 1
 51 | 			while i<len(data.data):
 52 | 				if data.data[i] == "\n":
 53 | 					break
 54 | 				latStr += data.data[i]
 55 | 				i += 1
 56 | 
 57 | 		try:
 58 | 			neighbour_one_lon = float(lonStr)
 59 | 			neighbour_one_lat = float(latStr)
 60 | 
 61 | 			last_neighbour_one_lon = neighbour_one_lon
 62 | 			last_neighbour_one_lat = neighbour_one_lat
 63 | 		except Exception, e:
 64 | 			print "decode exception:", e
 65 | 			neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
 66 | 			neighbour_one_lat = last_neighbour_one_lat
 67 | 		
 68 | 		latStr = ""
 69 | 		lonStr = ""
 70 | 		i = 3
 71 | 		if data.data[0:2] == "02o":        #receive the data of leader 1
 72 | 			while i<len(data.data):
 73 | 				if data.data[i] == "a":
 74 | 					break
 75 | 				lonStr += data.data[i]
 76 | 				i += 1
 77 | 			i += 1
 78 | 			while i<len(data.data):
 79 | 				if data.data[i] == "\n":
 80 | 					break
 81 | 				latStr += data.data[i]
 82 | 				i += 1
 83 | 
 84 | 		try:
 85 | 			neighbour_two_lon = float(lonStr)
 86 | 			neighbour_two_lat = float(latStr)
 87 | 
 88 | 			last_neighbour_two_lon = neighbour_two_lon
 89 | 			last_neighbour_two_lat = neighbour_two_lat
 90 | 		except Exception, e:
 91 | 			print "decode exception:", e
 92 | 			neighbour_two_lon = last_neighbour_two_lon    #maybe 0.0
 93 | 			neighbour_two_lat = last_neighbour_two_lat
 94 | 	else:
 95 | 		neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
 96 | 		neighbour_one_lat = last_neighbour_one_lat
 97 | 		neighbour_two_lon = last_neighbour_two_lon    #maybe 0.0
 98 | 		neighbour_two_lat = last_neighbour_two_lat
 99 | 		print "not received neighbour information!"
100 | 	
101 | if __name__ == '__main__':
102 | 	try:
103 | 		neighbour_one_lat = 0.0
104 | 		neighbour_one_lon = 0.0
105 | 		last_neighbour_one_lon = 0.0
106 | 		last_neighbour_one_lat = 0.0
107 | 	
108 | 		neighbour_two_lat = 0.0
109 | 		neighbour_two_lon = 0.0
110 | 		last_neighbour_two_lon = 0.0
111 | 		last_neighbour_two_lat = 0.0
112 | 	
113 | 		decode_node()
114 | 	except rospy.ROSInterruptException:
115 | 		pass
116 | 
117 | 
118 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/leader_control_decode.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | import rospy
 3 | from std_msgs.msg import String
 4 | from px4ros.msg import gpsPosition
 5 | 
 6 | def decode_node():
 7 | 	rospy.init_node('px4ros_leader_control_decode', anonymous=True)
 8 | 	pub = rospy.Publisher('neighbour_position', gpsPosition, queue_size=20)
 9 | 	rate = rospy.Rate(10) # 10hz
10 | 
11 | 	rospy.Subscriber("serial_data_received", String, decodeInfo)
12 | 	
13 | 	position = gpsPosition()	
14 | 	while not rospy.is_shutdown():
15 | 		position.lat = neighbour_one_lat
16 | 		position.lon = neighbour_one_lon
17 | 		pub.publish(position)
18 | 		rospy.loginfo("%s",position)
19 | 		rate.sleep()
20 | 
21 | '''callback function'''
22 | def decodeInfo(data):
23 | 	global neighbour_one_lon
24 | 	global neighbour_one_lat
25 | 	global last_neighbour_one_lon
26 | 	global last_neighbour_one_lat
27 | 
28 | 	if data.data:
29 | 		latStr = ""
30 | 		lonStr = ""
31 | 		i = 3
32 | 		if data.data[0:3] == "x2o":        #for leader1, to receive the data of leader 2
33 | 			while i<len(data.data):
34 | 				if data.data[i] == "a":
35 | 					break
36 | 				lonStr += data.data[i]
37 | 				i += 1
38 | 			i += 1
39 | 			while i<len(data.data):
40 | 				if data.data[i] == "\n":
41 | 					break
42 | 				latStr += data.data[i]
43 | 				i += 1
44 | 		try:
45 | 			neighbour_one_lon = float(lonStr)
46 | 			neighbour_one_lat = float(latStr)
47 | 
48 | 			last_neighbour_one_lon = neighbour_one_lon
49 | 			last_neighbour_one_lat = neighbour_one_lat
50 | 		except Exception, e:
51 | 			print "decode exception:", e
52 | 			neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
53 | 			neighbour_one_lat = last_neighbour_one_lat
54 | 	else:
55 | 		neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
56 | 		neighbour_one_lat = last_neighbour_one_lat
57 | 		print "not received neighbour information!"
58 | 	
59 | if __name__ == '__main__':
60 | 	try:
61 | 		neighbour_one_lat = 0.0
62 | 		neighbour_one_lon = 0.0
63 | 		last_neighbour_one_lon = 0.0
64 | 		last_neighbour_one_lat = 0.0
65 | 	
66 | 		decode_node()
67 | 	except rospy.ROSInterruptException:
68 | 		pass
69 | 
70 | 
71 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/leader_control_decode.py~:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | import rospy
 3 | from std_msgs.msg import String
 4 | from px4ros.msg import gpsPosition
 5 | 
 6 | def decode_node():
 7 | 	rospy.init_node('px4ros_leader_control_decode', anonymous=True)
 8 | 	pub = rospy.Publisher('neighbour_position', gpsPosition, queue_size=20)
 9 | 	rate = rospy.Rate(10) # 10hz
10 | 
11 | 	rospy.Subscriber("serial_data_received", String, decodeInfo)
12 | 	
13 | 	position = gpsPosition()	
14 | 	while not rospy.is_shutdown():
15 | 		position.lat = neighbour_one_lat
16 | 		position.lon = neighbour_one_lon
17 | 		pub.publish(position)
18 | 		rospy.loginfo("%s",position)
19 | 		rate.sleep()
20 | 
21 | '''callback function'''
22 | def decodeInfo(data):
23 | 	global neighbour_one_lon
24 | 	global neighbour_one_lat
25 | 	global last_neighbour_one_lon
26 | 	global last_neighbour_one_lat
27 | 
28 | 	if data.data:
29 | 		latStr = ""
30 | 		lonStr = ""
31 | 		i = 3
32 | 		if data.data[0:3] == "x2o":        #for leader1, to receive the data of leader 2
33 | 			while i<len(data.data):
34 | 				if data.data[i] == "a":
35 | 					break
36 | 				lonStr += data.data[i]
37 | 				i += 1
38 | 			i += 1
39 | 			while i<len(data.data):
40 | 				if data.data[i] == "\n":
41 | 					break
42 | 				latStr += data.data[i]
43 | 				i += 1
44 | 		try:
45 | 			neighbour_one_lon = float(lonStr)
46 | 			neighbour_one_lat = float(latStr)
47 | 
48 | 			last_neighbour_one_lon = neighbour_one_lon
49 | 			last_neighbour_one_lat = neighbour_one_lat
50 | 		except Exception, e:
51 | 			print "decode exception:", e
52 | 			neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
53 | 			neighbour_one_lat = last_neighbour_one_lat
54 | 	else:
55 | 		neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
56 | 		neighbour_one_lat = last_neighbour_one_lat
57 | 		print "not received neighbour information!"
58 | 	
59 | if __name__ == '__main__':
60 | 	try:
61 | 		neighbour_one_lat = 0.0
62 | 		neighbour_one_lon = 0.0
63 | 		last_neighbour_one_lon = 0.0
64 | 		last_neighbour_one_lat = 0.0
65 | 	
66 | 		decode_node()
67 | 	except rospy.ROSInterruptException:
68 | 		pass
69 | 
70 | 
71 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/leader_controller.py~:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | import rospy
 3 | from std_msgs.msg import String
 4 | 
 5 | def main():
 6 | 	rospy.init_node('px4ros_leader_control', anonymous=True)
 7 | 	pub = rospy.Publisher('neighbour_position', String, queue_size=20)
 8 | 	rate = rospy.Rate(10) # 10hz
 9 | 
10 | 	rospy.Subscriber("serial_data_received", String, decodeInfo)
11 | 
12 | #callback
13 | def decodeInfo(data):
14 | 	global neighbour_one_lon
15 | 	global neighbour_one_lat
16 | 	global last_neighbour_one_lon
17 | 	global last_neighbour_one_lat
18 | 
19 | 	if data.data:
20 | 		latStr = ""
21 | 		lonStr = ""
22 | 		i = 1
23 | 		if data.data[0] == "o":
24 | 			while i<len(data.data):
25 | 				if data.data[i] == "a":
26 | 					break
27 | 				lonStr += data.data[i]
28 | 				i += 1
29 | 			i += 1
30 | 			while i<len(data.data):
31 | 				if data.data[i] == "\n":
32 | 					break
33 | 				latStr += data.data[i]
34 | 				i += 1
35 | 
36 | 		try:
37 | 			neighbour_one_lon = float(lonStr)
38 | 			neighbour_one_lat = float(latStr)
39 | 
40 | 			last_neighbour_one_lon = neighbour_one_lon
41 | 			last_neighbour_one_lat = neighbour_one_lat
42 | 		except Exception, e:
43 | 			print "decode exception:", e
44 | 			neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
45 | 			neighbour_one_lat = last_neighbour_one_lat
46 | 	else:
47 | 		neighbour_one_lon = last_neighbour_one_lon    #maybe 0.0
48 | 		neighbour_one_lat = last_neighbour_one_lat
49 | 	
50 | if __name__ == '__main__':
51 | 	neighbour_one_lat = 0.0
52 | 	neighbour_one_lon = 0.0
53 | 	last_neighbour_one_lon = 0.0
54 | 	last_neighbour_one_lat = 0.0
55 | 	
56 | 	main()
57 | 
58 | 	print "neighbour lon:", neighbour_one_lon
59 | 	print "neighbour lat:", neighbour_one_lat
60 | 
61 | 
62 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/main_control.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | import rospy
 3 | from std_msgs.msg import String
 4 | from sensor_msgs.msg import NavSatFix
 5 | 
 6 | #custom msg
 7 | from px4ros.msg import gpsPosition
 8 | 
 9 | '''callback function'''
10 | def callback1(data):
11 | 	rospy.loginfo("I heard cb1 %s",data.data) 
12 | 	global self_position
13 | 	self_position = data.data
14 | 
15 | def callback2(data):
16 | 	rospy.loginfo("I heard cb2 %s",data) 
17 | 	global neighbour_position
18 | 	neighbour_position = data
19 | 
20 | def control():
21 | 	pass
22 | 
23 | def commuciation_check():
24 | 	pass
25 | 
26 | def takeoff(height):
27 | 	pass
28 | 
29 | if __name__ == '__main__':
30 | 	self_position = NavSatFix()
31 | 	neighbour_position = gpsPosition()
32 | 	
33 | 	rospy.init_node('px4ros_leader_control', anonymous=True)
34 | 	pub = rospy.Publisher('serial_data_send', String, queue_size=20)
35 | 	rate = rospy.Rate(10) # 10hz
36 | 
37 | 	#subscribe info from mavros
38 | 	rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, callback1)
39 | 	#subscribe info from leader_control_decode
40 | 	rospy.Subscriber("neighbour_position", gpsPosition, callback2)
41 | 
42 | 	while not rospy.is_shutdown():
43 | 		
44 | 		#pub.publish(globalPosition)
45 | 		#rospy.loginfo("%s",globalPosition)
46 | 		rate.sleep()
47 | 
48 | 	#while !commuciation_check():
49 | 	#	print "Waiting for connection!"
50 | 	
51 | 	#print "Commucate ok! Read to fly!"
52 | 	#takeoff(3)
53 | 
54 | 	#control()
55 | 
56 | 	
57 | 
58 | 	
59 | 	
60 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/main_control.py~:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | import rospy
 3 | from std_msgs.msg import String
 4 | from sensor_msgs.msg import NavSatFix
 5 | 
 6 | #custom msg
 7 | from px4ros.msg import gpsPosition
 8 | 
 9 | '''callback function'''
10 | def callback1(data):
11 | 	rospy.loginfo("I heard cb1 %s",data.data) 
12 | 	global self_position
13 | 	self_position = data.data
14 | 
15 | def callback2(data):
16 | 	rospy.loginfo("I heard cb2 %s",data) 
17 | 	global neighbour_position
18 | 	neighbour_position = data
19 | 
20 | def control():
21 | 	pass
22 | 
23 | def commuciation_check():
24 | 	pass
25 | 
26 | def takeoff(height):
27 | 	pass
28 | 
29 | if __name__ == '__main__':
30 | 	self_position = NavSatFix()
31 | 	neighbour_position = gpsPosition()
32 | 	
33 | 	rospy.init_node('px4ros_leader_control', anonymous=True)
34 | 	#pub = rospy.Publisher('serial_data_send', gpsPosition, queue_size=20)
35 | 	rate = rospy.Rate(10) # 10hz
36 | 
37 | 	#subscribe info from mavros
38 | 	rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, callback1)
39 | 	#subscribe info from leader_control_decode
40 | 	rospy.Subscriber("neighbour_position", gpsPosition, callback2)
41 | 
42 | 	while not rospy.is_shutdown():
43 | 		
44 | 		#pub.publish(globalPosition)
45 | 		#rospy.loginfo("%s",globalPosition)
46 | 		rate.sleep()
47 | 
48 | 	#while !commuciation_check():
49 | 	#	print "Waiting for connection!"
50 | 	
51 | 	#print "Commucate ok! Read to fly!"
52 | 	#takeoff(3)
53 | 
54 | 	#control()
55 | 
56 | 	
57 | 
58 | 	
59 | 	
60 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/serial_ros.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | import rospy
 3 | from std_msgs.msg import String
 4 | import serial
 5 | import time
 6 | 
 7 | #function: record current time(s)
 8 | current_milli_time = lambda: int(time.time()*1000)
 9 | 
10 | def serial_node():
11 | 
12 | 	pub = rospy.Publisher('serial_data_received', String, queue_size=20)
13 | 	rospy.init_node('px4ros_serial', anonymous=True)
14 | 	rate = rospy.Rate(10) # 10hz
15 | 
16 | 	rospy.Subscriber("serial_data_send", String, callback)
17 | 	
18 | 	opm_string = ''
19 | 	to_send_string = ''
20 | 	
21 | 	while not rospy.is_shutdown():	
22 | 		#hello_str = "hello world %s" % rospy.get_time()
23 | 
24 | 		opm_string = myserial.read(myserial.inWaiting())
25 | 		#if len(opm_string) >= 6:
26 | 		#	to_send_string = opm_string[5:len(opm_string)-2] 
27 | 		if len(opm_string) > 0:
28 | 			to_send_string = opm_string 
29 | 		else:
30 | 			to_send_string = ''
31 | 
32 | 		#print to_send_string
33 | 		rospy.loginfo("["+"%s"+"]  "+"%s",current_milli_time(),to_send_string)
34 | 		pub.publish(to_send_string)
35 | 		rate.sleep()
36 | 
37 | '''callback function'''
38 | def callback(data):
39 | 	rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
40 | 	myserial.write(data.data)
41 | 
42 | 
43 | if __name__ == '__main__':
44 | 	myserial = serial.Serial('/dev/ttyUSB0',115200,timeout=None)
45 | 	print myserial.portstr
46 | 	try:
47 | 		serial_node()
48 | 	except rospy.ROSInterruptException:
49 | 		pass
50 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/scripts/serial_ros.py~:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | import rospy
 3 | from std_msgs.msg import String
 4 | import serial
 5 | import time
 6 | 
 7 | #function: record current time(s)
 8 | current_milli_time = lambda: int(time.time()*1000)
 9 | 
10 | def serial_node():
11 | 
12 | 	pub = rospy.Publisher('serial_data_received', String, queue_size=20)
13 | 	rospy.init_node('px4ros_serial', anonymous=True)
14 | 	rate = rospy.Rate(10) # 10hz
15 | 
16 | 	rospy.Subscriber("serial_data_send", String, callback)
17 | 	
18 | 	opm_string = ''
19 | 	to_send_string = ''
20 | 	
21 | 	while not rospy.is_shutdown():	
22 | 		#hello_str = "hello world %s" % rospy.get_time()
23 | 
24 | 		opm_string = myserial.read(myserial.inWaiting())
25 | 		#if len(opm_string) >= 6:
26 | 		#	to_send_string = opm_string[5:len(opm_string)-2] 
27 | 		if len(opm_string) > 0:
28 | 			to_send_string = opm_string 
29 | 		else:
30 | 			to_send_string = ''
31 | 
32 | 		#print to_send_string
33 | 		rospy.loginfo("["+"%s"+"]  "+"%s",current_milli_time(),to_send_string)
34 | 		pub.publish(to_send_string)
35 | 		rate.sleep()
36 | 
37 | '''callback function'''
38 | def callback(data):
39 | 	rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
40 | 	#myserial.write(data.data)
41 | 
42 | 
43 | if __name__ == '__main__':
44 | 	myserial = serial.Serial('/dev/ttyUSB0',115200,timeout=None)
45 | 	print myserial.portstr
46 | 	try:
47 | 		serial_node()
48 | 	except rospy.ROSInterruptException:
49 | 		pass
50 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/src/px4_getpoint_node.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | #include <geometry_msgs/Point.h>
  3 | #include <geometry_msgs/PoseStamped.h>
  4 | #include <geometry_msgs/TwistStamped.h>
  5 | #include <sensor_msgs/NavSatFix.h>
  6 | #include <keyboard/Key.h>
  7 | #include <math.h>
  8 | #include <angles/angles.h>
  9 | #include <std_msgs/String.h>
 10 | #include <sstream>
 11 | 
 12 | //set velocity
 13 | ros::Publisher vel_sp_pub;
 14 | ros::Publisher gps_pub;
 15 | //current position 
 16 | // next position
 17 | geometry_msgs::PoseStamped currentPos,nextPos,initCirclePos;
 18 | geometry_msgs::TwistStamped vs;
 19 | sensor_msgs::NavSatFix currentGlobalPos;
 20 | bool isGetpoint, isGetGPSPoint;
 21 | float speed;
 22 | //radius
 23 | int R = 6371000;
 24 | 
 25 | // go to specific point 
 26 | void getPoint(double x, double y,double z){
 27 |     bool reached = false;
 28 |     double dis = sqrt((currentPos.pose.position.x-x)*(currentPos.pose.position.x-x) +(currentPos.pose.position.y-y)*(currentPos.pose.position.y-y)  + (currentPos.pose.position.z-z)*(currentPos.pose.position.z-z));
 29 |     double thresh = 0.5;
 30 |     if(dis < thresh){
 31 |     	reached = true;
 32 |     	isGetpoint =false;
 33 |     	ROS_INFO_STREAM("get There!");
 34 |     }
 35 | 
 36 |     geometry_msgs::Point velocityV;
 37 |     velocityV.x = x - currentPos.pose.position.x;
 38 |     velocityV.y = y - currentPos.pose.position.y;
 39 |     velocityV.z = z - currentPos.pose.position.z;
 40 | 
 41 |     //set velocity
 42 |     vs.twist.linear.x = velocityV.x*0.2;
 43 |     vs.twist.linear.y = velocityV.y*0.2;
 44 |     vs.twist.linear.z = velocityV.z*0.2;
 45 |     // ROS_INFO_STREAM("getPoint1");
 46 |     vs.header.stamp = ros::Time::now();
 47 |     vel_sp_pub.publish(vs);
 48 | }
 49 | 
 50 | //GPS point
 51 | void gotoGPSPoint(double lat, double lon){
 52 |     double delta_x = R * (lat - currentGlobalPos.latitude) * float(3.1415926535898 / 180);
 53 |     double delta_y = R * (lon - currentGlobalPos.longitude) * float(3.1415926535898 / 180);
 54 |     vs.twist.linear.x = delta_x*0.1;
 55 |     vs.twist.linear.y = -delta_y*0.1;
 56 |     vs.header.stamp = ros::Time::now();
 57 |     vel_sp_pub.publish(vs);
 58 | }
 59 | 
 60 | //received local position
 61 | void localPositionReceived(const geometry_msgs::PoseStampedConstPtr& msg){
 62 |     currentPos = *msg;
 63 | }
 64 | 
 65 | //
 66 | void serialDataReceived(const std_msgs::String::ConstPtr& msg){
 67 | 	ROS_INFO("I heard:[%s]",msg->data.c_str());
 68 | }
 69 | 
 70 | void globalPositionReceived(const sensor_msgs::NavSatFixConstPtr& msg){
 71 |    currentGlobalPos = *msg;
 72 | }
 73 | 
 74 | //keyboard command 
 75 | void sendCommand(const keyboard::Key &key){
 76 |    switch (key.code)
 77 |    {
 78 |    		case 'w':
 79 |    		{
 80 |    			ROS_INFO_STREAM("up");
 81 |    			vs.twist.linear.z += 0.1;
 82 |    			break;
 83 |    		}
 84 |       case 's':
 85 |       {
 86 |         ROS_INFO_STREAM("down");
 87 |         vs.twist.linear.z -= 0.1;
 88 |         break;
 89 |       }
 90 |    		case 'j':
 91 |    		{
 92 |    			ROS_INFO_STREAM("left");
 93 |    			vs.twist.linear.y += 0.1;
 94 |    			break;
 95 |    		}
 96 |    		case 'l':
 97 |    		{
 98 |    			ROS_INFO_STREAM("right");
 99 |    			vs.twist.linear.y -= 0.1;
100 |    			break;
101 |    		}
102 |       case 'i':
103 |       {
104 |         ROS_INFO_STREAM("forward");
105 |         vs.twist.linear.x += 0.1;
106 |         break;
107 |       }
108 |       case 'k':
109 |       {
110 |         ROS_INFO_STREAM("backward");
111 |         vs.twist.linear.x -= 0.1;
112 |         break;
113 |       }
114 |       case 'u':
115 |       {
116 |         ROS_INFO_STREAM("rotate left");
117 |         vs.twist.angular.z += 0.1;
118 |         break;
119 |       }
120 |       case 'o':
121 |       {
122 |         ROS_INFO_STREAM("rotate right");
123 |         vs.twist.angular.z -= 0.1;
124 |         break;
125 |       }
126 |       case 'p':
127 |       {
128 |       	isGetpoint = true;
129 |       	isGetGPSPoint = false;
130 |       	ROS_INFO_STREAM("get to point 10 10 3");
131 |               break;
132 |       }
133 |       case 'g':
134 |       {
135 |       	isGetGPSPoint = true;
136 |       	isGetpoint = false;
137 |       	ROS_INFO_STREAM("get to GPS point");
138 |       	break;
139 |       }
140 |       case 'h':
141 |       {
142 |         // turn to manual mode
143 |         isGetpoint = false;
144 |         isGetGPSPoint =false;
145 |         vs.twist.linear.x = 0;
146 |         vs.twist.linear.y = 0;
147 |         vs.twist.linear.z = 0;
148 |         vs.twist.angular.z = 0;
149 |         ROS_INFO_STREAM("Manual Mode");
150 |         break;
151 |       }
152 |       
153 |       case 'a':
154 |       {
155 |         speed += 0.1;
156 |         ROS_INFO_STREAM("speed up" << speed);
157 |         break;
158 |       }
159 |       case 'd':
160 |       {
161 |         speed -= 0.1;
162 |         ROS_INFO_STREAM("speed down" << speed);
163 |         break;
164 |       }
165 | 
166 |    		
167 |    		default:
168 |    		{
169 |    			
170 |    		}
171 | 
172 |    }
173 | }
174 | 
175 | int main(int argc, char **argv){
176 |   ros::init(argc, argv, "px4_offboard_velocity_control_node");
177 |   ros::NodeHandle nodeHandle;
178 |   //publish set_velocity topic 100ms
179 |   vel_sp_pub = nodeHandle.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);
180 |   ros::Subscriber commandSubscriber = nodeHandle.subscribe("/keyboard/keydown",1,sendCommand);
181 |   ros::Subscriber localPositionSubsciber = nodeHandle.subscribe("/mavros/local_position/local", 10, localPositionReceived);
182 |   ros::Subscriber globalPositionSubsciber =  nodeHandle.subscribe("/mavros/global_position/raw/fix", 10, globalPositionReceived);
183 | 
184 |   gps_pub = nodeHandle.advertise<std_msgs::String>("serial_data_send", 10);
185 |   ros::Subscriber serialSubscriber = nodeHandle.subscribe("serial_data_received", 10, serialDataReceived);
186 | 
187 |   isGetpoint = false;
188 |   isGetGPSPoint = false;
189 |   speed = 0.2;
190 | //ros rate 
191 |   ros::Rate loop_rate(10.0);
192 | 
193 |   while(ros::ok())
194 |   {
195 | 
196 |       vs.header.seq++;
197 |       //GPS topic 
198 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.longitude);
199 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.latitude);
200 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.altitude);
201 | 
202 |       if((!isGetpoint) && (!isGetGPSPoint)){
203 | 
204 |         vs.header.stamp = ros::Time::now();
205 |         //ROS_INFO_STREAM("send ps" << ps);
206 |         vel_sp_pub.publish(vs);
207 |       }else if(isGetGPSPoint) {
208 |          gotoGPSPoint(55.7530147,37.6273584);
209 |          ROS_INFO_STREAM("GPS");
210 |       }else if(isGetpoint){
211 |           getPoint(10,10,3);
212 |           ROS_INFO_STREAM("local position");
213 |       }
214 | 
215 |       std_msgs::String gps_location;
216 |       std::stringstream ss;
217 |       ss << "o120.11111111a30.3333333333#";
218 |       gps_location.data = ss.str();
219 |       gps_pub.publish(gps_location);      
220 | 		
221 |     ros::spinOnce();
222 | 
223 |     loop_rate.sleep();
224 |   }
225 | }
226 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/src/px4_getpoint_node.cpp~:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | #include <geometry_msgs/Point.h>
  3 | #include <geometry_msgs/PoseStamped.h>
  4 | #include <geometry_msgs/TwistStamped.h>
  5 | #include <sensor_msgs/NavSatFix.h>
  6 | #include <keyboard/Key.h>
  7 | #include <math.h>
  8 | #include <angles/angles.h>
  9 | #include <std_msgs/String.h>
 10 | #include <sstream>
 11 | #include <px4Ros/gpsPosition.h>
 12 | 
 13 | //set velocity
 14 | ros::Publisher vel_sp_pub;
 15 | ros::Publisher gps_pub;
 16 | //current position 
 17 | // next position
 18 | geometry_msgs::PoseStamped currentPos,nextPos,initCirclePos;
 19 | geometry_msgs::TwistStamped vs;
 20 | sensor_msgs::NavSatFix currentGlobalPos;
 21 | bool isGetpoint, isGetGPSPoint;
 22 | float speed;
 23 | //radius
 24 | int R = 6371000;
 25 | 
 26 | // go to specific point 
 27 | void getPoint(double x, double y,double z){
 28 |     bool reached = false;
 29 |     double dis = sqrt((currentPos.pose.position.x-x)*(currentPos.pose.position.x-x) +(currentPos.pose.position.y-y)*(currentPos.pose.position.y-y)  + (currentPos.pose.position.z-z)*(currentPos.pose.position.z-z));
 30 |     double thresh = 0.5;
 31 |     if(dis < thresh){
 32 |     	reached = true;
 33 |     	isGetpoint =false;
 34 |     	ROS_INFO_STREAM("get There!");
 35 |     }
 36 | 
 37 |     geometry_msgs::Point velocityV;
 38 |     velocityV.x = x - currentPos.pose.position.x;
 39 |     velocityV.y = y - currentPos.pose.position.y;
 40 |     velocityV.z = z - currentPos.pose.position.z;
 41 | 
 42 |     //set velocity
 43 |     vs.twist.linear.x = velocityV.x*0.2;
 44 |     vs.twist.linear.y = velocityV.y*0.2;
 45 |     vs.twist.linear.z = velocityV.z*0.2;
 46 |     // ROS_INFO_STREAM("getPoint1");
 47 |     vs.header.stamp = ros::Time::now();
 48 |     vel_sp_pub.publish(vs);
 49 | }
 50 | 
 51 | //GPS point
 52 | void gotoGPSPoint(double lat, double lon){
 53 |     double delta_x = R * (lat - currentGlobalPos.latitude) * float(3.1415926535898 / 180);
 54 |     double delta_y = R * (lon - currentGlobalPos.longitude) * float(3.1415926535898 / 180);
 55 |     vs.twist.linear.x = delta_x*0.1;
 56 |     vs.twist.linear.y = -delta_y*0.1;
 57 |     vs.header.stamp = ros::Time::now();
 58 |     vel_sp_pub.publish(vs);
 59 | }
 60 | 
 61 | //received local position
 62 | void localPositionReceived(const geometry_msgs::PoseStampedConstPtr& msg){
 63 |     currentPos = *msg;
 64 | }
 65 | 
 66 | //
 67 | void serialDataReceived(const std_msgs::String::ConstPtr& msg){
 68 | 	ROS_INFO("I heard:[%s]",msg->data.c_str());
 69 | }
 70 | 
 71 | void globalPositionReceived(const sensor_msgs::NavSatFixConstPtr& msg){
 72 |    currentGlobalPos = *msg;
 73 | }
 74 | 
 75 | //keyboard command 
 76 | void sendCommand(const keyboard::Key &key){
 77 |    switch (key.code)
 78 |    {
 79 |    		case 'w':
 80 |    		{
 81 |    			ROS_INFO_STREAM("up");
 82 |    			vs.twist.linear.z += 0.1;
 83 |    			break;
 84 |    		}
 85 |       case 's':
 86 |       {
 87 |         ROS_INFO_STREAM("down");
 88 |         vs.twist.linear.z -= 0.1;
 89 |         break;
 90 |       }
 91 |    		case 'j':
 92 |    		{
 93 |    			ROS_INFO_STREAM("left");
 94 |    			vs.twist.linear.y += 0.1;
 95 |    			break;
 96 |    		}
 97 |    		case 'l':
 98 |    		{
 99 |    			ROS_INFO_STREAM("right");
100 |    			vs.twist.linear.y -= 0.1;
101 |    			break;
102 |    		}
103 |       case 'i':
104 |       {
105 |         ROS_INFO_STREAM("forward");
106 |         vs.twist.linear.x += 0.1;
107 |         break;
108 |       }
109 |       case 'k':
110 |       {
111 |         ROS_INFO_STREAM("backward");
112 |         vs.twist.linear.x -= 0.1;
113 |         break;
114 |       }
115 |       case 'u':
116 |       {
117 |         ROS_INFO_STREAM("rotate left");
118 |         vs.twist.angular.z += 0.1;
119 |         break;
120 |       }
121 |       case 'o':
122 |       {
123 |         ROS_INFO_STREAM("rotate right");
124 |         vs.twist.angular.z -= 0.1;
125 |         break;
126 |       }
127 |       case 'p':
128 |       {
129 |       	isGetpoint = true;
130 |       	isGetGPSPoint = false;
131 |       	ROS_INFO_STREAM("get to point 10 10 3");
132 |               break;
133 |       }
134 |       case 'g':
135 |       {
136 |       	isGetGPSPoint = true;
137 |       	isGetpoint = false;
138 |       	ROS_INFO_STREAM("get to GPS point");
139 |       	break;
140 |       }
141 |       case 'h':
142 |       {
143 |         // turn to manual mode
144 |         isGetpoint = false;
145 |         isGetGPSPoint =false;
146 |         vs.twist.linear.x = 0;
147 |         vs.twist.linear.y = 0;
148 |         vs.twist.linear.z = 0;
149 |         vs.twist.angular.z = 0;
150 |         ROS_INFO_STREAM("Manual Mode");
151 |         break;
152 |       }
153 |       
154 |       case 'a':
155 |       {
156 |         speed += 0.1;
157 |         ROS_INFO_STREAM("speed up" << speed);
158 |         break;
159 |       }
160 |       case 'd':
161 |       {
162 |         speed -= 0.1;
163 |         ROS_INFO_STREAM("speed down" << speed);
164 |         break;
165 |       }
166 | 
167 |    		
168 |    		default:
169 |    		{
170 |    			
171 |    		}
172 | 
173 |    }
174 | }
175 | 
176 | int main(int argc, char **argv){
177 |   ros::init(argc, argv, "px4_offboard_velocity_control_node");
178 |   ros::NodeHandle nodeHandle;
179 |   //publish set_velocity topic 100ms
180 |   vel_sp_pub = nodeHandle.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);
181 |   ros::Subscriber commandSubscriber = nodeHandle.subscribe("/keyboard/keydown",1,sendCommand);
182 |   ros::Subscriber localPositionSubsciber = nodeHandle.subscribe("/mavros/local_position/local", 10, localPositionReceived);
183 |   ros::Subscriber globalPositionSubsciber =  nodeHandle.subscribe("/mavros/global_position/raw/fix", 10, globalPositionReceived);
184 | 
185 |   gps_pub = nodeHandle.advertise<std_msgs::String>("serial_data_send", 10);
186 |   ros::Subscriber serialSubscriber = nodeHandle.subscribe("serial_data_received", 10, serialDataReceived);
187 | 
188 |   isGetpoint = false;
189 |   isGetGPSPoint = false;
190 |   speed = 0.2;
191 | //ros rate 
192 |   ros::Rate loop_rate(10.0);
193 | 
194 |   while(ros::ok())
195 |   {
196 | 
197 |       vs.header.seq++;
198 |       //GPS topic 
199 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.longitude);
200 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.latitude);
201 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.altitude);
202 | 
203 |       if((!isGetpoint) && (!isGetGPSPoint)){
204 | 
205 |         vs.header.stamp = ros::Time::now();
206 |         //ROS_INFO_STREAM("send ps" << ps);
207 |         vel_sp_pub.publish(vs);
208 |       }else if(isGetGPSPoint) {
209 |          gotoGPSPoint(55.7530147,37.6273584);
210 |          ROS_INFO_STREAM("GPS");
211 |       }else if(isGetpoint){
212 |           getPoint(10,10,3);
213 |           ROS_INFO_STREAM("local position");
214 |       }
215 | 
216 |       std_msgs::String gps_location;
217 |       std::stringstream ss;
218 |       ss << "o120.11111111a30.3333333333#";
219 |       gps_location.data = ss.str();
220 |       gps_pub.publish(gps_location);      
221 | 		
222 |     ros::spinOnce();
223 | 
224 |     loop_rate.sleep();
225 |   }
226 | }
227 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/px4ros/src/px4_getpoint_node_bp.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | #include <geometry_msgs/Point.h>
  3 | #include <geometry_msgs/PoseStamped.h>
  4 | #include <geometry_msgs/TwistStamped.h>
  5 | #include <sensor_msgs/NavSatFix.h>
  6 | #include <keyboard/Key.h>
  7 | #include <math.h>
  8 | #include <angles/angles.h>
  9 | //set velocity
 10 | ros::Publisher vel_sp_pub;
 11 | //current position 
 12 | // next position
 13 | geometry_msgs::PoseStamped currentPos,nextPos,initCirclePos;
 14 | geometry_msgs::TwistStamped vs;
 15 | sensor_msgs::NavSatFix currentGlobalPos;
 16 | bool isGetpoint, isGetGPSPoint;
 17 | float speed;
 18 | //radius
 19 | int R = 6371000;
 20 | 
 21 | // go to specific point 
 22 | void getPoint(double x, double y,double z){
 23 |     bool reached = false;
 24 |     double dis = sqrt((currentPos.pose.position.x-x)*(currentPos.pose.position.x-x) +(currentPos.pose.position.y-y)*(currentPos.pose.position.y-y)  + (currentPos.pose.position.z-z)*(currentPos.pose.position.z-z));
 25 |     double thresh = 0.5;
 26 |     if(dis < thresh){
 27 |     	reached = true;
 28 |     	isGetpoint =false;
 29 |     	ROS_INFO_STREAM("get There!");
 30 |     }
 31 | 
 32 |     geometry_msgs::Point velocityV;
 33 |     velocityV.x = x - currentPos.pose.position.x;
 34 |     velocityV.y = y - currentPos.pose.position.y;
 35 |     velocityV.z = z - currentPos.pose.position.z;
 36 | 
 37 |     //set velocity
 38 |     vs.twist.linear.x = velocityV.x*0.2;
 39 |     vs.twist.linear.y = velocityV.y*0.2;
 40 |     vs.twist.linear.z = velocityV.z*0.2;
 41 |     // ROS_INFO_STREAM("getPoint1");
 42 |     vs.header.stamp = ros::Time::now();
 43 |     vel_sp_pub.publish(vs);
 44 | }
 45 | 
 46 | //GPS point
 47 | void gotoGPSPoint(double lat, double lon){
 48 |     double delta_x = R * (lat - currentGlobalPos.latitude) * float(3.1415926535898 / 180);
 49 |     double delta_y = R * (lon - currentGlobalPos.longitude) * float(3.1415926535898 / 180);
 50 |     vs.twist.linear.x = delta_x*0.1;
 51 |     vs.twist.linear.y = -delta_y*0.1;
 52 |     vs.header.stamp = ros::Time::now();
 53 |     vel_sp_pub.publish(vs);
 54 | }
 55 | //received local position
 56 | void localPositionReceived(const geometry_msgs::PoseStampedConstPtr& msg){
 57 |     currentPos = *msg;
 58 | }
 59 | 
 60 | void globalPositionReceived(const sensor_msgs::NavSatFixConstPtr& msg){
 61 |    currentGlobalPos = *msg;
 62 | }
 63 | //keyboard command 
 64 | void sendCommand(const keyboard::Key &key){
 65 |    switch (key.code)
 66 |    {
 67 |    		case 'w':
 68 |    		{
 69 |    			ROS_INFO_STREAM("up");
 70 |    			vs.twist.linear.z += 0.1;
 71 |    			break;
 72 |    		}
 73 |       case 's':
 74 |       {
 75 |         ROS_INFO_STREAM("down");
 76 |         vs.twist.linear.z -= 0.1;
 77 |         break;
 78 |       }
 79 |    		case 'j':
 80 |    		{
 81 |    			ROS_INFO_STREAM("left");
 82 |    			vs.twist.linear.y += 0.1;
 83 |    			break;
 84 |    		}
 85 |    		case 'l':
 86 |    		{
 87 |    			ROS_INFO_STREAM("right");
 88 |    			vs.twist.linear.y -= 0.1;
 89 |    			break;
 90 |    		}
 91 |       case 'i':
 92 |       {
 93 |         ROS_INFO_STREAM("forward");
 94 |         vs.twist.linear.x += 0.1;
 95 |         break;
 96 |       }
 97 |       case 'k':
 98 |       {
 99 |         ROS_INFO_STREAM("backward");
100 |         vs.twist.linear.x -= 0.1;
101 |         break;
102 |       }
103 |       case 'u':
104 |       {
105 |         ROS_INFO_STREAM("rotate left");
106 |         vs.twist.angular.z += 0.1;
107 |         break;
108 |       }
109 |       case 'o':
110 |       {
111 |         ROS_INFO_STREAM("rotate right");
112 |         vs.twist.angular.z -= 0.1;
113 |         break;
114 |       }
115 |       case 'p':
116 |       {
117 |       	isGetpoint = true;
118 |       	isGetGPSPoint = false;
119 |       	ROS_INFO_STREAM("get to point 10 10 3");
120 |               break;
121 |       }
122 |       case 'g':
123 |       {
124 |       	isGetGPSPoint = true;
125 |       	isGetpoint = false;
126 |       	ROS_INFO_STREAM("get to GPS point");
127 |       	break;
128 |       }
129 |       case 'h':
130 |       {
131 |         // turn to manual mode
132 |         isGetpoint = false;
133 |         isGetGPSPoint =false;
134 |         vs.twist.linear.x = 0;
135 |         vs.twist.linear.y = 0;
136 |         vs.twist.linear.z = 0;
137 |         vs.twist.angular.z = 0;
138 |         ROS_INFO_STREAM("Manual Mode");
139 |         break;
140 |       }
141 |       
142 |       case 'a':
143 |       {
144 |         speed += 0.1;
145 |         ROS_INFO_STREAM("speed up" << speed);
146 |         break;
147 |       }
148 |       case 'd':
149 |       {
150 |         speed -= 0.1;
151 |         ROS_INFO_STREAM("speed down" << speed);
152 |         break;
153 |       }
154 | 
155 |    		
156 |    		default:
157 |    		{
158 |    			
159 |    		}
160 | 
161 |    }
162 | }
163 | int main(int argc, char **argv){
164 |   ros::init(argc, argv, "px4_offboard_velocity_control_node");
165 |   ros::NodeHandle nodeHandle;
166 |   //publish set_velocity topic 100ms
167 |   vel_sp_pub = nodeHandle.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);
168 |   ros::Subscriber commandSubscriber = nodeHandle.subscribe("/keyboard/keydown",1,sendCommand);
169 |   ros::Subscriber localPositionSubsciber = nodeHandle.subscribe("/mavros/local_position/local", 10, localPositionReceived);
170 |   ros::Subscriber globalPositionSubsciber =  nodeHandle.subscribe("/mavros/global_position/raw/fix", 10, globalPositionReceived);
171 |   isGetpoint = false;
172 |   isGetGPSPoint = false;
173 |   speed = 0.2;
174 | //ros rate 
175 |   ros::Rate loop_rate(10.0);
176 | 
177 |   while(ros::ok())
178 |   {
179 | 
180 |       vs.header.seq++;
181 |       //GPS topic 
182 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.longitude);
183 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.latitude);
184 |       // ROS_INFO_STREAM("GPS longitude" << currentGlobalPos.altitude);
185 | 
186 |       if((!isGetpoint) && (!isGetGPSPoint)){
187 | 
188 |         vs.header.stamp = ros::Time::now();
189 |         //ROS_INFO_STREAM("send ps" << ps);
190 |         vel_sp_pub.publish(vs);
191 |       }else if(isGetGPSPoint) {
192 |          gotoGPSPoint(55.7530147,37.6273584);
193 |          ROS_INFO_STREAM("GPS");
194 |       }else if(isGetpoint){
195 |           getPoint(10,10,3);
196 |           ROS_INFO_STREAM("local position");
197 |       }
198 |     ros::spinOnce();
199 | 
200 |     loop_rate.sleep();
201 |   }
202 | }
203 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(spiri_go)
  3 | 
  4 | ## Find catkin macros and libraries
  5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  6 | ## is used, also find other catkin packages
  7 | find_package(catkin REQUIRED COMPONENTS
  8 |   roscpp
  9 |   rospy
 10 |   std_msgs
 11 |   actionlib_msgs
 12 |   actionlib
 13 | )
 14 | 
 15 | 
 16 | include_directories(
 17 |   ${catkin_INCLUDE_DIRS}
 18 | )
 19 | 
 20 | 
 21 | find_package(PythonLibs 2.7 REQUIRED)
 22 | find_package(PythonInterp 2.7 REQUIRED)
 23 | EXEC_PROGRAM ("${PYTHON_EXECUTABLE}"
 24 |   ARGS "-c 'import numpy; print numpy.get_include()'"
 25 |   OUTPUT_VARIABLE NUMPY_INCLUDE_DIR
 26 |   RETURN_VALUE NUMPY_NOT_FOUND)
 27 |   
 28 | 
 29 | # find_package(OpenCV REQUIRED)
 30 | 
 31 | ## Add folders to be run by python nosetests
 32 | # catkin_add_nosetests(test)
 33 | 
 34 | include_directories(include ${catkin_INCLUDE_DIRS})
 35 | include_directories(/usr/include/python2.7/)
 36 | 
 37 | 
 38 | ###############################################
 39 | ## Things needed to add messages should be here
 40 | ###############################################
 41 | add_service_files(
 42 |   FILES LocalPosition.srv LastState.srv
 43 | )
 44 | add_action_files(
 45 |   DIRECTORY action
 46 |   FILES Takeoff.action LandHere.action
 47 | )
 48 | ##############################
 49 | ## Now generate those messages
 50 | ##############################
 51 | generate_messages(
 52 |   DEPENDENCIES actionlib_msgs std_msgs  # Or other packages containing msgs
 53 | )
 54 | ######################
 55 | ## Call catkin_package
 56 | ######################
 57 | catkin_package(
 58 |   CATKIN_DEPENDS actionlib_msgs
 59 | )
 60 | 
 61 | #######################################
 62 | ## Use Cmake commands to build the code
 63 | #######################################
 64 | add_library(spiri_data_classes src/spiri_data_classes.cpp)
 65 | target_link_libraries(spiri_data_classes ${catkin_LIBRARIES})
 66 | add_executable(spiri_go src/spiri_go.cpp)
 67 | add_dependencies(spiri_go spiri_go_generate_messages_cpp)
 68 | target_link_libraries(spiri_go spiri_data_classes ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 69 | #add_library(spiri_boost src/spiri_boost.cpp)
 70 | #target_link_libraries(spiri_boost ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} spiri_go spiri_data_classes)
 71 | 
 72 | # Test
 73 | if(CATKIN_ENABLE_TESTING)
 74 | 	find_package(rostest REQUIRED)
 75 | 	add_rostest_gtest(spiri_go_test test/sitl.test src/test/spiri_go_test.cpp)
 76 | 	target_link_libraries(spiri_go_test ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
 77 | 	endif()
 78 | 
 79 | #my test
 80 | add_executable(takeoff_and_land src/takeoff_and_land.cpp)
 81 | add_dependencies(takeoff_and_land spiri_go_generate_messages_cpp)
 82 | target_link_libraries(takeoff_and_land spiri_data_classes ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 83 | 
 84 | add_executable(leader_control src/leader_control.cpp)
 85 | add_dependencies(leader_control spiri_go_generate_messages_cpp)
 86 | target_link_libraries(leader_control spiri_data_classes ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 87 | 
 88 | add_executable(collision_avoidance src/collision_avoidance.cpp)
 89 | add_dependencies(collision_avoidance spiri_go_generate_messages_cpp)
 90 | target_link_libraries(collision_avoidance spiri_data_classes ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 91 | 
 92 | add_executable(leader src/leader.cpp)
 93 | add_dependencies(leader spiri_go_generate_messages_cpp)
 94 | target_link_libraries(leader spiri_data_classes ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 95 | 
 96 | add_executable(follower src/follower.cpp)
 97 | add_dependencies(follower spiri_go_generate_messages_cpp)
 98 | target_link_libraries(follower spiri_data_classes ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 99 | 
100 | add_executable(test_com src/test/test_com.cpp)
101 | add_dependencies(test_com spiri_go_generate_messages_cpp)
102 | target_link_libraries(test_com spiri_data_classes ${catkin_LIBRARIES} ${Boost_LIBRARIES})
103 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/README.md:
--------------------------------------------------------------------------------
  1 | # SpiriGo
  2 | 
  3 | SpiriGo is a ROS package for performing autonomous flights with a MAVLink UAV. It is built upon [`mavros`](http://wiki.ros.org/mavros) and [ArduPilot](https://github.com/diydrones/ardupilot).
  4 | 
  5 | SpiriGo is currently in very early stages of development. We have some features planned for later:
  6 | 
  7 | - Trajectory planning for goals
  8 | - SLAM
  9 | - Depth mapping
 10 | 
 11 | But for now it provides some basic convenience methods for `mavros` use.
 12 | 
 13 | ## Installation and Usage
 14 | 
 15 | ### On the Jetson TK1
 16 | 
 17 | SpiriGo is primarily being developed for use on the [Jetson TK1](http://www.nvidia.ca/object/jetson-tk1-embedded-dev-kit.html) for now. We have thus created an [automated install script](https://github.com/Pleiades-Spiri/spiri_go/blob/master/install-spirigo.sh) that will install necessary components on the Jetson for autonomous UAV development. This includes:
 18 | 
 19 | - [ROS Indigo](http://wiki.ros.org/indigo), 
 20 | - [CUDA 6.5](https://developer.nvidia.com/cuda-toolkit-65), and
 21 | - [OpenCV4Tegra](http://elinux.org/Jetson/Computer_Vision_Performance#Hardware_Acceleration_of_OpenCV).
 22 | 
 23 | Although it is [generally recognized that ROS is incompatible with OpenCV4Tegra](http://wiki.ros.org/NvidiaJetsonTK1), our install script correctly works around the issue via [methods outlined in this post](https://devtalk.nvidia.com/default/topic/835118/embedded-systems/incorrect-configuration-in-opencv4tegra-debian-packages-and-solution). To install:
 24 | 
 25 | 1. Flash your Jetson with the latest L4T using [Jetpack](https://developer.nvidia.com/embedded/jetson-development-pack-archive);
 26 | 2. Install the [Grinch kernel](https://devtalk.nvidia.com/default/topic/766303/embedded-systems/-customkernel-the-grinch-19-3-8-for-jetson-tk1-developed/) for your version of L4T; then
 27 | 3. Get the installation script and run it *without* root:
 28 | 
 29 | ```bash
 30 | $ wget https://raw.githubusercontent.com/Pleiades-Spiri/spiri_go/master/install-spirigo.sh
 31 | $ chmod +x install-spirigo.sh
 32 | $ ./install-spirigo.sh
 33 | ```
 34 | 
 35 | Note: you will be prompted for your password right away even though it's without `sudo`. 
 36 | 
 37 | #### Connect the Jetson to the Pixhawk via serial/UART connection
 38 | 
 39 | 1. Connect the Jetson's UART ports to an added voltage converter (1.8 V to 5V);
 40 | 2. Connect the voltage converter to the Pixhawk;
 41 | 3. Make sure that the serial connection is established by checking `/dev/ttyTHS0`;
 42 | 4. Validate the Jetson to Pixhawk communication using maximum baud rate (921600).
 43 | 
 44 | | TK1           | TXB0104     | Pixhawk |
 45 | | ------------- | ----------- | ------- |
 46 | | 1.8V: P37     | VCCA - VCCB | 5V: P1  |
 47 | | GND: P38      | GND - GND   | GND: P6 |
 48 | | TXd1: P41     | A1 - B1     | TX1: P2 |
 49 | | RXd1: P44     | A2 - B2     | RX1: P3 |
 50 | 
 51 | See [this diagram](https://drive.google.com/open?id=0BxXn6LyBxnG6b01mc1N5X2diVlU) for pinout details.
 52 | 
 53 | #### Running the launch file
 54 | 
 55 | Use this command to launch SpiriGo with serial communication: 
 56 | 
 57 | ```
 58 | $ roslaunch spiri_go jetson.launch
 59 | ```
 60 | 
 61 | If you see 
 62 | 
 63 | ```
 64 | $ [ INFO] [1447370932.205419674]: CON: Got HEARTBEAT, connected.
 65 | ```
 66 | 
 67 | It means you've succeeded. Note the baud rate is set to `921600` because that's the highest allowed by `mavros`. We're looking into increasing this to `1500000` maybe if we really need to.
 68 | 
 69 | ### On Ubuntu 14.04 desktop (for simulator)
 70 | 
 71 | To run the simulator, clone a copy of the ardupilot project from ArduPilot. Follow the directions at [ArduPilot SITL](http://dev.ardupilot.com/wiki/sitl-simulator-software-in-the-loop) to set up the simulator. There is also a script, `initalization/apm_sim` that will do this set-up automatically. 
 72 | 
 73 | This script is meant to be ran from a fresh Ubuntu 14.04 installation. Please proceed with caution if you plan on installing on a system with ROS already installed.
 74 | 
 75 | ```bash
 76 | $ wget https://raw.githubusercontent.com/Pleiades-Spiri/spiri_go/master/initialize/apm_sim
 77 | $ chmod +x apm_sim
 78 | $ ./apm_sim
 79 | ```
 80 | 
 81 | #### Running the simulator and the launch file
 82 | 
 83 | Start the simulator with:
 84 | 
 85 | ```bash
 86 | $ sim_vehicle.sh --map --console --aircraft test
 87 | ```
 88 | 
 89 | Next launch the sitl node for spiri:
 90 | 
 91 | ```bash
 92 | $ roslaunch spiri_go sitl.launch
 93 | ```
 94 | 
 95 | Note: if you're running this the first time, you may need to disable RC calibration pre-arm check via the `mavproxy` console.
 96 | 
 97 | If you are using a ground control station such as APM Planner 2, it should connect on UDP port `14551`.
 98 | 
 99 | ### Python API
100 | 
101 | We also provide a python API to control Spiri, using the services and actions provided by the ROS library.
102 | 
103 | To install this on your system, run:
104 | 
105 | ```bash
106 | $ python setup.py install
107 | ```
108 | 
109 | from the root of this repository.
110 | 
111 | To use this in a python script, include the following line:
112 | 
113 | ```python
114 | from spiripy import api
115 | spiri = api.SpiriGo()
116 | ```
117 | 
118 | `spiri` will then be a SpiriGo instance with the following methods:
119 | 
120 | ```python
121 | wait_for_ros([timeout])
122 | wait_for_fcu([timeout])
123 | get_state()
124 | get_local_position()
125 | takeoff([height])
126 | land()
127 | ```
128 | 
129 | Your script can be launched as a normal python scripts, but `spiri_go`, and `mavros` packages must be running in the background for it to control a quadcopter.
130 | 
131 | Detailed API documentation coming soon.
132 | 
133 | ## Generating documentation
134 | 
135 | TODO
136 | 
137 | ## Developers
138 | 
139 | ### Running tests
140 | 
141 | To test the python API, run:
142 | 
143 | ```bash
144 | $ python test/api.py
145 | ```
146 | 
147 | To test those parts that require the simulator running, use:
148 | 
149 | ```bash
150 | $ python test/sim.py
151 | ```
152 | 
153 | Any new methods that are made MUST have a corresponding unit test, and if possible should have a corresponding unit test with the simulator. It is good practice to write the test before implementing the method.
154 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/action/LandHere.action:
--------------------------------------------------------------------------------
1 | #goal definition
2 | int32 height
3 | ---
4 | #result definition
5 | ---
6 | #feedback
7 | int32 currentHeight


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/action/Takeoff.action:
--------------------------------------------------------------------------------
1 | #goal definition
2 | float32 height
3 | ---
4 | #result definition
5 | ---
6 | #feedback
7 | int32 currentHeight


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/include/spiri_data_classes.h:
--------------------------------------------------------------------------------
 1 | #ifndef __SPIRI_DATA_CLASSES_H__
 2 | #define __SPIRI_DATA_CLASSES_H__
 3 | 
 4 | class SpiriAttitude{
 5 | public:
 6 | 	double roll, pitch, yaw;
 7 | 	SpiriAttitude();
 8 | 	SpiriAttitude(double roll, double pitch, double yaw);
 9 | };
10 | 
11 | #endif //__SPIRI_DATA_CLASSES__
12 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/include/spiri_go.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #ifndef __SPIRIGO_H
  4 | #define __SPIRIGO_H
  5 |  
  6 | 
  7 | #include "ros/ros.h"
  8 | #include "geometry_msgs/Twist.h"
  9 | #include "geometry_msgs/Vector3Stamped.h"
 10 | #include "geometry_msgs/Pose.h"
 11 | #include "sensor_msgs/Image.h"
 12 | #include "sensor_msgs/Imu.h"
 13 | #include <sensor_msgs/NavSatFix.h>
 14 | #include "tf/tfMessage.h"
 15 | #include "tf/transform_listener.h"
 16 | #include "tf/transform_broadcaster.h"
 17 | #include "tf2_ros/buffer.h"
 18 | #include "tf2_ros/transform_listener.h"
 19 | #include <tf2_ros/transform_broadcaster.h>
 20 | #include <tf2/LinearMath/Matrix3x3.h>
 21 | #include "std_msgs/Empty.h"
 22 | #include "std_srvs/Empty.h"
 23 | #include "mavros_msgs/State.h"
 24 | #include "mavros_msgs/Mavlink.h"
 25 | #include <spiri_data_classes.h>
 26 | #include <actionlib/server/simple_action_server.h>
 27 | #include <spiri_go/TakeoffAction.h>
 28 | #include <spiri_go/LocalPosition.h>
 29 | #include <spiri_go/LandHereAction.h>
 30 | #include <spiri_go/LastState.h>
 31 | #include <string>
 32 | 
 33 | using namespace std;
 34 | 
 35 | class SpiriGo
 36 | {
 37 | private:
 38 |     // subscribers
 39 |     // the local position of the copter
 40 |     ros::Subscriber local;
 41 |     // the state of the copter
 42 |     ros::Subscriber state;
 43 | 	// the gps position, I add
 44 | 	ros::Subscriber gps;
 45 | 	// the height, I add
 46 | 	ros::Subscriber alt;
 47 | 	// the connect state, I add
 48 | 	ros::Subscriber connect_state;
 49 | 
 50 |     // publishers
 51 |     // sets the velocity
 52 |     ros::Publisher vel;
 53 |     ros::Publisher rc_out;
 54 | 
 55 |     // service clients
 56 |     // Arm the copter
 57 |     ros::ServiceClient arm;
 58 |     // a service to set the mode
 59 |     ros::ServiceClient set_mode;
 60 | 	// a service to set the stream rate
 61 | 	ros::ServiceClient set_stream_rate;
 62 |     // tell the copter to take off
 63 |     ros::ServiceClient takeoff;
 64 |     // MAVLink service for other needs
 65 |     ros::ServiceClient mavlink_cmd_srv;
 66 | 
 67 |     ros::NodeHandle nh;
 68 | 
 69 |     // save last imu received for forwarding...
 70 |     sensor_msgs::Imu lastLocalReceived;
 71 | 
 72 |     // ROS message callbacks
 73 |     void localSubCb(const geometry_msgs::PoseStamped localPtr);
 74 |     void stateSubCb(const mavros_msgs::State);
 75 | 	void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg);    //I add
 76 | 	void altSubCb(const sensor_msgs::NavSatFixConstPtr& msg);
 77 | 	void connectSubCb(const mavros_msgs::MavlinkPtr& fromPtr);
 78 | 
 79 |     // state variables
 80 |     mavros_msgs::State last_state;
 81 |     bool taking_off; // has taken off
 82 |     bool flying; // has finished taking off
 83 | 
 84 |     // location last returned by mavros
 85 |     geometry_msgs::Pose last_location;
 86 |     // guess of nearest location, itegrated using velocity
 87 |     geometry_msgs::Pose location;
 88 |     // target velocity
 89 |     geometry_msgs::Twist velocity;
 90 |     // gps returned by mavros, I add
 91 | 	sensor_msgs::NavSatFix last_gps;
 92 | 	// height
 93 | 	sensor_msgs::NavSatFix last_alt;
 94 | 	//
 95 | 	mavros_msgs::Mavlink last_connect_state;
 96 | 
 97 |     // internal control methods
 98 |     geometry_msgs::Quaternion getOrientation();
 99 |     bool setMode(const char* targetMode);
100 |     bool setArmed();
101 |     //bool setGuided();
102 |     void takeOff(float targetAlt);
103 | 
104 |     // service callbacks
105 |     bool getLocalPositionSCB(spiri_go::LocalPosition::Request &req, spiri_go::LocalPosition::Response &rsp);
106 |     bool getLastStateSCB(spiri_go::LastState::Request &req, spiri_go::LastState::Response &rsp);
107 | 
108 |     void updateLocation(ros::Duration dt);
109 | 
110 | public:
111 | 
112 |     // action servers
113 |     actionlib::SimpleActionServer<spiri_go::TakeoffAction> takeoff_as;
114 |     actionlib::SimpleActionServer<spiri_go::LandHereAction> land_here_as;
115 | 
116 |     // services, should all be a descriptive name with "Service" at the end
117 |     // Their corresponding call back should have the same name, replacing "Service" with SCB
118 |     // thus if you have a service named doSomethingService, it's call back should be doSomethingSCB
119 |     ros::ServiceServer getLocalPositionService;
120 |     ros::ServiceServer getLastStateService;
121 | 
122 |     SpiriGo();
123 |     ~SpiriGo();
124 | 
125 |     // getter functions 
126 |     bool isArmed();
127 |     bool isControllable();
128 |     std::string getMode();
129 |     geometry_msgs::Point getLocalPosition();
130 |     SpiriAttitude getAttitude();
131 | 	bool isValid();
132 | 
133 | 	bool setGuided();
134 | 
135 | 	bool setStreamRate(unsigned char streamID, int rate);
136 | 
137 | 	
138 | 
139 |     // Yaw commander with MAVLink; angles are in degrees 
140 |     // Eigen::Vector3d getAttitude();
141 | 
142 | 
143 |     // basic Spiri control functions
144 |     void armAndTakeOff(const spiri_go::TakeoffGoalConstPtr& goal);
145 |     void landHere(const spiri_go::LandHereGoalConstPtr& goal);
146 |     void conditionYaw(float targetYaw, float targetYawRate);
147 |     void setENUVelocity(/*double eastwardVelocity, double northwardVelocity*/);
148 |     void setHorizontalVelocity(double u, double v);
149 | 
150 |     // main pose-estimation loop
151 |     void Loop();
152 | 
153 | };
154 | #endif /* __SPIRIGO_H */
155 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/include/spiri_go.h~:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #ifndef __SPIRIGO_H
  4 | #define __SPIRIGO_H
  5 |  
  6 | 
  7 | #include "ros/ros.h"
  8 | #include "geometry_msgs/Twist.h"
  9 | #include "geometry_msgs/Vector3Stamped.h"
 10 | #include "geometry_msgs/Pose.h"
 11 | #include "sensor_msgs/Image.h"
 12 | #include "sensor_msgs/Imu.h"
 13 | #include <sensor_msgs/NavSatFix.h>
 14 | #include "tf/tfMessage.h"
 15 | #include "tf/transform_listener.h"
 16 | #include "tf/transform_broadcaster.h"
 17 | #include "tf2_ros/buffer.h"
 18 | #include "tf2_ros/transform_listener.h"
 19 | #include <tf2_ros/transform_broadcaster.h>
 20 | #include <tf2/LinearMath/Matrix3x3.h>
 21 | #include "std_msgs/Empty.h"
 22 | #include "std_srvs/Empty.h"
 23 | #include "mavros_msgs/State.h"
 24 | #include "mavros_msgs/Mavlink.h"
 25 | #include <spiri_data_classes.h>
 26 | #include <actionlib/server/simple_action_server.h>
 27 | #include <spiri_go/TakeoffAction.h>
 28 | #include <spiri_go/LocalPosition.h>
 29 | #include <spiri_go/LandHereAction.h>
 30 | #include <spiri_go/LastState.h>
 31 | #include <string>
 32 | 
 33 | using namespace std;
 34 | 
 35 | class SpiriGo
 36 | {
 37 | private:
 38 |     // subscribers
 39 |     // the local position of the copter
 40 |     ros::Subscriber local;
 41 |     // the state of the copter
 42 |     ros::Subscriber state;
 43 | 	// the gps position, I add
 44 | 	ros::Subscriber gps;
 45 | 	// the height, I add
 46 | 	ros::Subscriber alt;
 47 | 	// the connect state, I add
 48 | 	ros::Subscriber connect_state;
 49 | 
 50 |     // publishers
 51 |     // sets the velocity
 52 |     ros::Publisher vel;
 53 |     ros::Publisher rc_out;
 54 | 
 55 |     // service clients
 56 |     // Arm the copter
 57 |     ros::ServiceClient arm;
 58 |     // a service to set the mode
 59 |     ros::ServiceClient set_mode;
 60 | 	// a service to set the stream rate
 61 | 	ros::ServiceClient set_stream_rate;
 62 |     // tell the copter to take off
 63 |     ros::ServiceClient takeoff;
 64 |     // MAVLink service for other needs
 65 |     ros::ServiceClient mavlink_cmd_srv;
 66 | 
 67 |     ros::NodeHandle nh;
 68 | 
 69 |     // save last imu received for forwarding...
 70 |     sensor_msgs::Imu lastLocalReceived;
 71 | 
 72 |     // ROS message callbacks
 73 |     void localSubCb(const geometry_msgs::PoseStamped localPtr);
 74 |     void stateSubCb(const mavros_msgs::State);
 75 | 	void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg);    //I add
 76 | 	void altSubCb(const sensor_msgs::NavSatFixConstPtr& msg);
 77 | 	void connectSubCb(const mavros_msgs::Mavlink fromPtr);
 78 | 
 79 |     // state variables
 80 |     mavros_msgs::State last_state;
 81 |     bool taking_off; // has taken off
 82 |     bool flying; // has finished taking off
 83 | 
 84 |     // location last returned by mavros
 85 |     geometry_msgs::Pose last_location;
 86 |     // guess of nearest location, itegrated using velocity
 87 |     geometry_msgs::Pose location;
 88 |     // target velocity
 89 |     geometry_msgs::Twist velocity;
 90 |     // gps returned by mavros, I add
 91 | 	sensor_msgs::NavSatFix last_gps;
 92 | 	// height
 93 | 	sensor_msgs::NavSatFix last_alt;
 94 | 	//
 95 | 	mavros_msgs::Mavlink last_connect_state;
 96 | 
 97 |     // internal control methods
 98 |     geometry_msgs::Quaternion getOrientation();
 99 |     bool setMode(const char* targetMode);
100 |     bool setArmed();
101 |     //bool setGuided();
102 |     void takeOff(float targetAlt);
103 | 
104 |     // service callbacks
105 |     bool getLocalPositionSCB(spiri_go::LocalPosition::Request &req, spiri_go::LocalPosition::Response &rsp);
106 |     bool getLastStateSCB(spiri_go::LastState::Request &req, spiri_go::LastState::Response &rsp);
107 | 
108 |     void updateLocation(ros::Duration dt);
109 | 
110 | public:
111 | 
112 |     // action servers
113 |     actionlib::SimpleActionServer<spiri_go::TakeoffAction> takeoff_as;
114 |     actionlib::SimpleActionServer<spiri_go::LandHereAction> land_here_as;
115 | 
116 |     // services, should all be a descriptive name with "Service" at the end
117 |     // Their corresponding call back should have the same name, replacing "Service" with SCB
118 |     // thus if you have a service named doSomethingService, it's call back should be doSomethingSCB
119 |     ros::ServiceServer getLocalPositionService;
120 |     ros::ServiceServer getLastStateService;
121 | 
122 |     SpiriGo();
123 |     ~SpiriGo();
124 | 
125 |     // getter functions 
126 |     bool isArmed();
127 |     bool isControllable();
128 |     std::string getMode();
129 |     geometry_msgs::Point getLocalPosition();
130 |     SpiriAttitude getAttitude();
131 | 	bool isValid();
132 | 
133 | 	bool setGuided();
134 | 
135 | 	bool setStreamRate(unsigned char streamID, int rate);
136 | 
137 | 	
138 | 
139 |     // Yaw commander with MAVLink; angles are in degrees 
140 |     // Eigen::Vector3d getAttitude();
141 | 
142 | 
143 |     // basic Spiri control functions
144 |     void armAndTakeOff(const spiri_go::TakeoffGoalConstPtr& goal);
145 |     void landHere(const spiri_go::LandHereGoalConstPtr& goal);
146 |     void conditionYaw(float targetYaw, float targetYawRate);
147 |     void setENUVelocity(/*double eastwardVelocity, double northwardVelocity*/);
148 |     void setHorizontalVelocity(double u, double v);
149 | 
150 |     // main pose-estimation loop
151 |     void Loop();
152 | 
153 | };
154 | #endif /* __SPIRIGO_H */
155 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/initialize/apm_sim:
--------------------------------------------------------------------------------
 1 | #! /bin/bash
 2 | ###############################################################################
 3 | ## First thing to do in any system, gotta install git to get anything done
 4 | ###############################################################################
 5 | 
 6 | sudo apt-get update
 7 | sudo apt-get install -y git screen
 8 | 
 9 | ###############################################################################
10 | ## Ardupilot and simulator installation
11 | ## Taken from http://dev.ardupilot.com/wiki/setting-up-sitl-on-linux/
12 | ###############################################################################
13 | 
14 | # Download a copy of our own fork of ardupilot
15 | git clone https://github.com/Pleiades-Spiri/ardupilot.git ~/ardupilot
16 | 
17 | # Install some required packages for the APM SITL simulator
18 | sudo apt-get -y install python-matplotlib python-serial python-wxgtk2.8 python-lxml
19 | sudo apt-get -y install python-scipy python-opencv ccache gawk git python-pip python-pexpect
20 | sudo pip install pymavlink MAVProxy
21 | 
22 | # Gotta source the simulator paths
23 | echo "
24 | export PATH=\$PATH:\$HOME/ardupilot/Tools/autotest 
25 | export PATH=/usr/lib/ccache:\$PATH
26 | " >> ~/.bashrc
27 | source ~/.bashrc
28 | 
29 | ###############################################################################
30 | ## ROS Installation 
31 | ## Taken from https://pixhawk.org/dev/ros/installation
32 | ###############################################################################
33 | 
34 | # Add the ROS repository for Trusty(14.04):
35 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
36 | 
37 | # Get the official ROS key
38 | wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add -
39 | 
40 | # And install the packages we recommend for having a development environment good for MAVs. 
41 | # You can install any other project-specific dependencies via rosdep or manually.
42 | sudo apt-get update
43 | sudo apt-get install -y ros-indigo-ros-base \
44 |                         ros-indigo-usb-cam \
45 |                         ros-indigo-mavlink \
46 |                         ros-indigo-mavros \
47 |                         ros-indigo-cv-bridge \
48 |                         ros-indigo-image-proc \
49 |                         ros-indigo-tf
50 | 
51 | # Setup rosdep which makes life a lot easier by auto-installing dependencies wherever possible.
52 | sudo apt-get install -y python-rosdep
53 | sudo rosdep init
54 | rosdep update
55 | 
56 | # Gotta source the ROS binary path
57 | echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
58 | source /opt/ros/indigo/setup.bash
59 | 
60 | ###############################################################################
61 | ## Set up minimal workspace for spiri_go
62 | ## On desktop we want just ONE package in the workspace to minimize compilation issues 
63 | ###############################################################################
64 | WORKSPACE=~/spiri_ws
65 | 
66 | # Make workspace directory then "compile" to initialize the ROS workspace
67 | mkdir -p $WORKSPACE/src
68 | cd $WORKSPACE/src
69 | catkin_init_workspace
70 | cd $WORKSPACE
71 | catkin_make 
72 | 
73 | # Download spiri_go package into source 
74 | cd $WORKSPACE/src
75 | git clone https://github.com/Pleiades-Spiri/spiri_go.git
76 | 
77 | # Now compile the one package we have
78 | cd $WORKSPACE
79 | catkin_make
80 | 
81 | # Gotta source the package paths for roslaunch/rosrun to find them
82 | sh -c "echo 'source $WORKSPACE/devel/setup.bash' >> ~/.bashrc"
83 | source $WORKSPACE/devel/setup.bash
84 | 
85 | ###############################################################################
86 | ## Commands for running the simulator
87 | ###############################################################################
88 | 
89 | # Run this to open the simulator (it'll take a while to compile the first time):
90 | # sim_vehicle.sh --map --console --aircraft test
91 | 
92 | # Once you're in run this in the mavproxy console
93 | # to get rid of the 'RC not calibrated' problem:
94 | # param load ../Tools/autotest/copter_params.parm
95 | 
96 | # In a different terminal run this to start spiri_go and mavros:
97 | # roslaunch spiri_go sitl.launch
98 | 
99 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/apm_pluginlists.yaml:
--------------------------------------------------------------------------------
 1 | plugin_blacklist:
 2 | # common
 3 | - actuator_control
 4 | - ftp
 5 | - safety_area
 6 | - setpoint_attitude
 7 | # extras
 8 | - image_pub
 9 | - 'mocap_*'
10 | - px4flow
11 | - 'vision_*'
12 | - distance_sensor
13 | - vibration
14 | 
15 | plugin_whitelist: []
16 | #- 'sys_*'
17 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/collision_avoidance.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 	<include file="$(find mavros)/launch/apm2.launch">
 4 | 	</include>
 5 | 	
 6 | 	<node pkg="px4ros" type="serial_ros.py" name="serial_ros" output="screen">
 7 | 	</node>
 8 | 	<node pkg="px4ros" type="leader_control_decode.py" name="leader_control_decode" output="screen">
 9 | 	</node>
10 | 
11 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="screen">
12 | 	</node>
13 | 	<node pkg="spiri_go" type="collision_avoidance" name="collision_avoidance" output="screen">
14 | 	</node>
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/collision_avoidance.launch~:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 	<include file="$(find mavros)/launch/apm2.launch">
 4 | 	</include>
 5 | 	
 6 | 	<node pkg="px4ros" type="serial_ros.py" name="serial_ros" output="screen">
 7 | 	</node>
 8 | 	<node pkg="px4ros" type="leader_control_decode.py" name="leader_control_decode" output="screen">
 9 | 	</node>
10 | 
11 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="screen">
12 | 	</node>
13 | 	<node pkg="spiri_go" type="collision_avoidance" name="collision_avoidance" output="screen">
14 | 	</node>
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/jetson.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |  
 3 |  	<!-- Launch parameters for Jetson TK1 serial connection -->
 4 | 	<arg name="fcu_url" default="serial:///dev/ttyTHS0:921600" />
 5 | 	<arg name="gcs_url" default="tcp-l://:14556" />
 6 | 	<arg name="tgt_system" default="1" />
 7 | 	<arg name="tgt_component" default="1" />
 8 | 	<arg name="log_output" default="screen" />
 9 | 
10 |  	<!-- First launch mavros -->
11 |  	<!-- Include instead of node because it already exists and it saves effort -->
12 | 	<include file="$(find mavros)/launch/node.launch">
13 | 		<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
14 | 		<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
15 | 
16 | 		<arg name="fcu_url" value="$(arg fcu_url)" />
17 | 		<arg name="gcs_url" value="$(arg gcs_url)" />
18 | 		<arg name="tgt_system" value="$(arg tgt_system)" />
19 | 		<arg name="tgt_component" value="$(arg tgt_component)" />
20 | 		<arg name="log_output" value="$(arg log_output)" />
21 | 	</include>
22 |  
23 |  	<!-- Launch camera nodes, but let's do this later -->
24 | <!-- 	<node name="camera" pkg="usb_cam" type="usb_cam_node">
25 | 		<param name="video_device" value="/dev/video0" />
26 | 		<param name="image_width" value="800" />
27 | 		<param name="image_height" value="600" />
28 | 		<param name="pixel_format" value="mjpeg" />
29 | 		<param name="framerate" value="30" />
30 | 		<param name="camera_frame_id" value="webcam" />
31 | 	</node> 
32 | 	<node name="viewer" pkg="image_view" type="image_view">
33 | 		<remap from="image" to="/camera/image_raw" />
34 |     </node> -->
35 |  
36 |  	<!-- Launch SpiriGo -->
37 | 	<node pkg="spiri_go" name="spiri_go" type="spiri_go" output="screen"></node>
38 | 
39 | </launch>
40 | 
41 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/kbteleop.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 | 	
3 | 	<node pkg="spiri_go" type="mavkbteleop" name="mavkbteleopr"  output="screen">
4 | 	</node>
5 | </launch>
6 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/leader-sitl.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 	<include file="$(find mavros)/launch/apm2-sitl.launch">
 4 | 	</include>
 5 | 
 6 | 	<node pkg="px4ros" type="serial_ros.py" name="serial_ros" output="log">
 7 | 	</node>
 8 | 	<node pkg="px4ros" type="leader_control_decode.py" name="leader_control_decode" output="log">
 9 | 	</node>
10 | 
11 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="log">
12 | 	</node>
13 | 	<node pkg="spiri_go" type="leader_control" name="leader_control" output="log">
14 | 	</node>
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/leader-sitl.launch~:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 	<include file="$(find mavros)/launch/apm2-sitl.launch">
 4 | 	</include>
 5 | 
 6 | 	<node pkg="px4ros" type="serial_ros.py" name="serial_ros" output="log">
 7 | 	</node>
 8 | 	<node pkg="px4ros" type="leader_control_decode.py" name="leader_control_decode" output="log">
 9 | 	</node>
10 | 
11 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="log">
12 | 	</node>
13 | 	<node pkg="spiri_go" type="leader_control" name="leader_control" output="log">
14 | 	</node>
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/leader.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 	<include file="$(find mavros)/launch/apm2.launch">
 4 | 	</include>
 5 | 	
 6 | 	<node pkg="px4ros" type="serial_ros.py" name="serial_ros" output="screen">
 7 | 	</node>
 8 | 	<node pkg="px4ros" type="leader_control_decode.py" name="leader_control_decode" output="screen">
 9 | 	</node>
10 | 
11 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="screen">
12 | 	</node>
13 | 	<node pkg="spiri_go" type="leader" name="leader" output="screen">
14 | 	</node>
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/leader.launch~:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 	<include file="$(find mavros)/launch/apm2.launch">
 4 | 	</include>
 5 | 	
 6 | 	<node pkg="px4ros" type="serial_ros.py" name="serial_ros" output="screen">
 7 | 	</node>
 8 | 	<node pkg="px4ros" type="leader_control_decode.py" name="leader_control_decode" output="screen">
 9 | 	</node>
10 | 
11 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="screen">
12 | 	</node>
13 | 	<node pkg="spiri_go" type="leader" name="leader" output="screen">
14 | 	</node>
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/sitl.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 
 4 | 	<arg name="fcu_url" default="udp://127.0.0.1:14550@" />
 5 | 	<arg name="gcs_url" default="udp://127.0.0.1:14551@" />
 6 | 	<arg name="tgt_system" default="1" />
 7 | 	<arg name="tgt_component" default="1" />
 8 | 	<arg name="log_output" default="screen" />
 9 | 
10 | 	<include file="$(find mavros)/launch/node.launch">
11 | 		<arg name="pluginlists_yaml" value="$(find spiri_go)/launch/apm_pluginlists.yaml" />
12 | 		<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
13 | 
14 | 		<arg name="fcu_url" value="$(arg fcu_url)" />
15 | 		<arg name="gcs_url" value="$(arg gcs_url)" />
16 | 		<arg name="tgt_system" value="$(arg tgt_system)" />
17 | 		<arg name="tgt_component" value="$(arg tgt_component)" />
18 | 		<arg name="log_output" value="$(arg log_output)" />
19 | 	</include>
20 | 
21 | 	<!-- Launch SpiriGo -->
22 | 
23 | 	<node pkg="spiri_go" name="spiri_go" type="spiri_go" output="screen"></node>
24 | </launch>
25 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/spiri_go.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="screen">
3 | 	</node>
4 | </launch>
5 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/takeoff_and_land.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 	<include file="$(find mavros)/launch/apm2.launch">
 4 | 	</include>
 5 | 
 6 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="screen">
 7 | 	</node>
 8 | 	<node pkg="spiri_go" type="takeoff_and_land" name="takeoff_and_land" output="screen">
 9 | 	</node>
10 | </launch>
11 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/launch/takeoff_and_land.launch~:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 	<include file="$(find mavros)/launch/apm2-sitl.launch">
 4 | 	</include>
 5 | 
 6 | 	<node pkg="spiri_go" type="spiri_go" name="spiri_go" output="screen">
 7 | 	</node>
 8 | 	<node pkg="spiri_go" type="takeoff_and_land" name="takeoff_and_land" output="screen">
 9 | 	</node>
10 | </launch>
11 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>spiri_go</name>
 4 |   <version>0.1.0</version>
 5 |   <description>The spiri_go package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="skyler.olson@pleiades.ca">Skyler Olson</maintainer>
11 |   <maintainer email="john.lian@pleiades.ca">John Lian</maintainer>
12 | 
13 | 
14 |   <!-- One license tag required, multiple allowed, one license per tag -->
15 |   <!-- Commonly used license strings: -->
16 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
17 |   <license>TODO</license>
18 | 
19 | 
20 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
21 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
22 |   <!-- Example: -->
23 |   <url type="website">https://github.com/Pleiades-Spiri/spiri_go</url>
24 | 
25 | 
26 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
27 |   <!-- Authors do not have to be maintianers, but could be -->
28 |   <!-- Example: -->
29 |   <author email="skyler.olson@pleiades.ca">Skyler Olson</author>
30 |   <author email="john.lian@pleiades.ca">John Lian</author>
31 | 
32 |   <!-- The *_depend tags are used to specify dependencies -->
33 |   <!-- Dependencies can be catkin packages or system dependencies -->
34 |   <!-- Examples: -->
35 |   <!-- Use build_depend for packages you need at compile time: -->
36 |   <!--   <build_depend>message_generation</build_depend> -->
37 |   <!-- Use buildtool_depend for build tool packages: -->
38 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
39 |   <!-- Use run_depend for packages you need at runtime: -->
40 |   <!--   <run_depend>message_runtime</run_depend> -->
41 |   <!-- Use test_depend for packages you need only for testing: -->
42 |   <!--   <test_depend>gtest</test_depend> -->
43 |   <buildtool_depend>catkin</buildtool_depend>
44 |   <build_depend>roscpp</build_depend>
45 |   <build_depend>rospy</build_depend>
46 |   <build_depend>boost</build_depend>
47 |   <build_depend>std_msgs</build_depend>
48 |   <build_depend>actionlib_msgs</build_depend>
49 |   <run_depend>roscpp</run_depend>
50 |   <run_depend>boost</run_depend>
51 |   <run_depend>rospy</run_depend>
52 |   <run_depend>std_msgs</run_depend>
53 |   <run_depend>actionlib_msgs</run_depend>
54 | 
55 | 
56 |   <!-- The export tag contains other, unspecified, tags -->
57 |   <export>
58 |     <!-- Other tools can request additional information be placed here -->
59 | 
60 |   </export>
61 | </package>
62 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/scripts/mavkbteleop.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | # vim:set ts=4 sw=4 et:
 3 | #
 4 | # Copyright 2014 Vladimir Ermakov.
 5 | #
 6 | # This file is part of the mavros package and subject to the license terms
 7 | # in the top-level LICENSE file of the mavros repository.
 8 | # https://github.com/mavlink/mavros/tree/master/LICENSE.md
 9 | 
10 | import rospy
11 | from mavros.srv import SetMode
12 | 
13 | def set_mode(mode):
14 |     try:
15 |         setmode_cl = rospy.ServiceProxy("/mavros/set_mode", SetMode)
16 |         
17 |         ret = setmode_cl(base_mode=0, custom_mode=mode)
18 |     except rospy.ServiceException as ex:
19 |         fault(ex)
20 | 
21 |     if not ret.success:
22 |         rospy.loginfo("SET MODE Request failed.")
23 |     else:
24 |         rospy.loginfo("SET MODE Request success")
25 | 
26 | 
27 | def rc_override_control():
28 |     
29 |     rospy.init_node("mavteleop")
30 |     rospy.loginfo("MAV-Teleop: RC Override control type.")
31 |     
32 |     while(1):
33 | 	    set_mode("GUIDED")
34 |         
35 | 
36 | 
37 | def main():
38 |     rc_override_control()
39 | 
40 | 
41 | if __name__ == '__main__':
42 |     main()
43 | 
44 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/setup.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | from distutils.core import setup
 4 | 
 5 | setup(name='SpiriGo',
 6 |       version='1.0',
 7 |       description='An API to control Spiri',
 8 |       author='Skyler Olson',
 9 |       author_email='skyler.olson@pleiades.ca',
10 |       url='',
11 |       packages=["spiripy"],
12 |      )
13 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/spiripy/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PenguinCheng/multiDrone-ROS/73df4bb83e1091565331f9467f779e4a3ad9bd95/multiDrone-ROS/spiri_go/spiripy/__init__.py


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/spiripy/api.py:
--------------------------------------------------------------------------------
  1 | import rospy
  2 | import roslib
  3 | import time
  4 | roslib.load_manifest('spiri_go')
  5 | import actionlib
  6 | # Load specific service and action server definitions
  7 | from spiri_go.msg import *  # imports actions too
  8 | from spiri_go.srv import *
  9 | # Import the exceptions defined here
 10 | from spiripy import spiri_except
 11 | 
 12 | 
 13 | class SpiriGo(object):
 14 |     """A top level python API to command Spiri to perform autonomous actions.
 15 | 
 16 |     The idea for this class is to supply methods for users to perform autonomous
 17 |     actions via simple python scripts.
 18 | 
 19 |     It interfaces with the SpiriGo ROS node via services and actions, not publishing
 20 |     and subscribing. This guarantees the promptness of information.
 21 | 
 22 |     Attributes:
 23 | 
 24 |     """
 25 | 
 26 |     def __init__(self):
 27 |         rospy.init_node('spiri_go_api', anonymous=True)
 28 | 
 29 |     def wait_for_ros(self, timeout=-1):
 30 |         """ Waits for the ros interfaces to come online
 31 | 
 32 |         Args:
 33 |             timeout (number): How long to wait until this should be considered
 34 |             a failure. If timeout is less than 0, will never stop trying
 35 | 
 36 |         Raises:
 37 |             SpiriGoConnectionError: If the connection times out.
 38 |         """
 39 |         try:
 40 |             rospy.wait_for_service('spiri_state', timeout)
 41 |         except:
 42 |             raise spiri_except.SpiriGoConnectionError("Timed out connecting to ROS")
 43 | 
 44 |     def wait_for_fcu(self, timeout=-1):
 45 |         """Waits for mavros to connect to the flight controller.
 46 | 
 47 |         Args:
 48 |             timeout (number): How long to wait until this should be considered
 49 |             a failure. If timeout is less than 0, will never stop trying
 50 | 
 51 |         Raises:
 52 |             SpiriGoConnectionError: If the connection times out.
 53 |         """
 54 |         start_time = time.time()
 55 |         state = self.get_state()
 56 | 
 57 |         while not state.connected:
 58 |             time.sleep(0.5)
 59 |             state = self.get_state()
 60 |             duration = time.time() - start_time
 61 |             # print "waiting for fuc: " + str(duration) + "/" + str(timeout)
 62 |             # check if this should time out
 63 |             if timeout > 0 and duration > timeout:
 64 |                 raise spiri_except.SpiriGoConnectionError("Timed out connecting to the FCU")
 65 | 
 66 |     def get_state(self):
 67 |         """mavros_msgs/State: returns state of the copter."""
 68 |         state_service_client = get_service_client('spiri_state', LastState)
 69 |         return state_service_client()
 70 | 
 71 |     def get_local_position(self):
 72 |         """geometry_msgs/Point Message: returns the 3D position vector in meters."""
 73 |         pos_service_client = get_service_client('spiri_local_position', LocalPosition)
 74 |         return pos_service_client()
 75 | 
 76 |     def takeoff(self, height=4):
 77 |         """Commands the copter to takeoff autonomously.
 78 | 
 79 |         Note:
 80 |             The copter will go to GUIDED mode, arm motors, and takeoff all in once shot.
 81 |             This is a ROS action and will be blocking until it completes.
 82 | 
 83 |         Args:
 84 |             height (int): The takeoff height, default is 4 meters.
 85 | 
 86 |         Returns:
 87 |             bool: True if successful, False otherwise.
 88 |         """
 89 | 
 90 |         # Connection to the server
 91 |         client = get_action_client('spiri_take_off', TakeoffAction)
 92 | 
 93 |         # Make a goal object for the copter to complete
 94 |         goal = TakeoffGoal()
 95 |         goal.height = height
 96 |         print "Sending takoff command"
 97 |         client.send_goal(goal)
 98 |         client.wait_for_result()
 99 | 
100 |         return True
101 | 
102 |     def land(self):
103 |         """Command the copter to land in place.
104 | 
105 |         Note:
106 |             The copter will go to LAND mode.
107 |             The method will block until it completes.
108 | 
109 |         Args:
110 | 
111 |         Returns:
112 |             bool: True if successful, False otherwise.
113 |         """
114 |         client = get_action_client('spiri_land_here', LandHereAction)
115 |         goal = LandHereGoal()
116 |         goal.height = 0
117 |         print "Sending land here command"
118 |         client.send_goal(goal)
119 |         client.wait_for_result()
120 | 
121 |         return True
122 | 
123 | 
124 | def get_action_client(name, action):
125 |     """Gets the client and ensures that the server is running
126 | 
127 |     Args:
128 |         name: a string, the server's name (eg. 'spiri_take_off').
129 |         action: an object, the type of server (eg. TakeoffAction).
130 | 
131 |     Returns:
132 |         The action client object.
133 |     """
134 |     client = actionlib.SimpleActionClient(name, action)
135 |     server_present = client.wait_for_server(rospy.Duration(1))
136 |     if server_present:
137 |         return client
138 |     else:
139 |         raise spiri_except.SpiriGoConnectionError("Could not create action client " + name)
140 | 
141 | 
142 | def get_service_client(name, service):
143 |     """Ensure that the service exists.
144 | 
145 |     Args:
146 |         service: an object, the name of the service.
147 | 
148 |     Returns:
149 |         The service client object.
150 |     """
151 |     try:
152 |         rospy.wait_for_service(name, 1)
153 |     except:
154 |         raise spiri_except.SpiriGoConnectionError("Could not create service client " + name)
155 |     return rospy.ServiceProxy(name, service)
156 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/spiripy/spiri_except.py:
--------------------------------------------------------------------------------
1 | class SpiriGoConnectionError(Exception):
2 |     def __init__(self, description = "unknown"):
3 |         self.description = description
4 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/arm_and_takeoff.cpp~:
--------------------------------------------------------------------------------
 1 | /**
 2 |  * simpely takeoff and land
 3 |  */
 4 | 
 5 | #include "ros/ros.h"
 6 | #include <actionlib/client/simple_action_client.h>
 7 | #include <actionlib/client/terminal_state.h>
 8 | #include <spiri_go/TakeoffAction.h>
 9 | #include <spiri_go/LandHereAction.h>
10 | 
11 | int main(int argc, char **argv) {
12 | 	ros::init(argc, argv, "spiri_go_client");
13 | 	
14 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);      //first param: server name, second param: true means a new thread
15 | 	actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
16 | 
17 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
18 | 
19 | 	ac.waitForServer();	
20 | 	
21 | 	ROS_INFO("Action server started--Taking off!");
22 | 
23 | 	spiri_go::TakeoffGoal goal;
24 | 
25 | 	//set the target height to take off!
26 | 	goal.height = 10;
27 | 	ac.sendGoal(goal);
28 | 
29 | 	//wait for action(takeOff) to return
30 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
31 | 
32 | 	if(is_takeOff_finished ) {
33 | 		ROS_INFO("reached target height!");
34 | 
35 | 		//do something
36 | 		ros::Duration(10).sleep();
37 | 	}
38 | 	else {
39 | 		ROS_INFO("should set mode Land!");
40 | 	}
41 | 
42 | 	//land
43 | 	ROS_INFO("Waiting for action server(land) to start.");
44 | 	ac1.waitForServer();	
45 | 	ROS_INFO("Start to land!.");
46 | 	
47 | 	spiri_go::LandHereGoal goal_1;
48 | 	goal_1.height = 0;
49 | 	ac1.sendGoal(goal_1);
50 | 
51 | 	//wait for action(land) to return
52 | 	bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
53 | 	if(is_land_finished ) {
54 | 		ROS_INFO("Land success!");
55 | 	}
56 | 
57 | 	return 0;
58 | 
59 | }
60 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/collision_avoidance.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | /*******************************************************************************
  3 |  *  The Spiri Project
  4 |  *
  5 |  *  File: collision_avoidance.cpp
  6 |  *
  7 |  *  Purpose: collision avoidance control using releative position.
  8 |  *
  9 |  *  @author Peng Cheng
 10 |  *  @version 0.1.0    2016/3/30
 11 |  ******************************************************************************/
 12 | 
 13 | #include "ros/ros.h"
 14 | #include <actionlib/client/simple_action_client.h>
 15 | #include <actionlib/client/terminal_state.h>
 16 | #include <spiri_go/TakeoffAction.h>
 17 | #include "spiri_go.h"
 18 | #include <std_msgs/String.h>
 19 | #include <sensor_msgs/NavSatFix.h>
 20 | #include <geometry_msgs/TwistStamped.h>
 21 | #include <sstream>
 22 | #include <px4ros/gpsPosition.h>
 23 | #include <math.h>
 24 | #include <fstream>
 25 | 
 26 | 
 27 | //global variables
 28 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
 29 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
 30 | 
 31 | #define R  6371000         //meter
 32 | #define PI 3.1415926535898
 33 | 
 34 | /*******************************************************************************
 35 |  *  Callback function on the gps of the copter
 36 |  *
 37 |  *  @param gpsPositionConstPtr& the gps callback pointer
 38 |  ******************************************************************************/
 39 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
 40 |    selfGlobalPos = *msg;
 41 | }
 42 | 
 43 | /*******************************************************************************
 44 |  *  Callback function on the gps of the neighbour copter
 45 |  *
 46 |  *  @param gpsPositionConstPtr& the gps callback pointer
 47 |  ******************************************************************************/
 48 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
 49 | {
 50 |     neigGlobalPos = *msg;
 51 | }
 52 | 
 53 | /*******************************************************************************
 54 |  *  Main function
 55 |  *
 56 |  *  @param 
 57 |  ******************************************************************************/
 58 | int main(int argc, char **argv) {
 59 | 	ros::init(argc, argv, "spiri_go_client");
 60 | 	ros::NodeHandle nh;
 61 | 
 62 | 	ros::Rate loop_rate(10.0);
 63 | 	
 64 | 	//velocity to publish
 65 | 	geometry_msgs::TwistStamped vel;
 66 | 	vel.twist.linear.x = 0.0;
 67 | 	vel.twist.linear.y = 0.0;
 68 | 	vel.header.stamp = ros::Time::now();
 69 | 
 70 | 	//variables definition
 71 | 	float x_0 = 0.0;
 72 | 	float y_0 = 0.0;
 73 | 	float x_1 = 0.0;
 74 | 	float y_1 = 0.0;
 75 | 	float d12 = 10;   //meter,two leaders mantain the distance
 76 |     float r = 1.0;     // the region that collision avoidance algorithm will take effect
 77 |     float r1 = 5.0;     // the region that collision avoidance algorithm will take effect
 78 | 	float vx0 = 0.0;
 79 | 	float vy0 = 0.0;
 80 | 	float currentDisPow = 0.0;
 81 |     float x_offset = 0.0;
 82 |     float y_offset = 0.0;
 83 | 	
 84 | 	ofstream ofile;
 85 | 	float begin_time = ros::Time::now().toSec();
 86 | 	ROS_INFO("begin_time,%6.4f",begin_time);
 87 | 	
 88 | 	//takeoff action object
 89 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
 90 |     //land action object
 91 |     actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
 92 | 
 93 |     // Subscriber to the Copter gps
 94 |     string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
 95 |     ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
 96 | 
 97 |     // Subscriber to the neighbour gps
 98 |     string neigGps_ = nh.resolveName("neighbour_position");
 99 |     ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
100 | 
101 | 	//Publisher to the gps(toString)
102 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
103 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
104 | 
105 | 	//Publisher to the velocity of the copter
106 | 	string vel_pub_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
107 | 	ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>(vel_pub_, 5);
108 | 	
109 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
110 | 
111 | 	ac.waitForServer();	
112 | 	
113 | 	ROS_INFO("Action server started--Taking off!");
114 | 
115 | 	spiri_go::TakeoffGoal goal;
116 | 
117 | 	//set the target height to take off!
118 | 	goal.height = 3;
119 | 	
120 | 	ac.sendGoal(goal);
121 | 
122 | 	//wait for action(takeOff) to return
123 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
124 | 	ros::Duration(2).sleep();
125 | 	if(is_takeOff_finished ) {
126 | 		ROS_INFO("reached target height!");
127 |         ofile.open("./log/data.log", ios::trunc);
128 | 
129 | 		while(nh.ok()) {
130 | 			//1.publish self gps(toString)
131 | 			std_msgs::String gps_string;
132 |       		std::stringstream ss;
133 |       		ss << "o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
134 | 		//ss << "o"<<120.87654321<<"a"<<30.12345678<<"#";
135 |       		gps_string.data = ss.str();
136 |       		gps_string_pub.publish(gps_string);
137 | 	
138 | 			ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
139 | 			ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
140 |  
141 | 			//2.read neighbour gps
142 | 			ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
143 | 			ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
144 | 
145 | 			//3.calculate velocity(adaptive control)
146 | 			if(neigGlobalPos.lat == 0.0 || neigGlobalPos.lon == 0.0) {
147 | 				vel.twist.linear.x = 0.0;
148 | 				vel.twist.linear.y = 0.0;
149 | 			}
150 | 			else {
151 | 				x_1 = float(PI/180) * R * neigGlobalPos.lat;
152 | 				y_1 = float(PI/180) * R * neigGlobalPos.lon;
153 | 				x_0 = float(PI/180) * R * selfGlobalPos.latitude;
154 | 				y_0 = float(PI/180) * R * selfGlobalPos.longitude;
155 | 				
156 | /*
157 | 				currentDisPow = pow(x_1-x_0, 2) + pow(y_1-y_0, 2);
158 | 
159 | 				vel.twist.linear.x = vx0 + (x_1 - x_0)*(currentDisPow - pow(d12, 2))*0.01;
160 | 				vel.twist.linear.y = vy0 + (y_1 - y_0)*(currentDisPow - pow(d12, 2))*0.01;
161 | 
162 | 				vel.twist.linear.y = -1 * vel.twist.linear.y;  */
163 | 
164 | 				x_offset = x_0 - x_1;
165 |                 y_offset = y_0 - y_1;
166 | 				
167 | 				if(abs(x_offset) <= r1) {
168 | 				    vel.twist.linear.x = -8*( (pow(x_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset ) / pow((pow(x_offset,2)-pow(r,2)),3);
169 | 				}
170 |                 else {
171 |   					vel.twist.linear.x = 0;
172 |                 }
173 | 				if(abs(y_offset) <= r1) {
174 | 				    vel.twist.linear.y = -8*( (pow(y_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset ) / pow((pow(y_offset,2)-pow(r,2)),3);
175 | 				}
176 |                 else {
177 |   					vel.twist.linear.y = 0;
178 |                 }
179 | 			}
180 | 
181 | 			
182 | 			if(vel.twist.linear.x >= 1) {
183 | 				vel.twist.linear.x = 1;
184 | 			}else if(vel.twist.linear.x <= -1) {
185 | 				vel.twist.linear.x = -1;
186 | 			}
187 | 			if(vel.twist.linear.y >= 1) {
188 | 				vel.twist.linear.y = 1;
189 | 			}else if(vel.twist.linear.y <= -1) {
190 | 				vel.twist.linear.y = -1;
191 | 			}
192 | 			
193 | 
194 | 			vel.header.stamp = ros::Time::now();
195 | 
196 | 			//4.publish velocity to mavros 
197 | 			vel_pub.publish(vel);
198 | 			//ROS_INFO("velocity_x:%f", vel.twist.linear.x);
199 | 			//ROS_INFO("velocity_y:%f", vel.twist.linear.y);
200 |             ofile << vel.twist.linear.x << " " << vel.twist.linear.y << x_0 << y_0 << x_1 << y_1 << std::endl;
201 | 
202 |             // if fly for 4 minutes
203 | 		    if( (ros::Time::now().toSec() - begin_time) > 4*60 ) {
204 | 				//land
205 | 				ROS_INFO("Waiting for action server(land) to start.");
206 | 				ac1.waitForServer();	
207 | 				ROS_INFO("Start to land!.");
208 | 	
209 | 				spiri_go::LandHereGoal goal_1;
210 | 				goal_1.height = 0;
211 | 				ac1.sendGoal(goal_1);
212 | 
213 | 				//wait for action(land) to return
214 | 				bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
215 | 				if(is_land_finished ) {
216 | 					ROS_INFO("Land success!");
217 | 				}
218 |             }
219 | 		
220 | 			ros::spinOnce();
221 |     		loop_rate.sleep();
222 | 		}		
223 | 	}
224 | 	else {
225 | 		ROS_INFO("should set mode Land!");
226 | 	}
227 | 
228 | 	return 0;
229 | 
230 | }
231 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/collision_avoidance.cpp~:
--------------------------------------------------------------------------------
  1 | 
  2 | /*******************************************************************************
  3 |  *  The Spiri Project
  4 |  *
  5 |  *  File: collision_avoidance.cpp
  6 |  *
  7 |  *  Purpose: collision avoidance control using releative position.
  8 |  *
  9 |  *  @author Peng Cheng
 10 |  *  @version 0.1.0    2016/3/30
 11 |  ******************************************************************************/
 12 | 
 13 | #include "ros/ros.h"
 14 | #include <actionlib/client/simple_action_client.h>
 15 | #include <actionlib/client/terminal_state.h>
 16 | #include <spiri_go/TakeoffAction.h>
 17 | #include "spiri_go.h"
 18 | #include <std_msgs/String.h>
 19 | #include <sensor_msgs/NavSatFix.h>
 20 | #include <geometry_msgs/TwistStamped.h>
 21 | #include <sstream>
 22 | #include <px4ros/gpsPosition.h>
 23 | #include <math.h>
 24 | #include <fstream>
 25 | 
 26 | 
 27 | //global variables
 28 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
 29 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
 30 | 
 31 | #define R  6371000         //meter
 32 | #define PI 3.1415926535898
 33 | 
 34 | /*******************************************************************************
 35 |  *  Callback function on the gps of the copter
 36 |  *
 37 |  *  @param gpsPositionConstPtr& the gps callback pointer
 38 |  ******************************************************************************/
 39 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
 40 |    selfGlobalPos = *msg;
 41 | }
 42 | 
 43 | /*******************************************************************************
 44 |  *  Callback function on the gps of the neighbour copter
 45 |  *
 46 |  *  @param gpsPositionConstPtr& the gps callback pointer
 47 |  ******************************************************************************/
 48 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
 49 | {
 50 |     neigGlobalPos = *msg;
 51 | }
 52 | 
 53 | /*******************************************************************************
 54 |  *  Main function
 55 |  *
 56 |  *  @param 
 57 |  ******************************************************************************/
 58 | int main(int argc, char **argv) {
 59 | 	ros::init(argc, argv, "spiri_go_client");
 60 | 	ros::NodeHandle nh;
 61 | 
 62 | 	ros::Rate loop_rate(10.0);
 63 | 	
 64 | 	//velocity to publish
 65 | 	geometry_msgs::TwistStamped vel;
 66 | 	vel.twist.linear.x = 0.0;
 67 | 	vel.twist.linear.y = 0.0;
 68 | 	vel.header.stamp = ros::Time::now();
 69 | 
 70 | 	//variables definition
 71 | 	float x_0 = 0.0;
 72 | 	float y_0 = 0.0;
 73 | 	float x_1 = 0.0;
 74 | 	float y_1 = 0.0;
 75 | 	float d12 = 10;   //meter,two leaders mantain the distance
 76 |     float r = 1.0;     // the region that collision avoidance algorithm will take effect
 77 |     float r1 = 5.0;     // the region that collision avoidance algorithm will take effect
 78 | 	float vx0 = 0.0;
 79 | 	float vy0 = 0.0;
 80 | 	float currentDisPow = 0.0;
 81 |     float x_offset = 0.0;
 82 |     float y_offset = 0.0;
 83 | 	
 84 | 	ofstream ofile;
 85 | 	float begin_time = ros::Time::now().toSec();
 86 | 	ROS_INFO("begin_time,%6.4f",begin_time);
 87 | 	
 88 | 	//takeoff action object
 89 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
 90 |     //land action object
 91 |     actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
 92 | 
 93 |     // Subscriber to the Copter gps
 94 |     string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
 95 |     ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
 96 | 
 97 |     // Subscriber to the neighbour gps
 98 |     string neigGps_ = nh.resolveName("neighbour_position");
 99 |     ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
100 | 
101 | 	//Publisher to the gps(toString)
102 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
103 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
104 | 
105 | 	//Publisher to the velocity of the copter
106 | 	string vel_pub_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
107 | 	ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>(vel_pub_, 5);
108 | 	
109 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
110 | 
111 | 	ac.waitForServer();	
112 | 	
113 | 	ROS_INFO("Action server started--Taking off!");
114 | 
115 | 	spiri_go::TakeoffGoal goal;
116 | 
117 | 	//set the target height to take off!
118 | 	goal.height = 3;
119 | 	
120 | 	ac.sendGoal(goal);
121 | 
122 | 	//wait for action(takeOff) to return
123 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
124 | 	ros::Duration(2).sleep();
125 | 	if(is_takeOff_finished ) {
126 | 		ROS_INFO("reached target height!");
127 |         ofile.open("./log/data.log", ios::trunc);
128 | 
129 | 		while(nh.ok()) {
130 | 			//1.publish self gps(toString)
131 | 			std_msgs::String gps_string;
132 |       		std::stringstream ss;
133 |       		ss << "o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
134 | 		//ss << "o"<<120.87654321<<"a"<<30.12345678<<"#";
135 |       		gps_string.data = ss.str();
136 |       		gps_string_pub.publish(gps_string);
137 | 	
138 | 			ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
139 | 			ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
140 |  
141 | 			//2.read neighbour gps
142 | 			ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
143 | 			ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
144 | 
145 | 			//3.calculate velocity(adaptive control)
146 | 			if(neigGlobalPos.lat == 0.0 || neigGlobalPos.lon == 0.0) {
147 | 				vel.twist.linear.x = 0.0;
148 | 				vel.twist.linear.y = 0.0;
149 | 			}
150 | 			else {
151 | 				x_1 = float(PI/180) * R * neigGlobalPos.lat;
152 | 				y_1 = float(PI/180) * R * neigGlobalPos.lon;
153 | 				x_0 = float(PI/180) * R * selfGlobalPos.latitude;
154 | 				y_0 = float(PI/180) * R * selfGlobalPos.longitude;
155 | 				
156 | /*
157 | 				currentDisPow = pow(x_1-x_0, 2) + pow(y_1-y_0, 2);
158 | 
159 | 				vel.twist.linear.x = vx0 + (x_1 - x_0)*(currentDisPow - pow(d12, 2))*0.01;
160 | 				vel.twist.linear.y = vy0 + (y_1 - y_0)*(currentDisPow - pow(d12, 2))*0.01;
161 | 
162 | 				vel.twist.linear.y = -1 * vel.twist.linear.y;  */
163 | 
164 | 				x_offset = x_0 - x_1;
165 |                 y_offset = y_0 - y_1;
166 | 				
167 | 				if(abs(x_offset) <= r1) {
168 | 				    vel.twist.linear.x = -8*( (pow(x_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset ) / pow((pow(x_offset,2)-pow(r,2)),3)*0.01;
169 | 				}
170 |                 else {
171 |   					vel.twist.linear.x = 0;
172 |                 }
173 | 				if(abs(y_offset) <= r1) {
174 | 				    vel.twist.linear.y = -8*( (pow(y_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset ) / pow((pow(y_offset,2)-pow(r,2)),3)*0.01;
175 | 				}
176 |                 else {
177 |   					vel.twist.linear.y = 0;
178 |                 }
179 | 			}
180 | 
181 | 			
182 | 			if(vel.twist.linear.x >= 1) {
183 | 				vel.twist.linear.x = 1;
184 | 			}else if(vel.twist.linear.x <= -1) {
185 | 				vel.twist.linear.x = -1;
186 | 			}
187 | 			if(vel.twist.linear.y >= 1) {
188 | 				vel.twist.linear.y = 1;
189 | 			}else if(vel.twist.linear.y <= -1) {
190 | 				vel.twist.linear.y = -1;
191 | 			}
192 | 			
193 | 
194 | 			vel.header.stamp = ros::Time::now();
195 | 
196 | 			//4.publish velocity to mavros 
197 | 			vel_pub.publish(vel);
198 | 			//ROS_INFO("velocity_x:%f", vel.twist.linear.x);
199 | 			//ROS_INFO("velocity_y:%f", vel.twist.linear.y);
200 |             ofile << vel.twist.linear.x << " " << vel.twist.linear.y << x_0 << y_0 << x_1 << y_1 << std::endl;
201 | 
202 |             // if fly for 4 minutes
203 | 		    if( (ros::Time::now().toSec() - begin_time) > 4*60 ) {
204 | 				//land
205 | 				ROS_INFO("Waiting for action server(land) to start.");
206 | 				ac1.waitForServer();	
207 | 				ROS_INFO("Start to land!.");
208 | 	
209 | 				spiri_go::LandHereGoal goal_1;
210 | 				goal_1.height = 0;
211 | 				ac1.sendGoal(goal_1);
212 | 
213 | 				//wait for action(land) to return
214 | 				bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
215 | 				if(is_land_finished ) {
216 | 					ROS_INFO("Land success!");
217 | 				}
218 |             }
219 | 		
220 | 			ros::spinOnce();
221 |     		loop_rate.sleep();
222 | 		}		
223 | 	}
224 | 	else {
225 | 		ROS_INFO("should set mode Land!");
226 | 	}
227 | 
228 | 	return 0;
229 | 
230 | }
231 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/follower.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | /*******************************************************************************
  3 |  *  The Spiri Project
  4 |  *
  5 |  *  File: leader.cpp
  6 |  *
  7 |  *  Purpose: Formation control of leaders with collision avoidance using releative position.
  8 |  *
  9 |  *  @author Peng Cheng
 10 |  *  @version 0.1.0    2016/4/06
 11 |  ******************************************************************************/
 12 | 
 13 | #include "ros/ros.h"
 14 | #include <actionlib/client/simple_action_client.h>
 15 | #include <actionlib/client/terminal_state.h>
 16 | #include <spiri_go/TakeoffAction.h>
 17 | #include "spiri_go.h"
 18 | #include <std_msgs/String.h>
 19 | #include <sensor_msgs/NavSatFix.h>
 20 | #include <geometry_msgs/TwistStamped.h>
 21 | #include <sstream>
 22 | #include <px4ros/gpsPosition.h>
 23 | #include <math.h>
 24 | #include <fstream>
 25 | 
 26 | 
 27 | //global variables
 28 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
 29 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
 30 | px4ros::gpsPosition neigGlobalPos_2;
 31 | 
 32 | #define R  6371000         //meter
 33 | #define PI 3.1415926535898
 34 | #define T 0.1
 35 | 
 36 | /*******************************************************************************
 37 |  *  Callback function on the gps of the copter
 38 |  *
 39 |  *  @param gpsPositionConstPtr& the gps callback pointer
 40 |  ******************************************************************************/
 41 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
 42 |    selfGlobalPos = *msg;
 43 | }
 44 | 
 45 | /*******************************************************************************
 46 |  *  Callback function on the gps of the neighbour copter
 47 |  *
 48 |  *  @param gpsPositionConstPtr& the gps callback pointer
 49 |  ******************************************************************************/
 50 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
 51 | {
 52 |     neigGlobalPos = *msg;
 53 | }
 54 | 
 55 | /*******************************************************************************
 56 |  *  Callback function on the gps of the neighbour copter
 57 |  *
 58 |  *  @param gpsPositionConstPtr& the gps callback pointer
 59 |  ******************************************************************************/
 60 | void neigGpsSubCb2(const px4ros::gpsPositionConstPtr& msg)
 61 | {
 62 |     neigGlobalPos_2 = *msg;
 63 | }
 64 | 
 65 | /*******************************************************************************
 66 |  *  Main function
 67 |  *
 68 |  *  @param 
 69 |  ******************************************************************************/
 70 | int main(int argc, char **argv) {
 71 | 	ros::init(argc, argv, "spiri_go_client");
 72 | 	ros::NodeHandle nh;
 73 | 
 74 | 	ros::Rate loop_rate(10.0);
 75 | 	
 76 | 	//velocity to publish
 77 | 	geometry_msgs::TwistStamped vel;
 78 | 	vel.twist.linear.x = 0.0;
 79 | 	vel.twist.linear.y = 0.0;
 80 | 	vel.header.stamp = ros::Time::now();
 81 | 
 82 | 	//variables definition
 83 | 	float x_0 = 0.0;
 84 | 	float y_0 = 0.0;
 85 | 	float x_1 = 0.0;
 86 | 	float y_1 = 0.0;
 87 | 	float x_2 = 0.0;
 88 | 	float y_2 = 0.0;
 89 | 	float d01 = 10;   //meter
 90 | 	float d02 = 10;
 91 |     	float r = 1.0;     // the region that collision avoidance algorithm will take effect
 92 |     	float r1 = 5.0;     // the region that collision avoidance algorithm will take effect
 93 | 	float vx0 = 0.0;
 94 | 	float vy0 = 0.0;
 95 | 	float distancePow_01 = 0.0;
 96 | 	float distancePow_02 = 0.0;
 97 | 	float diff_theta_x = 0.0;
 98 | 	float diff_theta_y = 0.0;
 99 | 	float theta_x = 0.0;
100 | 	float theta_y = 0.0;
101 | 	float last_theta_x = 0.0;
102 | 	float last_theta_y = 0.0;
103 | 
104 |     	float x_offset = 0.0;
105 |     	float y_offset = 0.0;
106 |     	float x_offset_ = 0.0;
107 |     	float y_offset_ = 0.0;
108 | 	
109 | 	ofstream ofile;
110 | 	float begin_time = ros::Time::now().toSec();
111 | 	ROS_INFO("begin_time,%6.4f",begin_time);
112 | 	
113 | 	//takeoff action object
114 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
115 | 	//land action object
116 | 	actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
117 | 
118 | 	// Subscriber to the Copter gps
119 | 	string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
120 | 	ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
121 | 
122 | 	// Subscriber to the neighbour gps
123 | 	string neigGps_ = nh.resolveName("neighbour_position");
124 | 	ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
125 | 
126 | 	// Subscriber to the neighbour gps
127 | 	string neigGps2_ = nh.resolveName("neighbour_position_2");
128 | 	ros::Subscriber neigGps2 = nh.subscribe(neigGps2_, 5, neigGpsSubCb2);
129 | 
130 | 	//Publisher to the gps(toString)
131 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
132 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
133 | 
134 | 	//Publisher to the velocity of the copter
135 | 	string vel_pub_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
136 | 	ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>(vel_pub_, 5);
137 | 	
138 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
139 | 
140 | 	ac.waitForServer();	
141 | 	
142 | 	ROS_INFO("Action server started--Taking off!");
143 | 
144 | 	spiri_go::TakeoffGoal goal;
145 | 
146 | 	//set the target height to take off!
147 | 	goal.height = 3;
148 | 	
149 | 	ac.sendGoal(goal);
150 | 
151 | 	//wait for action(takeOff) to return
152 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
153 | 	ros::Duration(5).sleep();
154 | 	if(is_takeOff_finished ) {
155 | 		ROS_INFO("reached target height!");
156 |         	ofile.open("./log/data.log", ios::trunc);
157 | 
158 | 		while(nh.ok()) {
159 | 			//1.publish self gps(toString)
160 | 			std_msgs::String gps_string;
161 |       			std::stringstream ss;
162 |       			ss << "x3o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
163 | 			//ss << "o"<<120.87654321<<"a"<<30.12345678<<"#";
164 |       			gps_string.data = ss.str();
165 |       			gps_string_pub.publish(gps_string);
166 | 	
167 | 			ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
168 | 			ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
169 |  
170 | 			//2.read neighbour gps
171 | 			ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
172 | 			ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
173 | 
174 | 			//3.calculate velocity(adaptive control)
175 | 			if(neigGlobalPos.lat == 0.0 || neigGlobalPos.lon == 0.0
176 | 				|| neigGlobalPos_2.lat == 0.0 || neigGlobalPos_2.lon == 0.0) {
177 | 				vel.twist.linear.x = 0.0;
178 | 				vel.twist.linear.y = 0.0;
179 | 			}
180 | 			else {
181 | 				x_1 = float(PI/180) * R * neigGlobalPos.lat;
182 | 				y_1 = float(PI/180) * R * neigGlobalPos.lon;
183 | 				x_2 = float(PI/180) * R * neigGlobalPos_2.lat;
184 | 				y_2 = float(PI/180) * R * neigGlobalPos_2.lon;
185 | 				x_0 = float(PI/180) * R * selfGlobalPos.latitude;
186 | 				y_0 = float(PI/180) * R * selfGlobalPos.longitude;
187 | 				
188 | 				// formation control 
189 | 				distancePow_01 = pow(x_1-x_0, 2) + pow(y_1-y_0, 2);
190 | 				distancePow_02 = pow(x_2-x_0, 2) + pow(y_2-y_0, 2);
191 | 				diff_theta_x = ( (x_1-x_0) * (distancePow_01 - pow(d01, 2)) + (x_2-x_0) * (distancePow_02 - pow(d02,2)) ) * 0.01;
192 | 				theta_x = (T * diff_theta_x + last_theta_x) * 0.01;
193 | 				last_theta_x = theta_x;
194 | 				vel.twist.linear.x = theta_x + diff_theta_x;
195 | 
196 | 				diff_theta_y = ( (y_1-y_0) * (distancePow_01 - pow(d01, 2)) + (y_2-y_0) * (distancePow_02 - pow(d02,2)) ) * 0.01;
197 | 				theta_y = (T * diff_theta_y + last_theta_y) * 0.01;
198 | 				last_theta_y = theta_y;
199 | 				vel.twist.linear.y = theta_y + diff_theta_y;
200 | 
201 | 
202 | 				// collision avoidance control
203 | 				x_offset = x_0 - x_1;
204 |                 		y_offset = y_0 - y_1;
205 | 				x_offset_ = x_0 - x_2;
206 |                 		y_offset_ = y_0 - y_2;
207 | 				
208 | 				if( (abs(x_offset) <= r1) && (abs(x_offset_) <= r1) ){
209 | 				    vel.twist.linear.x = -8*( (pow(x_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset ) / pow((pow(x_offset,2)-pow(r,2)),3)
210 | 						       + (-8*( (pow(x_offset_,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset_ ) / pow((pow(x_offset_,2)-pow(r,2)),3));
211 | 				}
212 | 				else if(abs(x_offset) <= r1) {
213 | 				    vel.twist.linear.x = -8*( (pow(x_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset ) / pow((pow(x_offset,2)-pow(r,2)),3);
214 | 				}
215 | 				else if(abs(x_offset_) <= r1) {
216 | 				    vel.twist.linear.x = -8*( (pow(x_offset_,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset_ ) / pow((pow(x_offset_,2)-pow(r,2)),3);
217 | 				}
218 |                 		else {
219 |   					vel.twist.linear.x = 0;
220 |                 		}
221 | 
222 | 				if( (abs(y_offset) <= r1) && (abs(y_offset_) <= r1) ) {
223 | 				    vel.twist.linear.y = -8*( (pow(y_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset ) / pow((pow(y_offset,2)-pow(r,2)),3)
224 |                                                        + (-8*( (pow(y_offset_,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset_ ) / pow((pow(y_offset_,2)-pow(r,2)),3));
225 | 				}
226 | 				else if(abs(y_offset) <= r1) {
227 | 				    vel.twist.linear.y = -8*( (pow(y_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset ) / pow((pow(y_offset,2)-pow(r,2)),3);
228 | 				}
229 | 				else if(abs(y_offset_) <= r1) {
230 | 				    vel.twist.linear.y = -8*( (pow(y_offset_,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset_ ) / pow((pow(y_offset_,2)-pow(r,2)),3);
231 | 				}
232 |                 		else {
233 |   					vel.twist.linear.y = 0;
234 |                 		}
235 | 			}
236 | 
237 | 			
238 | 			if(vel.twist.linear.x >= 1) {
239 | 				vel.twist.linear.x = 1;
240 | 			}else if(vel.twist.linear.x <= -1) {
241 | 				vel.twist.linear.x = -1;
242 | 			}
243 | 			if(vel.twist.linear.y >= 1) {
244 | 				vel.twist.linear.y = 1;
245 | 			}else if(vel.twist.linear.y <= -1) {
246 | 				vel.twist.linear.y = -1;
247 | 			}
248 | 			
249 | 
250 | 			vel.header.stamp = ros::Time::now();
251 | 
252 | 			//4.publish velocity to mavros 
253 | 			vel_pub.publish(vel);
254 | 			//ROS_INFO("velocity_x:%f", vel.twist.linear.x);
255 | 			//ROS_INFO("velocity_y:%f", vel.twist.linear.y);
256 |             		ofile << vel.twist.linear.x << " " << vel.twist.linear.y << x_0 << y_0 << x_1 << y_1 << std::endl;
257 | 
258 |             		// if fly for 4 minutes
259 | 		    	if( (ros::Time::now().toSec() - begin_time) > 4*60 ) {
260 | 				//land
261 | 				ROS_INFO("Waiting for action server(land) to start.");
262 | 				ac1.waitForServer();	
263 | 				ROS_INFO("Start to land!.");
264 | 	
265 | 				spiri_go::LandHereGoal goal_1;
266 | 				goal_1.height = 0;
267 | 				ac1.sendGoal(goal_1);
268 | 
269 | 				//wait for action(land) to return
270 | 				bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
271 | 				if(is_land_finished ) {
272 | 					ROS_INFO("Land success!");
273 | 				}
274 |             		}
275 | 		
276 | 			ros::spinOnce();
277 |     			loop_rate.sleep();
278 | 		}		
279 | 	}
280 | 	else {
281 | 		ROS_INFO("should set mode Land!");
282 | 	}
283 | 
284 | 	return 0;
285 | 
286 | }
287 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/follower.cpp~:
--------------------------------------------------------------------------------
  1 | 
  2 | /*******************************************************************************
  3 |  *  The Spiri Project
  4 |  *
  5 |  *  File: leader.cpp
  6 |  *
  7 |  *  Purpose: Formation control of leaders with collision avoidance using releative position.
  8 |  *
  9 |  *  @author Peng Cheng
 10 |  *  @version 0.1.0    2016/4/06
 11 |  ******************************************************************************/
 12 | 
 13 | #include "ros/ros.h"
 14 | #include <actionlib/client/simple_action_client.h>
 15 | #include <actionlib/client/terminal_state.h>
 16 | #include <spiri_go/TakeoffAction.h>
 17 | #include "spiri_go.h"
 18 | #include <std_msgs/String.h>
 19 | #include <sensor_msgs/NavSatFix.h>
 20 | #include <geometry_msgs/TwistStamped.h>
 21 | #include <sstream>
 22 | #include <px4ros/gpsPosition.h>
 23 | #include <math.h>
 24 | #include <fstream>
 25 | 
 26 | 
 27 | //global variables
 28 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
 29 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
 30 | px4ros::gpsPosition neigGlobalPos_2;
 31 | 
 32 | #define R  6371000         //meter
 33 | #define PI 3.1415926535898
 34 | #define T 0.1
 35 | 
 36 | /*******************************************************************************
 37 |  *  Callback function on the gps of the copter
 38 |  *
 39 |  *  @param gpsPositionConstPtr& the gps callback pointer
 40 |  ******************************************************************************/
 41 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
 42 |    selfGlobalPos = *msg;
 43 | }
 44 | 
 45 | /*******************************************************************************
 46 |  *  Callback function on the gps of the neighbour copter
 47 |  *
 48 |  *  @param gpsPositionConstPtr& the gps callback pointer
 49 |  ******************************************************************************/
 50 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
 51 | {
 52 |     neigGlobalPos = *msg;
 53 | }
 54 | 
 55 | /*******************************************************************************
 56 |  *  Callback function on the gps of the neighbour copter
 57 |  *
 58 |  *  @param gpsPositionConstPtr& the gps callback pointer
 59 |  ******************************************************************************/
 60 | void neigGpsSubCb2(const px4ros::gpsPositionConstPtr& msg)
 61 | {
 62 |     neigGlobalPos_2 = *msg;
 63 | }
 64 | 
 65 | /*******************************************************************************
 66 |  *  Main function
 67 |  *
 68 |  *  @param 
 69 |  ******************************************************************************/
 70 | int main(int argc, char **argv) {
 71 | 	ros::init(argc, argv, "spiri_go_client");
 72 | 	ros::NodeHandle nh;
 73 | 
 74 | 	ros::Rate loop_rate(10.0);
 75 | 	
 76 | 	//velocity to publish
 77 | 	geometry_msgs::TwistStamped vel;
 78 | 	vel.twist.linear.x = 0.0;
 79 | 	vel.twist.linear.y = 0.0;
 80 | 	vel.header.stamp = ros::Time::now();
 81 | 
 82 | 	//variables definition
 83 | 	float x_0 = 0.0;
 84 | 	float y_0 = 0.0;
 85 | 	float x_1 = 0.0;
 86 | 	float y_1 = 0.0;
 87 | 	float x_2 = 0.0;
 88 | 	float y_2 = 0.0;
 89 | 	float d01 = 10;   //meter
 90 | 	float d02 = 10;
 91 |     	float r = 1.0;     // the region that collision avoidance algorithm will take effect
 92 |     	float r1 = 5.0;     // the region that collision avoidance algorithm will take effect
 93 | 	float vx0 = 0.0;
 94 | 	float vy0 = 0.0;
 95 | 	float distancePow_01 = 0.0;
 96 | 	float distancePow_02 = 0.0;
 97 | 	float diff_theta_x = 0.0;
 98 | 	float diff_theta_y = 0.0;
 99 | 	float theta_x = 0.0;
100 | 	float theta_y = 0.0;
101 | 	float last_theta_x = 0.0;
102 | 	float last_theta_y = 0.0;
103 | 
104 |     	float x_offset = 0.0;
105 |     	float y_offset = 0.0;
106 | 	
107 | 	ofstream ofile;
108 | 	float begin_time = ros::Time::now().toSec();
109 | 	ROS_INFO("begin_time,%6.4f",begin_time);
110 | 	
111 | 	//takeoff action object
112 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
113 | 	//land action object
114 | 	actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
115 | 
116 | 	// Subscriber to the Copter gps
117 | 	string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
118 | 	ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
119 | 
120 | 	// Subscriber to the neighbour gps
121 | 	string neigGps_ = nh.resolveName("neighbour_position");
122 | 	ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
123 | 
124 | 	// Subscriber to the neighbour gps
125 | 	string neigGps2_ = nh.resolveName("neighbour_position_2");
126 | 	ros::Subscriber neigGps2 = nh.subscribe(neigGps2_, 5, neigGpsSubCb2);
127 | 
128 | 	//Publisher to the gps(toString)
129 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
130 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
131 | 
132 | 	//Publisher to the velocity of the copter
133 | 	string vel_pub_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
134 | 	ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>(vel_pub_, 5);
135 | 	
136 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
137 | 
138 | 	ac.waitForServer();	
139 | 	
140 | 	ROS_INFO("Action server started--Taking off!");
141 | 
142 | 	spiri_go::TakeoffGoal goal;
143 | 
144 | 	//set the target height to take off!
145 | 	goal.height = 3;
146 | 	
147 | 	ac.sendGoal(goal);
148 | 
149 | 	//wait for action(takeOff) to return
150 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
151 | 	ros::Duration(5).sleep();
152 | 	if(is_takeOff_finished ) {
153 | 		ROS_INFO("reached target height!");
154 |         	ofile.open("./log/data.log", ios::trunc);
155 | 
156 | 		while(nh.ok()) {
157 | 			//1.publish self gps(toString)
158 | 			std_msgs::String gps_string;
159 |       			std::stringstream ss;
160 |       			ss << "01o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
161 | 			//ss << "o"<<120.87654321<<"a"<<30.12345678<<"#";
162 |       			gps_string.data = ss.str();
163 |       			gps_string_pub.publish(gps_string);
164 | 	
165 | 			ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
166 | 			ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
167 |  
168 | 			//2.read neighbour gps
169 | 			ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
170 | 			ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
171 | 
172 | 			//3.calculate velocity(adaptive control)
173 | 			if(neigGlobalPos.lat == 0.0 || neigGlobalPos.lon == 0.0
174 | 				|| neigGlobalPos_2.lat == 0.0 || neigGlobalPos_2.lon == 0.0) {
175 | 				vel.twist.linear.x = 0.0;
176 | 				vel.twist.linear.y = 0.0;
177 | 			}
178 | 			else {
179 | 				x_1 = float(PI/180) * R * neigGlobalPos.lat;
180 | 				y_1 = float(PI/180) * R * neigGlobalPos.lon;
181 | 				x_2 = float(PI/180) * R * neigGlobalPos_2.lat;
182 | 				y_2 = float(PI/180) * R * neigGlobalPos_2.lon;
183 | 				x_0 = float(PI/180) * R * selfGlobalPos.latitude;
184 | 				y_0 = float(PI/180) * R * selfGlobalPos.longitude;
185 | 				
186 | 				// formation control 
187 | 				distancePow_01 = pow(x_1-x_0, 2) + pow(y_1-y_0, 2);
188 | 				distancePow_02 = pow(x_2-x_0, 2) + pow(y_2-y_0, 2);
189 | 				diff_theta_x = ( (x_1-x_0) * (distancePow_01 - pow(d01, 2)) + (x_2-x_0) * (distancePow_02 - pow(d02,2)) ) * 0.01;
190 | 				theta_x = (T * diff_theta_x + last_theta_x) * 0.01;
191 | 				last_theta_x = theta_x;
192 | 				vel.twist.linear.x = theta_x + diff_theta_x;
193 | 
194 | 				diff_theta_y = ( (y_1-y_0) * (distancePow_01 - pow(d01, 2)) + (y_2-y_0) * (distancePow_02 - pow(d02,2)) ) * 0.01;
195 | 				theta_y = (T * diff_theta_y + last_theta_y) * 0.01;
196 | 				last_theta_y = theta_y;
197 | 				vel.twist.linear.y = theta_y + diff_theta_y;
198 | 
199 | 
200 | 				// collision avoidance control
201 | 				x_offset = x_0 - x_1;
202 |                 		y_offset = y_0 - y_1;
203 | 				x_offset_ = x_0 - x_2;
204 |                 		y_offset_ = y_0 - y_2;
205 | 				
206 | 				if(abs(x_offset) <= r1) {
207 | 				    vel.twist.linear.x = -8*( (pow(x_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset ) / pow((pow(x_offset,2)-pow(r,2)),3)
208 | 						       + (-8*( (pow(x_offset_,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset_ ) / pow((pow(x_offset_,2)-pow(r,2)),3));
209 | 				}
210 |                 		else {
211 |   					vel.twist.linear.x = 0;
212 |                 		}
213 | 
214 | 				if(abs(y_offset) <= r1) {
215 | 				    vel.twist.linear.y = -8*( (pow(y_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset ) / pow((pow(y_offset,2)-pow(r,2)),3)
216 |                                                        + (-8*( (pow(y_offset_,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset_ ) / pow((pow(y_offset_,2)-pow(r,2)),3));
217 | 				}
218 |                 		else {
219 |   					vel.twist.linear.y = 0;
220 |                 		}
221 | 			}
222 | 
223 | 			
224 | 			if(vel.twist.linear.x >= 1) {
225 | 				vel.twist.linear.x = 1;
226 | 			}else if(vel.twist.linear.x <= -1) {
227 | 				vel.twist.linear.x = -1;
228 | 			}
229 | 			if(vel.twist.linear.y >= 1) {
230 | 				vel.twist.linear.y = 1;
231 | 			}else if(vel.twist.linear.y <= -1) {
232 | 				vel.twist.linear.y = -1;
233 | 			}
234 | 			
235 | 
236 | 			vel.header.stamp = ros::Time::now();
237 | 
238 | 			//4.publish velocity to mavros 
239 | 			vel_pub.publish(vel);
240 | 			//ROS_INFO("velocity_x:%f", vel.twist.linear.x);
241 | 			//ROS_INFO("velocity_y:%f", vel.twist.linear.y);
242 |             		ofile << vel.twist.linear.x << " " << vel.twist.linear.y << x_0 << y_0 << x_1 << y_1 << std::endl;
243 | 
244 |             		// if fly for 4 minutes
245 | 		    	if( (ros::Time::now().toSec() - begin_time) > 4*60 ) {
246 | 				//land
247 | 				ROS_INFO("Waiting for action server(land) to start.");
248 | 				ac1.waitForServer();	
249 | 				ROS_INFO("Start to land!.");
250 | 	
251 | 				spiri_go::LandHereGoal goal_1;
252 | 				goal_1.height = 0;
253 | 				ac1.sendGoal(goal_1);
254 | 
255 | 				//wait for action(land) to return
256 | 				bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
257 | 				if(is_land_finished ) {
258 | 					ROS_INFO("Land success!");
259 | 				}
260 |             		}
261 | 		
262 | 			ros::spinOnce();
263 |     			loop_rate.sleep();
264 | 		}		
265 | 	}
266 | 	else {
267 | 		ROS_INFO("should set mode Land!");
268 | 	}
269 | 
270 | 	return 0;
271 | 
272 | }
273 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/leader.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | /*******************************************************************************
  3 |  *  The Spiri Project
  4 |  *
  5 |  *  File: leader.cpp
  6 |  *
  7 |  *  Purpose: Formation control of leaders with collision avoidance using releative position.
  8 |  *
  9 |  *  @author Peng Cheng
 10 |  *  @version 0.1.0    2016/4/06
 11 |  ******************************************************************************/
 12 | 
 13 | #include "ros/ros.h"
 14 | #include <actionlib/client/simple_action_client.h>
 15 | #include <actionlib/client/terminal_state.h>
 16 | #include <spiri_go/TakeoffAction.h>
 17 | #include "spiri_go.h"
 18 | #include <std_msgs/String.h>
 19 | #include <sensor_msgs/NavSatFix.h>
 20 | #include <geometry_msgs/TwistStamped.h>
 21 | #include <sstream>
 22 | #include <px4ros/gpsPosition.h>
 23 | #include <math.h>
 24 | #include <fstream>
 25 | 
 26 | 
 27 | //global variables
 28 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
 29 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
 30 | 
 31 | #define R  6371000         //meter
 32 | #define PI 3.1415926535898
 33 | 
 34 | /*******************************************************************************
 35 |  *  Callback function on the gps of the copter
 36 |  *
 37 |  *  @param gpsPositionConstPtr& the gps callback pointer
 38 |  ******************************************************************************/
 39 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
 40 |    selfGlobalPos = *msg;
 41 | }
 42 | 
 43 | /*******************************************************************************
 44 |  *  Callback function on the gps of the neighbour copter
 45 |  *
 46 |  *  @param gpsPositionConstPtr& the gps callback pointer
 47 |  ******************************************************************************/
 48 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
 49 | {
 50 |     neigGlobalPos = *msg;
 51 | }
 52 | 
 53 | /*******************************************************************************
 54 |  *  Main function
 55 |  *
 56 |  *  @param 
 57 |  ******************************************************************************/
 58 | int main(int argc, char **argv) {
 59 | 	ros::init(argc, argv, "spiri_go_client");
 60 | 	ros::NodeHandle nh;
 61 | 
 62 | 	ros::Rate loop_rate(10.0);
 63 | 	
 64 | 	//velocity to publish
 65 | 	geometry_msgs::TwistStamped vel;
 66 | 	vel.twist.linear.x = 0.0;
 67 | 	vel.twist.linear.y = 0.0;
 68 | 	vel.header.stamp = ros::Time::now();
 69 | 
 70 | 	//variables definition
 71 | 	float x_0 = 0.0;
 72 | 	float y_0 = 0.0;
 73 | 	float x_1 = 0.0;
 74 | 	float y_1 = 0.0;
 75 | 	float d12 = 10;   //meter,two leaders mantain the distance
 76 |     	float r = 1.0;     // the region that collision avoidance algorithm will take effect
 77 |     	float r1 = 5.0;     // the region that collision avoidance algorithm will take effect
 78 | 	float vx0 = 0.0;
 79 | 	float vy0 = 0.0;
 80 | 	float currentDisPow = 0.0;
 81 |     	float x_offset = 0.0;
 82 |     	float y_offset = 0.0;
 83 | 	
 84 | 	ofstream ofile;
 85 | 	float begin_time = ros::Time::now().toSec();
 86 | 	ROS_INFO("begin_time,%6.4f",begin_time);
 87 | 	
 88 | 	//takeoff action object
 89 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
 90 | 	//land action object
 91 | 	actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
 92 | 
 93 | 	// Subscriber to the Copter gps
 94 | 	string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
 95 | 	ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
 96 | 
 97 | 	// Subscriber to the neighbour gps
 98 | 	string neigGps_ = nh.resolveName("neighbour_position");
 99 | 	ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
100 | 
101 | 	//Publisher to the gps(toString)
102 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
103 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
104 | 
105 | 	//Publisher to the velocity of the copter
106 | 	string vel_pub_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
107 | 	ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>(vel_pub_, 5);
108 | 	
109 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
110 | 
111 | 	ac.waitForServer();	
112 | 	
113 | 	ROS_INFO("Action server started--Taking off!");
114 | 
115 | 	spiri_go::TakeoffGoal goal;
116 | 
117 | 	//set the target height to take off!
118 | 	goal.height = 3;
119 | 	
120 | 	ac.sendGoal(goal);
121 | 
122 | 	//wait for action(takeOff) to return
123 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
124 | 	ros::Duration(5).sleep();
125 | 	if(is_takeOff_finished ) {
126 | 		ROS_INFO("reached target height!");
127 |         	ofile.open("./log/data.log", ios::trunc);
128 | 
129 | 		while(nh.ok()) {
130 | 			//1.publish self gps(toString)
131 | 			std_msgs::String gps_string;
132 |       			std::stringstream ss;
133 |       			ss << "x1o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
134 | 			//ss << "o"<<120.87654321<<"a"<<30.12345678<<"#";
135 |       			gps_string.data = ss.str();
136 |       			gps_string_pub.publish(gps_string);
137 | 	
138 | 			ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
139 | 			ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
140 |  
141 | 			//2.read neighbour gps
142 | 			ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
143 | 			ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
144 | 
145 | 			//3.calculate velocity(adaptive control)
146 | 			if(neigGlobalPos.lat == 0.0 || neigGlobalPos.lon == 0.0) {
147 | 				vel.twist.linear.x = 0.0;
148 | 				vel.twist.linear.y = 0.0;
149 | 			}
150 | 			else {
151 | 				x_1 = float(PI/180) * R * neigGlobalPos.lat;
152 | 				y_1 = float(PI/180) * R * neigGlobalPos.lon;
153 | 				x_0 = float(PI/180) * R * selfGlobalPos.latitude;
154 | 				y_0 = float(PI/180) * R * selfGlobalPos.longitude;
155 | 				
156 | 				// formation control 
157 | 				currentDisPow = pow(x_1-x_0, 2) + pow(y_1-y_0, 2);
158 | 				vel.twist.linear.x = vx0 + (x_1 - x_0)*(currentDisPow - pow(d12, 2))*0.01;
159 | 				vel.twist.linear.y = vy0 + (y_1 - y_0)*(currentDisPow - pow(d12, 2))*0.01;
160 | 				//vel.twist.linear.y = -1 * vel.twist.linear.y;  
161 | 
162 | 				// collision avoidance control
163 | 				x_offset = x_0 - x_1;
164 |                 		y_offset = y_0 - y_1;
165 | 				
166 | 				if(abs(x_offset) <= r1) {
167 | 				    vel.twist.linear.x = -8*( (pow(x_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset ) / pow((pow(x_offset,2)-pow(r,2)),3);
168 | 				}
169 |                 		else {
170 |   					vel.twist.linear.x = 0;
171 |                 		}
172 | 
173 | 				if(abs(y_offset) <= r1) {
174 | 				    vel.twist.linear.y = -8*( (pow(y_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset ) / pow((pow(y_offset,2)-pow(r,2)),3);
175 | 				}
176 |                 		else {
177 |   					vel.twist.linear.y = 0;
178 |                 		}
179 | 			}
180 | 
181 | 			
182 | 			if(vel.twist.linear.x >= 1) {
183 | 				vel.twist.linear.x = 1;
184 | 			}else if(vel.twist.linear.x <= -1) {
185 | 				vel.twist.linear.x = -1;
186 | 			}
187 | 			if(vel.twist.linear.y >= 1) {
188 | 				vel.twist.linear.y = 1;
189 | 			}else if(vel.twist.linear.y <= -1) {
190 | 				vel.twist.linear.y = -1;
191 | 			}
192 | 			
193 | 
194 | 			vel.header.stamp = ros::Time::now();
195 | 
196 | 			//4.publish velocity to mavros 
197 | 			vel_pub.publish(vel);
198 | 			//ROS_INFO("velocity_x:%f", vel.twist.linear.x);
199 | 			//ROS_INFO("velocity_y:%f", vel.twist.linear.y);
200 |             		ofile << vel.twist.linear.x << " " << vel.twist.linear.y << x_0 << y_0 << x_1 << y_1 << std::endl;
201 | 
202 |             		// if fly for 4 minutes
203 | 		    	if( (ros::Time::now().toSec() - begin_time) > 4*60 ) {
204 | 				//land
205 | 				ROS_INFO("Waiting for action server(land) to start.");
206 | 				ac1.waitForServer();	
207 | 				ROS_INFO("Start to land!.");
208 | 	
209 | 				spiri_go::LandHereGoal goal_1;
210 | 				goal_1.height = 0;
211 | 				ac1.sendGoal(goal_1);
212 | 
213 | 				//wait for action(land) to return
214 | 				bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
215 | 				if(is_land_finished ) {
216 | 					ROS_INFO("Land success!");
217 | 				}
218 |             		}
219 | 		
220 | 			ros::spinOnce();
221 |     			loop_rate.sleep();
222 | 		}		
223 | 	}
224 | 	else {
225 | 		ROS_INFO("should set mode Land!");
226 | 	}
227 | 
228 | 	return 0;
229 | 
230 | }
231 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/leader.cpp~:
--------------------------------------------------------------------------------
  1 | 
  2 | /*******************************************************************************
  3 |  *  The Spiri Project
  4 |  *
  5 |  *  File: leader.cpp
  6 |  *
  7 |  *  Purpose: Formation control of leaders with collision avoidance using releative position.
  8 |  *
  9 |  *  @author Peng Cheng
 10 |  *  @version 0.1.0    2016/4/06
 11 |  ******************************************************************************/
 12 | 
 13 | #include "ros/ros.h"
 14 | #include <actionlib/client/simple_action_client.h>
 15 | #include <actionlib/client/terminal_state.h>
 16 | #include <spiri_go/TakeoffAction.h>
 17 | #include "spiri_go.h"
 18 | #include <std_msgs/String.h>
 19 | #include <sensor_msgs/NavSatFix.h>
 20 | #include <geometry_msgs/TwistStamped.h>
 21 | #include <sstream>
 22 | #include <px4ros/gpsPosition.h>
 23 | #include <math.h>
 24 | #include <fstream>
 25 | 
 26 | 
 27 | //global variables
 28 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
 29 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
 30 | 
 31 | #define R  6371000         //meter
 32 | #define PI 3.1415926535898
 33 | 
 34 | /*******************************************************************************
 35 |  *  Callback function on the gps of the copter
 36 |  *
 37 |  *  @param gpsPositionConstPtr& the gps callback pointer
 38 |  ******************************************************************************/
 39 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
 40 |    selfGlobalPos = *msg;
 41 | }
 42 | 
 43 | /*******************************************************************************
 44 |  *  Callback function on the gps of the neighbour copter
 45 |  *
 46 |  *  @param gpsPositionConstPtr& the gps callback pointer
 47 |  ******************************************************************************/
 48 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
 49 | {
 50 |     neigGlobalPos = *msg;
 51 | }
 52 | 
 53 | /*******************************************************************************
 54 |  *  Main function
 55 |  *
 56 |  *  @param 
 57 |  ******************************************************************************/
 58 | int main(int argc, char **argv) {
 59 | 	ros::init(argc, argv, "spiri_go_client");
 60 | 	ros::NodeHandle nh;
 61 | 
 62 | 	ros::Rate loop_rate(10.0);
 63 | 	
 64 | 	//velocity to publish
 65 | 	geometry_msgs::TwistStamped vel;
 66 | 	vel.twist.linear.x = 0.0;
 67 | 	vel.twist.linear.y = 0.0;
 68 | 	vel.header.stamp = ros::Time::now();
 69 | 
 70 | 	//variables definition
 71 | 	float x_0 = 0.0;
 72 | 	float y_0 = 0.0;
 73 | 	float x_1 = 0.0;
 74 | 	float y_1 = 0.0;
 75 | 	float d12 = 10;   //meter,two leaders mantain the distance
 76 |     	float r = 1.0;     // the region that collision avoidance algorithm will take effect
 77 |     	float r1 = 5.0;     // the region that collision avoidance algorithm will take effect
 78 | 	float vx0 = 0.0;
 79 | 	float vy0 = 0.0;
 80 | 	float currentDisPow = 0.0;
 81 |     	float x_offset = 0.0;
 82 |     	float y_offset = 0.0;
 83 | 	
 84 | 	ofstream ofile;
 85 | 	float begin_time = ros::Time::now().toSec();
 86 | 	ROS_INFO("begin_time,%6.4f",begin_time);
 87 | 	
 88 | 	//takeoff action object
 89 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
 90 | 	//land action object
 91 | 	actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
 92 | 
 93 | 	// Subscriber to the Copter gps
 94 | 	string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
 95 | 	ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
 96 | 
 97 | 	// Subscriber to the neighbour gps
 98 | 	string neigGps_ = nh.resolveName("neighbour_position");
 99 | 	ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
100 | 
101 | 	//Publisher to the gps(toString)
102 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
103 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
104 | 
105 | 	//Publisher to the velocity of the copter
106 | 	string vel_pub_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
107 | 	ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>(vel_pub_, 5);
108 | 	
109 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
110 | 
111 | 	ac.waitForServer();	
112 | 	
113 | 	ROS_INFO("Action server started--Taking off!");
114 | 
115 | 	spiri_go::TakeoffGoal goal;
116 | 
117 | 	//set the target height to take off!
118 | 	goal.height = 3;
119 | 	
120 | 	ac.sendGoal(goal);
121 | 
122 | 	//wait for action(takeOff) to return
123 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
124 | 	ros::Duration(5).sleep();
125 | 	if(is_takeOff_finished ) {
126 | 		ROS_INFO("reached target height!");
127 |         	ofile.open("./log/data.log", ios::trunc);
128 | 
129 | 		while(nh.ok()) {
130 | 			//1.publish self gps(toString)
131 | 			std_msgs::String gps_string;
132 |       			std::stringstream ss;
133 |       			ss << "x1o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
134 | 			//ss << "o"<<120.87654321<<"a"<<30.12345678<<"#";
135 |       			gps_string.data = ss.str();
136 |       			gps_string_pub.publish(gps_string);
137 | 	
138 | 			ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
139 | 			ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
140 |  
141 | 			//2.read neighbour gps
142 | 			ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
143 | 			ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
144 | 
145 | 			//3.calculate velocity(adaptive control)
146 | 			if(neigGlobalPos.lat == 0.0 || neigGlobalPos.lon == 0.0) {
147 | 				vel.twist.linear.x = 0.0;
148 | 				vel.twist.linear.y = 0.0;
149 | 			}
150 | 			else {
151 | 				x_1 = float(PI/180) * R * neigGlobalPos.lat;
152 | 				y_1 = float(PI/180) * R * neigGlobalPos.lon;
153 | 				x_0 = float(PI/180) * R * selfGlobalPos.latitude;
154 | 				y_0 = float(PI/180) * R * selfGlobalPos.longitude;
155 | 				
156 | 				// formation control 
157 | 				currentDisPow = pow(x_1-x_0, 2) + pow(y_1-y_0, 2);
158 | 				vel.twist.linear.x = vx0 + (x_1 - x_0)*(currentDisPow - pow(d12, 2))*0.01;
159 | 				vel.twist.linear.y = vy0 + (y_1 - y_0)*(currentDisPow - pow(d12, 2))*0.01;
160 | 				//vel.twist.linear.y = -1 * vel.twist.linear.y;  
161 | 
162 | 				// collision avoidance control
163 | 				x_offset = x_0 - x_1;
164 |                 		y_offset = y_0 - y_1;
165 | 				
166 | 				if(abs(x_offset) <= r1) {
167 | 				    vel.twist.linear.x = -8*( (pow(x_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*x_offset ) / pow((pow(x_offset,2)-pow(r,2)),3);
168 | 				}
169 |                 		else {
170 |   					vel.twist.linear.x = 0;
171 |                 		}
172 | 
173 | 				if(abs(y_offset) <= r1) {
174 | 				    vel.twist.linear.y = -8*( (pow(y_offset,2)-2*pow(r1,2)+pow(r,2))*(pow(r1,2)-pow(r,2))*y_offset ) / pow((pow(y_offset,2)-pow(r,2)),3);
175 | 				}
176 |                 		else {
177 |   					vel.twist.linear.y = 0;
178 |                 		}
179 | 			}
180 | 
181 | 			
182 | 			if(vel.twist.linear.x >= 1) {
183 | 				vel.twist.linear.x = 1;
184 | 			}else if(vel.twist.linear.x <= -1) {
185 | 				vel.twist.linear.x = -1;
186 | 			}
187 | 			if(vel.twist.linear.y >= 1) {
188 | 				vel.twist.linear.y = 1;
189 | 			}else if(vel.twist.linear.y <= -1) {
190 | 				vel.twist.linear.y = -1;
191 | 			}
192 | 			
193 | 
194 | 			vel.header.stamp = ros::Time::now();
195 | 
196 | 			//4.publish velocity to mavros 
197 | 			vel_pub.publish(vel);
198 | 			//ROS_INFO("velocity_x:%f", vel.twist.linear.x);
199 | 			//ROS_INFO("velocity_y:%f", vel.twist.linear.y);
200 |             		ofile << vel.twist.linear.x << " " << vel.twist.linear.y << x_0 << y_0 << x_1 << y_1 << std::endl;
201 | 
202 |             		// if fly for 4 minutes
203 | 		    	if( (ros::Time::now().toSec() - begin_time) > 4*60 ) {
204 | 				//land
205 | 				ROS_INFO("Waiting for action server(land) to start.");
206 | 				ac1.waitForServer();	
207 | 				ROS_INFO("Start to land!.");
208 | 	
209 | 				spiri_go::LandHereGoal goal_1;
210 | 				goal_1.height = 0;
211 | 				ac1.sendGoal(goal_1);
212 | 
213 | 				//wait for action(land) to return
214 | 				bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
215 | 				if(is_land_finished ) {
216 | 					ROS_INFO("Land success!");
217 | 				}
218 |             		}
219 | 		
220 | 			ros::spinOnce();
221 |     			loop_rate.sleep();
222 | 		}		
223 | 	}
224 | 	else {
225 | 		ROS_INFO("should set mode Land!");
226 | 	}
227 | 
228 | 	return 0;
229 | 
230 | }
231 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/leader_control.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * leader adaptive control
  3 |  */
  4 | 
  5 | #include "ros/ros.h"
  6 | #include <actionlib/client/simple_action_client.h>
  7 | #include <actionlib/client/terminal_state.h>
  8 | #include <spiri_go/TakeoffAction.h>
  9 | #include "spiri_go.h"
 10 | #include <std_msgs/String.h>
 11 | #include <sensor_msgs/NavSatFix.h>
 12 | #include <geometry_msgs/TwistStamped.h>
 13 | #include <sstream>
 14 | #include <px4ros/gpsPosition.h>
 15 | #include <math.h>
 16 | 
 17 | 
 18 | //global variables
 19 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
 20 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
 21 | 
 22 | #define R  6371000         //meter
 23 | #define PI 3.1415926535898
 24 | 
 25 | /*******************************************************************************
 26 |  *  Callback function on the gps of the copter
 27 |  *
 28 |  *  @param gpsPositionConstPtr& the gps callback pointer
 29 |  ******************************************************************************/
 30 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
 31 |    selfGlobalPos = *msg;
 32 | }
 33 | 
 34 | /*******************************************************************************
 35 |  *  Callback function on the gps of the neighbour copter
 36 |  *
 37 |  *  @param gpsPositionConstPtr& the gps callback pointer
 38 |  ******************************************************************************/
 39 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
 40 | {
 41 |     neigGlobalPos = *msg;
 42 | }
 43 | 
 44 | /*******************************************************************************
 45 |  *  Main function
 46 |  *
 47 |  *  @param 
 48 |  ******************************************************************************/
 49 | int main(int argc, char **argv) {
 50 | 	ros::init(argc, argv, "spiri_go_client");
 51 | 	ros::NodeHandle nh;
 52 | 
 53 | 	ros::Rate loop_rate(10.0);
 54 | 	
 55 | 	//velocity to publish
 56 | 	geometry_msgs::TwistStamped vel;
 57 | 	vel.twist.linear.x = 0.0;
 58 | 	vel.twist.linear.y = 0.0;
 59 | 	vel.header.stamp = ros::Time::now();
 60 | 
 61 | 	//variables definition
 62 | 	float x_0 = 0.0;
 63 | 	float y_0 = 0.0;
 64 | 	float x_1 = 0.0;
 65 | 	float y_1 = 0.0;
 66 | 	float d12 = 10;   //meter,two leaders mantain the distance
 67 | 	float vx0 = 0.0;
 68 | 	float vy0 = 0.0;
 69 | 	float currentDisPow = 0.0;
 70 | 	
 71 | 	//takeoff action object
 72 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
 73 | 
 74 |     // Subscriber to the Copter gps
 75 |     string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
 76 |     ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
 77 | 
 78 |     // Subscriber to the neighbour gps
 79 |     string neigGps_ = nh.resolveName("neighbour_position");
 80 |     ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
 81 | 
 82 | 	//Publisher to the gps(toString)
 83 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
 84 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
 85 | 
 86 | 	//Publisher to the velocity of the copter
 87 | 	string vel_pub_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
 88 | 	ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>(vel_pub_, 5);
 89 | 	
 90 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
 91 | 
 92 | 	ac.waitForServer();	
 93 | 	
 94 | 	ROS_INFO("Action server started--Taking off!");
 95 | 
 96 | 	spiri_go::TakeoffGoal goal;
 97 | 
 98 | 	//set the target height to take off!
 99 | 	goal.height = 3;
100 | 	
101 | 	ac.sendGoal(goal);
102 | 
103 | 	//wait for action(takeOff) to return
104 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
105 | 	ros::Duration(10).sleep();
106 | 	if(is_takeOff_finished ) {
107 | 		ROS_INFO("reached target height!");
108 | 
109 | 		while(nh.ok()) {
110 | 			//1.publish self gps(toString)
111 | 			std_msgs::String gps_string;
112 |       		std::stringstream ss;
113 |       		ss << "o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
114 |       		gps_string.data = ss.str();
115 |       		gps_string_pub.publish(gps_string);
116 | 	
117 | 			ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
118 | 			ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
119 |  
120 | 			//2.read neighbour gps
121 | 			ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
122 | 			ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
123 | 
124 | 			//3.calculate velocity(adaptive control)
125 | 			if(neigGlobalPos.lat == 0.0 || neigGlobalPos.lon == 0.0) {
126 | 				vel.twist.linear.x = 0.0;
127 | 				vel.twist.linear.y = 0.0;
128 | 			}
129 | 			else {
130 | 				x_1 = float(PI/180) * R * neigGlobalPos.lat;
131 | 				y_1 = float(PI/180) * R * neigGlobalPos.lon;
132 | 				x_0 = float(PI/180) * R * selfGlobalPos.latitude;
133 | 				y_0 = float(PI/180) * R * selfGlobalPos.longitude;
134 | 				
135 | 				currentDisPow = pow(x_1-x_0, 2) + pow(y_1-y_0, 2);
136 | 
137 | 				vel.twist.linear.x = vx0 + (x_1 - x_0)*(currentDisPow - pow(d12, 2))*0.01;
138 | 				vel.twist.linear.y = vy0 + (y_1 - y_0)*(currentDisPow - pow(d12, 2))*0.01;
139 | 
140 | 				vel.twist.linear.y = -1 * vel.twist.linear.y; 
141 | 			}
142 | 
143 | 			
144 | 			if(vel.twist.linear.x >= 1) {
145 | 				vel.twist.linear.x = 1;
146 | 			}else if(vel.twist.linear.x <= -1) {
147 | 				vel.twist.linear.x = -1;
148 | 			}
149 | 			if(vel.twist.linear.y >= 1) {
150 | 				vel.twist.linear.y = 1;
151 | 			}else if(vel.twist.linear.y <= -1) {
152 | 				vel.twist.linear.y = -1;
153 | 			}
154 | 			
155 | 
156 | 			vel.header.stamp = ros::Time::now();
157 | 
158 | 			//4.publish velocity to mavros 
159 | 			vel_pub.publish(vel);
160 | 			ROS_INFO("velocity_x:%f", vel.twist.linear.x);
161 | 			ROS_INFO("velocity_y:%f", vel.twist.linear.y);
162 | 		
163 | 			ros::spinOnce();
164 |     		loop_rate.sleep();
165 | 		}		
166 | 	}
167 | 	else {
168 | 		ROS_INFO("should set mode Land!");
169 | 	}
170 | 
171 | 	return 0;
172 | 
173 | }
174 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/leader_control.cpp~:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * leader adaptive control
  3 |  */
  4 | 
  5 | #include "ros/ros.h"
  6 | #include <actionlib/client/simple_action_client.h>
  7 | #include <actionlib/client/terminal_state.h>
  8 | #include <spiri_go/TakeoffAction.h>
  9 | #include "spiri_go.h"
 10 | #include <std_msgs/String.h>
 11 | #include <sensor_msgs/NavSatFix.h>
 12 | #include <geometry_msgs/TwistStamped.h>
 13 | #include <sstream>
 14 | #include <px4ros/gpsPosition.h>
 15 | #include <math.h>
 16 | 
 17 | 
 18 | //global variables
 19 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
 20 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
 21 | 
 22 | #define R  6371000         //meter
 23 | #define PI 3.1415926535898
 24 | 
 25 | /*******************************************************************************
 26 |  *  Callback function on the gps of the copter
 27 |  *
 28 |  *  @param gpsPositionConstPtr& the gps callback pointer
 29 |  ******************************************************************************/
 30 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
 31 |    selfGlobalPos = *msg;
 32 | }
 33 | 
 34 | /*******************************************************************************
 35 |  *  Callback function on the gps of the neighbour copter
 36 |  *
 37 |  *  @param gpsPositionConstPtr& the gps callback pointer
 38 |  ******************************************************************************/
 39 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
 40 | {
 41 |     neigGlobalPos = *msg;
 42 | }
 43 | 
 44 | /*******************************************************************************
 45 |  *  Main function
 46 |  *
 47 |  *  @param 
 48 |  ******************************************************************************/
 49 | int main(int argc, char **argv) {
 50 | 	ros::init(argc, argv, "spiri_go_client");
 51 | 	ros::NodeHandle nh;
 52 | 
 53 | 	ros::Rate loop_rate(10.0);
 54 | 	
 55 | 	//velocity to publish
 56 | 	geometry_msgs::TwistStamped vel;
 57 | 	vel.twist.linear.x = 0.0;
 58 | 	vel.twist.linear.y = 0.0;
 59 | 	vel.header.stamp = ros::Time::now();
 60 | 
 61 | 	//variables definition
 62 | 	float x_0 = 0.0;
 63 | 	float y_0 = 0.0;
 64 | 	float x_1 = 0.0;
 65 | 	float y_1 = 0.0;
 66 | 	float d12 = 10;   //meter,two leaders mantain the distance
 67 | 	float vx0 = 0.0;
 68 | 	float vy0 = 0.0;
 69 | 	float currentDisPow = 0.0;
 70 | 	
 71 | 	//takeoff action object
 72 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
 73 | 
 74 |     // Subscriber to the Copter gps
 75 |     string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
 76 |     ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
 77 | 
 78 |     // Subscriber to the neighbour gps
 79 |     string neigGps_ = nh.resolveName("neighbour_position");
 80 |     ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
 81 | 
 82 | 	//Publisher to the gps(toString)
 83 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
 84 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
 85 | 
 86 | 	//Publisher to the velocity of the copter
 87 | 	string vel_pub_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
 88 | 	ros::Publisher vel_pub = nh.advertise<geometry_msgs::TwistStamped>(vel_pub_, 5);
 89 | 	
 90 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
 91 | 
 92 | 	ac.waitForServer();	
 93 | 	
 94 | 	ROS_INFO("Action server started--Taking off!");
 95 | 
 96 | 	spiri_go::TakeoffGoal goal;
 97 | 
 98 | 	//set the target height to take off!
 99 | 	goal.height = 3;
100 | 	
101 | 	ac.sendGoal(goal);
102 | 
103 | 	//wait for action(takeOff) to return
104 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
105 | 	ros::Duration(10).sleep();
106 | 	if(is_takeOff_finished ) {
107 | 		ROS_INFO("reached target height!");
108 | 
109 | 		while(nh.ok()) {
110 | 			//1.publish self gps(toString)
111 | 			std_msgs::String gps_string;
112 |       		std::stringstream ss;
113 |       		ss << "o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
114 |       		gps_string.data = ss.str();
115 |       		gps_string_pub.publish(gps_string);
116 | 	
117 | 			ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
118 | 			ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
119 |  
120 | 			//2.read neighbour gps
121 | 			ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
122 | 			ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
123 | 
124 | 			//3.calculate velocity(adaptive control)
125 | 			if(neigGlobalPos.lat == 0.0 || neigGlobalPos.lon == 0.0) {
126 | 				vel.twist.linear.x = 0.0;
127 | 				vel.twist.linear.y = 0.0;
128 | 			}
129 | 			else {
130 | 				x_1 = float(PI/180) * R * neigGlobalPos.lat;
131 | 				y_1 = float(PI/180) * R * neigGlobalPos.lon;
132 | 				x_0 = float(PI/180) * R * selfGlobalPos.latitude;
133 | 				y_0 = float(PI/180) * R * selfGlobalPos.longitude;
134 | 				
135 | 				currentDisPow = pow(x_1-x_0, 2) + pow(y_1-y_0, 2);
136 | 
137 | 				vel.twist.linear.x = vx0 + (x_1 - x_0)*(currentDisPow - pow(d12, 2))*0.01;
138 | 				vel.twist.linear.y = vy0 + (y_1 - y_0)*(currentDisPow - pow(d12, 2))*0.01;
139 | 
140 | 				vel.twist.linear.y = -1 * vel.twist.linear.y; 
141 | 			}
142 | 
143 | 			
144 | 			if(vel.twist.linear.x >= 1) {
145 | 				vel.twist.linear.x = 1;
146 | 			}else if(vel.twist.linear.x <= -1) {
147 | 				vel.twist.linear.x = -1;
148 | 			}
149 | 			if(vel.twist.linear.y >= 1) {
150 | 				vel.twist.linear.y = 1;
151 | 			}else if(vel.twist.linear.y <= -1) {
152 | 				vel.twist.linear.y = -1;
153 | 			}
154 | 			
155 | 
156 | 			vel.header.stamp = ros::Time::now();
157 | 
158 | 			//4.publish velocity to mavros 
159 | 			vel_pub.publish(vel);
160 | 			ROS_INFO("velocity_x:%f", vel.twist.linear.x);
161 | 			ROS_INFO("velocity_y:%f", vel.twist.linear.y);
162 | 		
163 | 			ros::spinOnce();
164 |     		loop_rate.sleep();
165 | 		}		
166 | 	}
167 | 	else {
168 | 		ROS_INFO("should set mode Land!");
169 | 	}
170 | 
171 | 	return 0;
172 | 
173 | }
174 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/leader_go.cpp~:
--------------------------------------------------------------------------------
  1 | // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
  2 | 
  3 | /*******************************************************************************
  4 |  *  The Spiri Project
  5 |  *
  6 |  *  File: spiri_go.cpp
  7 |  *
  8 |  *  Purpose: Sets up the ROS node that serves as the service 
  9 |  *  and action server for autonomous flights.
 10 |  *
 11 |  *  @author Skyler Olson
 12 |  *  @author John Lian
 13 |  *  @version 0.1.0 27/11/15
 14 |  ******************************************************************************/
 15 | 
 16 | #include "ros/ros.h"
 17 | #include "ros/package.h"
 18 | #include "geometry_msgs/Twist.h"
 19 | #include "geometry_msgs/PoseStamped.h"
 20 | #include "std_msgs/Empty.h"
 21 | #include "std_srvs/Empty.h"
 22 | #include "mavros_msgs/State.h"
 23 | #include "mavros_msgs/SetMode.h"
 24 | #include "mavros_msgs/CommandTOL.h"
 25 | #include "mavros_msgs/CommandBool.h"
 26 | #include "mavros_msgs/CommandLong.h"
 27 | #include "mavros_msgs/OverrideRCIn.h"
 28 | #include "mavros_msgs/CommandLong.h"
 29 | #include "mavros_msgs/CommandBool.h"
 30 | #include "std_msgs/String.h"
 31 | #include "spiri_go.h"
 32 | #include <sys/stat.h>
 33 | #include <string>
 34 | 
 35 | /*******************************************************************************
 36 |  *  The SpiriGo constructor. The constructor will set up subscribers and 
 37 |  *  publishers to mavros.
 38 |  ******************************************************************************/
 39 | SpiriGo::SpiriGo():
 40 | takeoff_as(nh, "spiri_take_off", boost::bind(&SpiriGo::armAndTakeOff, this, _1), false),
 41 | land_here_as(nh, "spiri_land_here", boost::bind(&SpiriGo::landHere, this, _1), false)
 42 | {
 43 |     ROS_INFO("Constructing Go");
 44 | 
 45 |     // Subscriber to the position of the copter centered on the home location
 46 |     string local_ = nh.resolveName("/mavros/local_position/pose");
 47 |     local = nh.subscribe(local_, 1, &SpiriGo::localSubCb, this);
 48 | 
 49 |     // Subscriber to the states of the copter
 50 |     string state_ = nh.resolveName("/mavros/state");
 51 |     state = nh.subscribe(state_, 1, &SpiriGo::stateSubCb, this);
 52 | 
 53 |     // Control the velocity of the copter
 54 |     string vel_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
 55 |     vel = nh.advertise<geometry_msgs::TwistStamped>(vel_,1);
 56 | 
 57 |     // Set the mode 
 58 |     string set_mode_ = nh.resolveName("/mavros/set_mode");
 59 |     set_mode = nh.serviceClient<mavros_msgs::SetMode>(set_mode_);
 60 | 
 61 |     // Arming
 62 |     string arm_ = nh.resolveName("/mavros/cmd/arming");
 63 |     arm = nh.serviceClient<mavros_msgs::CommandBool>(arm_);
 64 | 
 65 |     // Tell the copter to take off autonomously
 66 |     string takeoff_ = nh.resolveName("/mavros/cmd/takeoff");
 67 |     takeoff = nh.serviceClient<mavros_msgs::CommandTOL>(takeoff_);
 68 | 
 69 |     // MAVLink commander for yaw because setAttitude doesn't work in APM
 70 |     string mavlink_cmd_srv_ = nh.resolveName("/mavros/cmd/command");
 71 |     mavlink_cmd_srv = nh.serviceClient<mavros_msgs::CommandLong>(mavlink_cmd_srv_);
 72 | 
 73 |     // Create the services and actions that other nodes can interact with spiri_go through
 74 |     getLocalPositionService = nh.advertiseService("spiri_local_position", &SpiriGo::getLocalPositionSCB, this);
 75 |     getLastStateService = nh.advertiseService("spiri_state", &SpiriGo::getLastStateSCB, this);
 76 | 
 77 |     // State variables
 78 |     last_state.armed = false;
 79 |     last_state.connected = false;
 80 |     last_state.guided = false;
 81 |     last_state.mode = "STABILIZE";
 82 |     taking_off = false;
 83 |     flying = false;
 84 | 
 85 |     // location z has to start here so you don't set flying too soon
 86 |     location.position.z = 0;
 87 | 
 88 |     // set the velocity to 0 all around
 89 |     velocity.linear.x = 0;
 90 |     velocity.linear.y = 0;
 91 |     velocity.linear.z = 0;
 92 | 
 93 |     ROS_INFO("Constructed Go");
 94 | 
 95 | }
 96 | 
 97 | /*******************************************************************************
 98 |  *  The SpiriGo destructor
 99 |  ******************************************************************************/
100 | SpiriGo::~SpiriGo()
101 | {
102 | 
103 | }
104 | 
105 | ////////////////////////////////////////////////////////////////////////////////
106 | // Callback functions
107 | ////////////////////////////////////////////////////////////////////////////////
108 | 
109 | /*******************************************************************************
110 |  *  Callback function on the local position of the copter
111 |  *
112 |  *  @param loalPtr the location callback pointer
113 |  ******************************************************************************/
114 | void SpiriGo::localSubCb(const geometry_msgs::PoseStamped localPtr)
115 | {
116 |     location = localPtr.pose;
117 |     last_location = localPtr.pose;
118 | }
119 | 
120 | /*******************************************************************************
121 |  *  Callback function on the state of the copter
122 |  *
123 |  *  @param statePtr the state callback pointer
124 |  ******************************************************************************/
125 | void SpiriGo::stateSubCb(const mavros_msgs::State statePtr)
126 | {
127 |     last_state = statePtr;
128 | }
129 | 
130 | /*******************************************************************************
131 |  *  Service callback function on the local position of the copter
132 |  *
133 |  *  @return True if the service is successfully called 
134 |  ******************************************************************************/
135 | bool SpiriGo::getLocalPositionSCB(spiri_go::LocalPosition::Request &req, 
136 |                                   spiri_go::LocalPosition::Response &rsp){
137 |     rsp.x = location.position.x;
138 |     rsp.y = location.position.y;
139 |     rsp.z = location.position.z;
140 |     return true;
141 | }
142 | 
143 | /*******************************************************************************
144 |  *  Service callback function on the state of the copter
145 |  *
146 |  *  @return True if the service is successfully called 
147 |  ******************************************************************************/
148 | bool SpiriGo::getLastStateSCB(spiri_go::LastState::Request &req, 
149 |                               spiri_go::LastState::Response &rsp){
150 |     rsp.armed = last_state.armed;
151 |     rsp.connected = last_state.connected;
152 |     rsp.guided = last_state.guided;
153 |     rsp.mode = last_state.guided;
154 |     return true;
155 | }
156 | 
157 | ////////////////////////////////////////////////////////////////////////////////
158 | // Spiri control methods 
159 | ////////////////////////////////////////////////////////////////////////////////
160 | 
161 | /*******************************************************************************
162 |  *  A function to set the desired mode of the copter
163 |  *
164 |  *  @param targetMode is a string for the name of the desired mode
165 |  *      Note: the function will fail if the targetMode name is not valid
166 |  *
167 |  *  @return True if the mode set is successfully called 
168 |  ******************************************************************************/
169 | bool SpiriGo::setMode(const char* targetMode)
170 | {
171 |     mavros_msgs::SetMode modeCmd;
172 | 
173 |     modeCmd.request.base_mode = 0;
174 |     modeCmd.request.custom_mode = targetMode;
175 | 
176 |     if(set_mode.call(modeCmd)){
177 |         ROS_INFO("Set to %s Mode.", targetMode);
178 |         return modeCmd.response.success;
179 |     }else{
180 |         ROS_INFO("Failed to set to %s Mode. Currently in %s mode", targetMode, last_state.mode.c_str());
181 |         return false;
182 |     }
183 | }
184 | 
185 | /*******************************************************************************
186 |  *  A function to the copter to GUIDED mode
187 |  *
188 |  *  @return True if the the copter has been successfully set to GUIDED mode
189 |  ******************************************************************************/
190 | bool SpiriGo::setGuided()
191 | {
192 |     return setMode("GUIDED");
193 | }
194 | 
195 | /*******************************************************************************
196 |  *  A function to arm the motors of the copter
197 |  *
198 |  *  @return True if the the copter has been successfully armed
199 |  ******************************************************************************/
200 | bool SpiriGo::setArmed()
201 | {
202 |     mavros_msgs::CommandBool set_armed;
203 |     set_armed.request.value = true;
204 | 
205 |     if(arm.call(set_armed)){
206 |         ROS_INFO("Set Armed.");
207 |         return set_armed.response.success;
208 |     }else{
209 |         ROS_INFO("Failed to set to Armed.");
210 |         return false;
211 |     }
212 | }
213 | 
214 | /*******************************************************************************
215 |  *  Make the copter takeoff to a specified height
216 |  *
217 |  *  @param targetAlt a float value of the target altitude (relative to the ground)
218 |  *  @return True if the takeoff command has been successfully issued to the copter
219 |  ******************************************************************************/
220 | void SpiriGo::takeOff(float targetAlt = 5)
221 | {
222 |     // try to take off
223 |     mavros_msgs::CommandTOL to_cmd;
224 | 
225 |     to_cmd.request.altitude = targetAlt;
226 | 
227 |     // TODO: make this a bool function based on the success of the call
228 |     if(takeoff.call(to_cmd)){
229 |         ROS_INFO("Taking off");
230 |         taking_off = true;
231 |     }else{
232 |         ROS_INFO("Failed to initiate take off");
233 |     }
234 | }
235 | 
236 | /*******************************************************************************
237 |  *  Tell the copter to go to specified heading
238 |  *
239 |  *  @param targetYaw angle (in degrees) of the desired heading (from north = 0 degrees)
240 |  *  @param targetYawRate angular velocity in deg/s of the desired yaw rate
241 |  *  @return True if the yaw command has been successfully issued to the copter
242 |  ******************************************************************************/
243 | void SpiriGo::conditionYaw(float targetYaw, float targetYawRate)
244 | {
245 |     mavros_msgs::CommandLong yawCmd;
246 | 
247 |     yawCmd.request.command = 155;           // MAVLink command ID for MAV_CMD_CONDITION_YAW
248 |     yawCmd.request.confirmation = 0;        // 0 is default for confirmation
249 |     yawCmd.request.param1 = targetYaw;      // target heading/yaw in degrees from north (0 to 360)
250 |     yawCmd.request.param2 = targetYawRate;  // target yaw rate in deg/s
251 |     yawCmd.request.param3 = 1;              // direction; -1 ccw, 1 cw TODO: make this automatic
252 |     yawCmd.request.param4 = 0;              // relative offset 1, absolute angle 0
253 | 
254 |     // TODO: return true if the call was a success 
255 |     if(mavlink_cmd_srv.call(yawCmd)){
256 |         ROS_INFO("Controlling yaw");
257 |     }else{
258 |         ROS_INFO("Condition yaw command rejected");
259 |     }
260 | }
261 | 
262 | // TODO: @ssorl what is your plan with this
263 | void SpiriGo::setENUVelocity(/*double eastwardVelocity, double northwardVelocity */)
264 | {
265 |     geometry_msgs::TwistStamped control_msg;
266 | 
267 |     control_msg.twist.linear.x = velocity.linear.x;
268 |     control_msg.twist.linear.y = velocity.linear.y;
269 | 
270 |     vel.publish(control_msg);
271 | }
272 | 
273 | ////////////////////////////////////////////////////////////////////////////////
274 | // Action-specific methods - these will sleep until finished!!!
275 | ////////////////////////////////////////////////////////////////////////////////
276 | 
277 | /*******************************************************************************
278 |  *  Full autonomous takeoff action
279 |  *
280 |  *  This will ask the copter to go to GUIDED
281 |  *
282 |  *  @param TakeoffGoalConstPtr the takeoff action goal object 
283 |  ******************************************************************************/
284 | void SpiriGo::armAndTakeOff(const spiri_go::TakeoffGoalConstPtr& goal)
285 | {
286 |     if(!takeoff_as.isActive()||takeoff_as.isPreemptRequested()) return;
287 |     ros::Rate takeoff_rate(5);
288 |     bool success = true;
289 | 
290 |     // Set to guided mode and THEN arm the copter
291 |     bool armed_ = setArmed();
292 |     while(not last_state.armed)
293 |     {
294 |         if(not last_state.guided){
295 |             setGuided();
296 |         }else{
297 |             bool armed_ = setArmed();
298 |         }
299 |         takeoff_rate.sleep();
300 |     }
301 | 
302 |     // Sleep the method until the takeoff is complete
303 |     taking_off = false;
304 |     while(not taking_off)
305 |     {
306 |         takeOff(goal->height);
307 |         takeoff_rate.sleep();
308 |     }
309 |     while(location.position.z < goal->height*0.96)
310 |     {
311 |         // Only wait to reach 96% of the target altitude to handle undershoot
312 |         takeoff_rate.sleep();
313 |         ROS_INFO("height: %f", location.position.z);
314 |     }
315 |     ROS_INFO("took off");
316 |     flying = true;
317 | 
318 |     if(ros::ok()){
319 |         takeoff_as.setSucceeded();
320 |         ROS_INFO("Takeoff finished");
321 |     } else {
322 |         takeoff_as.setAborted();
323 |         ROS_INFO("Takeoff aborted");
324 |     }
325 | }
326 | 
327 | /*******************************************************************************
328 |  *  Full autonomous land action
329 |  *
330 |  *  This will ask the copter to go to LAND mode
331 |  *
332 |  *  @param LandHereGoalConstPtr the land_here action goal object 
333 |  ******************************************************************************/
334 | void SpiriGo::landHere(const spiri_go::LandHereGoalConstPtr& goal)
335 | {
336 |     if(!land_here_as.isActive()||land_here_as.isPreemptRequested()) return;
337 |     ros::Rate land_rate(5);
338 |     bool success = true;
339 | 
340 |     ROS_INFO("Attempting to land in place");
341 | 
342 |     // Change the mode to LAND in APM, which is autonomous landing
343 |     while(last_state.mode != "LAND")
344 |     {
345 |         setMode("LAND");
346 |         land_rate.sleep();
347 |     }
348 | 
349 |     // Sleep until for the robot to think it's 5cm off the ground
350 |     while(location.position.z > 0.05)
351 |     {
352 |         land_rate.sleep();
353 |     }
354 | 
355 |     if(ros::ok()){
356 |         land_here_as.setSucceeded();
357 |         ROS_INFO("Landing finished");
358 |     } else {
359 |         land_here_as.setAborted();
360 |         ROS_INFO("Landing finished");
361 |     }
362 | }
363 | 
364 | 
365 | ////////////////////////////////////////////////////////////////////////////////
366 | // Getter functions
367 | ////////////////////////////////////////////////////////////////////////////////
368 | 
369 | /*******************************************************************************
370 |  *  @return The latest information on whether the copter is armed
371 |  ******************************************************************************/
372 | bool SpiriGo::isArmed(){
373 |     return last_state.armed;
374 | }
375 | 
376 | /*******************************************************************************
377 |  *    @return The latest information on whether the copter is in GUIDED mode
378 |  ******************************************************************************/
379 | bool SpiriGo::isControllable(){
380 |     return last_state.guided;
381 | }
382 | 
383 | /*******************************************************************************
384 |  *  @return The latest information the mode of the copter
385 |  ******************************************************************************/
386 | std::string SpiriGo::getMode(){
387 |     return last_state.mode;
388 | }
389 | 
390 | 
391 | ////////////////////////////////////////////////////////////////////////////////
392 | // Utility Methods 
393 | ////////////////////////////////////////////////////////////////////////////////
394 | 
395 | /*******************************************************************************
396 |  *  Make a best guess as to the current location
397 |  *  since the real value is provided fairly often
398 |  *  don't need to be too precise about it
399 |  *
400 |  *  @param dt_dur how long since the last update
401 |  ******************************************************************************/
402 | void SpiriGo::updateLocation(ros::Duration dt_dur){
403 |     float dt = dt_dur.toSec();
404 |     location.position.x += velocity.linear.x*dt;
405 |     location.position.y += velocity.linear.y*dt;
406 |     location.position.z += velocity.linear.z*dt;
407 |     // TODO: deal with the angular part too
408 | }
409 | 
410 | ////////////////////////////////////////////////////////////////////////////////
411 | // Main loop
412 | ////////////////////////////////////////////////////////////////////////////////
413 | 
414 | void SpiriGo::Loop()
415 | {
416 |     ros::Rate pub_rate(10);
417 | 
418 |     ros::Time last_time = ros::Time::now();
419 |     while (nh.ok()){
420 |         ros::spinOnce();
421 |         if(flying){
422 |             ros::Duration dt =  ros::Time::now() - last_time;
423 |             last_time = ros::Time::now();
424 |             updateLocation(dt);
425 |             setENUVelocity();
426 |         }
427 | 
428 |         pub_rate.sleep();
429 |     }
430 | 
431 | }
432 | 
433 | int main(int argc, char **argv)
434 | {
435 | 
436 |     ros::init(argc, argv, "spiri_go");
437 | 
438 |     SpiriGo go_thing;
439 | 
440 |     // start action servers
441 |     go_thing.takeoff_as.start();
442 |     go_thing.land_here_as.start();
443 | 
444 |     go_thing.Loop();
445 | 
446 |     return 0;
447 | }
448 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/read me.txt:
--------------------------------------------------------------------------------
1 | *********Server***********
2 | spiri_go.cpp    *the original file, takeoff and land server
3 | *********Client***********
4 | arm_and_takeoff.cpp  *arm and takeoff
5 | leader_control.cpp   *leader adaptive control
6 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/read me.txt~:
--------------------------------------------------------------------------------
1 | *********Server***********
2 | spiri_go.cpp    *the original file, takeoff and land server
3 | *********Client***********
4 | arm_and_takeoff.cpp  *arm and takeoff
5 | leader_control.cpp   *leader adaptive control
6 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/spiri_data_classes.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * This is a collection of classes that are used in spiri_go and that
 3 |  * are exposed by boost in spiri_boost so that they can be read and written
 4 |  * in stand-alone python scripts as well as ROS.
 5 |  */
 6 | 
 7 | #include <spiri_data_classes.h>
 8 | 
 9 | SpiriAttitude::SpiriAttitude(){
10 | 	roll = 0;
11 | 	pitch = 0;
12 | 	yaw = 0;
13 | }
14 | 
15 | SpiriAttitude::SpiriAttitude(double roll, double pitch, double yaw){
16 | 	this->roll = roll;
17 | 	this->pitch = pitch;
18 | 	this->yaw = yaw;
19 | }
20 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/spiri_go.cpp:
--------------------------------------------------------------------------------
  1 | // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
  2 | 
  3 | /*******************************************************************************
  4 |  *  The Spiri Project
  5 |  *
  6 |  *  File: spiri_go.cpp
  7 |  *
  8 |  *  Purpose: Sets up the ROS node that serves as the service 
  9 |  *  and action server for autonomous flights.
 10 |  *
 11 |  *  @author Skyler Olson
 12 |  *  @author John Lian
 13 |  *  @author Peng Cheng
 14 |  *  @version 0.1.0 27/11/15
 15 |  *  @version 0.1.1 2016/03/16
 16 |  ******************************************************************************/
 17 | 
 18 | #include "ros/ros.h"
 19 | #include "ros/package.h"
 20 | #include "geometry_msgs/Twist.h"
 21 | #include "geometry_msgs/PoseStamped.h"
 22 | #include "std_msgs/Empty.h"
 23 | #include "std_srvs/Empty.h"
 24 | #include "mavros_msgs/State.h"
 25 | #include "mavros_msgs/SetMode.h"
 26 | #include "mavros_msgs/StreamRate.h"
 27 | #include "mavros_msgs/CommandTOL.h"
 28 | #include "mavros_msgs/CommandBool.h"
 29 | #include "mavros_msgs/CommandLong.h"
 30 | #include "mavros_msgs/OverrideRCIn.h"
 31 | #include "mavros_msgs/CommandLong.h"
 32 | #include "mavros_msgs/CommandBool.h"
 33 | #include "std_msgs/String.h"
 34 | #include "spiri_go.h"
 35 | #include <sys/stat.h>
 36 | #include <string>
 37 | 
 38 | /*******************************************************************************
 39 |  *  The SpiriGo constructor. The constructor will set up subscribers and 
 40 |  *  publishers to mavros.
 41 |  ******************************************************************************/
 42 | SpiriGo::SpiriGo():
 43 | takeoff_as(nh, "spiri_take_off", boost::bind(&SpiriGo::armAndTakeOff, this, _1), false),
 44 | land_here_as(nh, "spiri_land_here", boost::bind(&SpiriGo::landHere, this, _1), false)
 45 | {
 46 |     ROS_INFO("Constructing Go");
 47 | 
 48 |     // Subscriber to the position of the copter centered on the home location
 49 |     string local_ = nh.resolveName("/mavros/local_position/pose");
 50 |     local = nh.subscribe(local_, 1, &SpiriGo::localSubCb, this);
 51 | 
 52 |     // Subscriber to the states of the copter
 53 |     string state_ = nh.resolveName("/mavros/state");
 54 |     state = nh.subscribe(state_, 1, &SpiriGo::stateSubCb, this);
 55 | 
 56 |     // Subscriber to gps of copter, I add
 57 |     string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
 58 |     gps = nh.subscribe(gps_, 1, &SpiriGo::gpsSubCb, this);
 59 | 
 60 |     // Subscriber to height of copter, I add
 61 |     string alt_ = nh.resolveName("/mavros/global_position/global");
 62 |     alt = nh.subscribe(alt_, 1, &SpiriGo::altSubCb, this);
 63 | 
 64 |     //Subscriber to the mavlink/from of the copter, I add
 65 |     string connect_state_ = nh.resolveName("/mavlink/from");
 66 |     connect_state = nh.subscribe(connect_state_, 1, &SpiriGo::connectSubCb, this);
 67 | 
 68 |     // Control the velocity of the copter
 69 |     string vel_ = nh.resolveName("/mavros/setpoint_velocity/cmd_vel");
 70 |     vel = nh.advertise<geometry_msgs::TwistStamped>(vel_,1);
 71 | 
 72 |     // Set the mode 
 73 |     string set_mode_ = nh.resolveName("/mavros/set_mode");
 74 |     set_mode = nh.serviceClient<mavros_msgs::SetMode>(set_mode_);
 75 | 
 76 |     // Set the stream rate
 77 |     string set_stream_rate_ = nh.resolveName("/mavros/set_stream_rate");
 78 |     set_stream_rate = nh.serviceClient<mavros_msgs::StreamRate>(set_stream_rate_);
 79 | 
 80 |     // Arming
 81 |     string arm_ = nh.resolveName("/mavros/cmd/arming");
 82 |     arm = nh.serviceClient<mavros_msgs::CommandBool>(arm_);
 83 | 
 84 |     // Tell the copter to take off autonomously
 85 |     string takeoff_ = nh.resolveName("/mavros/cmd/takeoff");
 86 |     takeoff = nh.serviceClient<mavros_msgs::CommandTOL>(takeoff_);
 87 | 
 88 |     // MAVLink commander for yaw because setAttitude doesn't work in APM
 89 |     string mavlink_cmd_srv_ = nh.resolveName("/mavros/cmd/command");
 90 |     mavlink_cmd_srv = nh.serviceClient<mavros_msgs::CommandLong>(mavlink_cmd_srv_);
 91 | 
 92 |     // Create the services and actions that other nodes can interact with spiri_go through
 93 |     getLocalPositionService = nh.advertiseService("spiri_local_position", &SpiriGo::getLocalPositionSCB, this);
 94 |     getLastStateService = nh.advertiseService("spiri_state", &SpiriGo::getLastStateSCB, this);
 95 | 
 96 |     // State variables
 97 |     last_state.armed = false;
 98 |     last_state.connected = false;
 99 |     last_state.guided = false;
100 |     last_state.mode = "STABILIZE";
101 |     taking_off = false;
102 |     flying = false;
103 | 
104 |     // location z has to start here so you don't set flying too soon
105 |     location.position.z = 0;
106 | 
107 |     // set the velocity to 0 all around
108 |     velocity.linear.x = 0;
109 |     velocity.linear.y = 0;
110 |     velocity.linear.z = 0;
111 | 
112 |     ROS_INFO("Constructed Go");
113 | 
114 | }
115 | 
116 | /*******************************************************************************
117 |  *  The SpiriGo destructor
118 |  ******************************************************************************/
119 | SpiriGo::~SpiriGo()
120 | {
121 | 
122 | }
123 | 
124 | ////////////////////////////////////////////////////////////////////////////////
125 | // Callback functions
126 | ////////////////////////////////////////////////////////////////////////////////
127 | 
128 | /*******************************************************************************
129 |  *  Callback function on the local position of the copter
130 |  *
131 |  *  @param loalPtr the location callback pointer
132 |  ******************************************************************************/
133 | void SpiriGo::localSubCb(const geometry_msgs::PoseStamped localPtr)
134 | {
135 |     location = localPtr.pose;
136 |     last_location = localPtr.pose;
137 | }
138 | 
139 | /*******************************************************************************
140 |  *  Callback function on the state of the copter
141 |  *
142 |  *  @param statePtr the state callback pointer
143 |  ******************************************************************************/
144 | void SpiriGo::stateSubCb(const mavros_msgs::State statePtr)
145 | {
146 |     last_state = statePtr;
147 | }
148 | 
149 | /*******************************************************************************
150 |  *  Callback function on the gps of the copter
151 |  *
152 |  *  @param NavSatFixConstPtr the state callback pointer
153 |  ******************************************************************************/
154 | void SpiriGo::gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg)
155 | {
156 |     last_gps = *msg;
157 | }
158 | 
159 | /*******************************************************************************
160 |  *  Callback function on the height of the copter
161 |  *
162 |  *  @param NavSatFixConstPtr the state callback pointer
163 |  ******************************************************************************/
164 | void SpiriGo::altSubCb(const sensor_msgs::NavSatFixConstPtr& msg)
165 | {
166 |     last_alt = *msg;
167 |     ROS_INFO("altitude is %f", last_alt.altitude);
168 | }
169 | 
170 | /*******************************************************************************
171 |  *  Callback function on the /mavlink/from of the copter
172 |  *
173 |  *  @param NavSatFixConstPtr the state callback pointer
174 |  ******************************************************************************/
175 | void SpiriGo::connectSubCb(const mavros_msgs::MavlinkPtr& fromPtr)
176 | {
177 |     last_connect_state = *fromPtr;
178 | //ROS_INFO("is_valid:%d",last_connect_state.is_valid);
179 | //ROS_INFO("seq:%d",last_connect_state.seq);
180 | }
181 | 
182 | /*******************************************************************************
183 |  *  Service callback function on the local position of the copter
184 |  *
185 |  *  @return True if the service is successfully called 
186 |  ******************************************************************************/
187 | bool SpiriGo::getLocalPositionSCB(spiri_go::LocalPosition::Request &req, 
188 |                                   spiri_go::LocalPosition::Response &rsp){
189 |     rsp.x = location.position.x;
190 |     rsp.y = location.position.y;
191 |     rsp.z = location.position.z;
192 |     return true;
193 | }
194 | 
195 | /*******************************************************************************
196 |  *  Service callback function on the state of the copter
197 |  *
198 |  *  @return True if the service is successfully called 
199 |  ******************************************************************************/
200 | bool SpiriGo::getLastStateSCB(spiri_go::LastState::Request &req, 
201 |                               spiri_go::LastState::Response &rsp){
202 |     rsp.armed = last_state.armed;
203 |     rsp.connected = last_state.connected;
204 |     rsp.guided = last_state.guided;
205 |     rsp.mode = last_state.guided;
206 |     return true;
207 | }
208 | 
209 | ////////////////////////////////////////////////////////////////////////////////
210 | // Spiri control methods 
211 | ////////////////////////////////////////////////////////////////////////////////
212 | 
213 | /*******************************************************************************
214 |  *  A function to set the desired mode of the copter
215 |  *
216 |  *  @param targetMode is a string for the name of the desired mode
217 |  *      Note: the function will fail if the targetMode name is not valid
218 |  *
219 |  *  @return True if the mode set is successfully called 
220 |  ******************************************************************************/
221 | bool SpiriGo::setMode(const char* targetMode)
222 | {
223 |     mavros_msgs::SetMode modeCmd;
224 | 
225 |     modeCmd.request.base_mode = 0;
226 |     modeCmd.request.custom_mode = targetMode;
227 | 
228 |     if(set_mode.call(modeCmd)){
229 |         ROS_INFO("Set to %s Mode.", targetMode);
230 |         return modeCmd.response.success;
231 |     }else{
232 |         ROS_INFO("Failed to set to %s Mode. Currently in %s mode", targetMode, last_state.mode.c_str());
233 |         return false;
234 |     }
235 | }
236 | 
237 | /*******************************************************************************
238 |  *  A function to set the desired mode of the copter
239 |  *
240 |  *  @param streamID is a string for the name of the desired stream
241 |  *         rate is the desired stream rate
242 |  *      Note: the function will fail if the targetMode name is not valid
243 |  *
244 |  *  @return True if the mode set is successfully called 
245 |  ******************************************************************************/
246 | bool SpiriGo::setStreamRate(unsigned char streamID, int rate)
247 | {
248 |     mavros_msgs::StreamRate streamRateCmd;
249 | 
250 |     streamRateCmd.request.stream_id = streamID;
251 |     streamRateCmd.request.message_rate = rate;
252 |     streamRateCmd.request.on_off = (rate!=0);
253 | 
254 |     if(set_stream_rate.call(streamRateCmd)){
255 |         ROS_INFO("Set stream rate success!");
256 |         return true;
257 |     }
258 |     else
259 |     {
260 |         ROS_INFO("Set stream rate failed!");
261 |         return false;
262 |     }
263 | }
264 | 
265 | /*******************************************************************************
266 |  *  A function to the copter to GUIDED mode
267 |  *
268 |  *  @return True if the the copter has been successfully set to GUIDED mode
269 |  ******************************************************************************/
270 | bool SpiriGo::setGuided()
271 | {
272 |     return setMode("GUIDED");
273 | }
274 | 
275 | /*******************************************************************************
276 |  *  A function to arm the motors of the copter
277 |  *
278 |  *  @return True if the the copter has been successfully armed
279 |  ******************************************************************************/
280 | bool SpiriGo::setArmed()
281 | {
282 |     mavros_msgs::CommandBool set_armed;
283 |     set_armed.request.value = true;
284 | 
285 |     if(arm.call(set_armed)){
286 |         ROS_INFO("Set Armed.");
287 |         return set_armed.response.success;
288 |     }else{
289 |         ROS_INFO("Failed to set to Armed.");
290 |         return false;
291 |     }
292 | }
293 | 
294 | /*******************************************************************************
295 |  *  Make the copter takeoff to a specified height
296 |  *
297 |  *  @param targetAlt a float value of the target altitude (relative to the ground)
298 |  *  @return True if the takeoff command has been successfully issued to the copter
299 |  ******************************************************************************/
300 | void SpiriGo::takeOff(float targetAlt = 5)
301 | {
302 |     // try to take off
303 |     mavros_msgs::CommandTOL to_cmd;
304 | 
305 |     to_cmd.request.altitude = targetAlt;
306 | 
307 |     // TODO: make this a bool function based on the success of the call
308 |     if(takeoff.call(to_cmd)){
309 |         ROS_INFO("Taking off");
310 |         taking_off = true;
311 |     }else{
312 |         ROS_INFO("Failed to initiate take off");
313 |     }
314 | }
315 | 
316 | /*******************************************************************************
317 |  *  Tell the copter to go to specified heading
318 |  *
319 |  *  @param targetYaw angle (in degrees) of the desired heading (from north = 0 degrees)
320 |  *  @param targetYawRate angular velocity in deg/s of the desired yaw rate
321 |  *  @return True if the yaw command has been successfully issued to the copter
322 |  ******************************************************************************/
323 | void SpiriGo::conditionYaw(float targetYaw, float targetYawRate)
324 | {
325 |     mavros_msgs::CommandLong yawCmd;
326 | 
327 |     yawCmd.request.command = 155;           // MAVLink command ID for MAV_CMD_CONDITION_YAW
328 |     yawCmd.request.confirmation = 0;        // 0 is default for confirmation
329 |     yawCmd.request.param1 = targetYaw;      // target heading/yaw in degrees from north (0 to 360)
330 |     yawCmd.request.param2 = targetYawRate;  // target yaw rate in deg/s
331 |     yawCmd.request.param3 = 1;              // direction; -1 ccw, 1 cw TODO: make this automatic
332 |     yawCmd.request.param4 = 0;              // relative offset 1, absolute angle 0
333 | 
334 |     // TODO: return true if the call was a success 
335 |     if(mavlink_cmd_srv.call(yawCmd)){
336 |         ROS_INFO("Controlling yaw");
337 |     }else{
338 |         ROS_INFO("Condition yaw command rejected");
339 |     }
340 | }
341 | 
342 | // TODO: @ssorl what is your plan with this
343 | void SpiriGo::setENUVelocity(/*double eastwardVelocity, double northwardVelocity */)
344 | {
345 |     geometry_msgs::TwistStamped control_msg;
346 | 
347 |     control_msg.twist.linear.x = velocity.linear.x;
348 |     control_msg.twist.linear.y = velocity.linear.y;
349 | 
350 |     vel.publish(control_msg);
351 | }
352 | 
353 | ////////////////////////////////////////////////////////////////////////////////
354 | // Action-specific methods - these will sleep until finished!!!
355 | ////////////////////////////////////////////////////////////////////////////////
356 | 
357 | /*******************************************************************************
358 |  *  Full autonomous takeoff action
359 |  *
360 |  *  This will ask the copter to go to GUIDED
361 |  *
362 |  *  @param TakeoffGoalConstPtr the takeoff action goal object 
363 |  ******************************************************************************/
364 | void SpiriGo::armAndTakeOff(const spiri_go::TakeoffGoalConstPtr& goal)
365 | {
366 |     ROS_INFO("server excuteCB!");
367 |     if(!takeoff_as.isActive()||takeoff_as.isPreemptRequested()) return;
368 |     ros::Rate takeoff_rate(5);
369 |     bool success = true;
370 | 
371 |     // Set to guided mode and THEN arm the copter
372 |     bool armed_ = setArmed();
373 |     while(not last_state.armed)
374 |     {
375 |         if(not last_state.guided){
376 |             setGuided();
377 |         }else{
378 |             bool armed_ = setArmed();
379 |         }
380 |         takeoff_rate.sleep();
381 |     }
382 | 
383 |     // Sleep the method until the takeoff is complete
384 |     taking_off = false;
385 |     while(not taking_off)
386 |     {
387 |         takeOff(goal->height);
388 |         takeoff_rate.sleep();
389 |     }
390 |     while(last_alt.altitude < goal->height*0.96)
391 |     {
392 |         // Only wait to reach 96% of the target altitude to handle undershoot
393 |         takeoff_rate.sleep();
394 |         ROS_INFO("height: %f", last_alt.altitude);
395 |     }
396 |     ROS_INFO("took off");
397 |     flying = true;
398 | 
399 |     if(ros::ok()){
400 |         takeoff_as.setSucceeded();
401 |         ROS_INFO("Takeoff finished");
402 |     } else {
403 |         takeoff_as.setAborted();
404 |         ROS_INFO("Takeoff aborted");
405 |     }
406 | }
407 | 
408 | /*******************************************************************************
409 |  *  Full autonomous land action
410 |  *
411 |  *  This will ask the copter to go to LAND mode
412 |  *
413 |  *  @param LandHereGoalConstPtr the land_here action goal object 
414 |  ******************************************************************************/
415 | void SpiriGo::landHere(const spiri_go::LandHereGoalConstPtr& goal)
416 | {
417 |     ROS_INFO("server excuteCB!");
418 |     if(!land_here_as.isActive()||land_here_as.isPreemptRequested()) return;
419 |     ros::Rate land_rate(5);
420 |     bool success = true;
421 | 
422 |     ROS_INFO("Attempting to land in place");
423 | 
424 |     // Change the mode to LAND in APM, which is autonomous landing
425 |     while(last_state.mode != "LAND")
426 |     {
427 |         setMode("LAND");
428 |         land_rate.sleep();
429 |     }
430 | 
431 |     // Sleep until for the robot to think it's 5cm off the ground
432 |     while(location.position.z > 0.05)
433 |     {
434 |         land_rate.sleep();
435 |     }
436 | 
437 |     if(ros::ok()){
438 |         land_here_as.setSucceeded();
439 |         ROS_INFO("Landing finished");
440 |     } else {
441 |         land_here_as.setAborted();
442 |         ROS_INFO("Landing finished");
443 |     }
444 | }
445 | 
446 | 
447 | ////////////////////////////////////////////////////////////////////////////////
448 | // Getter functions
449 | ////////////////////////////////////////////////////////////////////////////////
450 | 
451 | /*******************************************************************************
452 |  *  @return The latest information on whether the copter is armed
453 |  ******************************************************************************/
454 | bool SpiriGo::isArmed(){
455 |     return last_state.armed;
456 | }
457 | 
458 | /*******************************************************************************
459 |  *    @return The latest information on whether the copter is in GUIDED mode
460 |  ******************************************************************************/
461 | bool SpiriGo::isControllable(){
462 |     return last_state.guided;
463 | }
464 | 
465 | /*******************************************************************************
466 |  *    @return The latest information on whether the copter has connected to mavros
467 |  ******************************************************************************/
468 | bool SpiriGo::isValid() {
469 |     return last_connect_state.is_valid;
470 | }
471 | 
472 | /*******************************************************************************
473 |  *  @return The latest information the mode of the copter
474 |  ******************************************************************************/
475 | std::string SpiriGo::getMode(){
476 |     return last_state.mode;
477 | }
478 | 
479 | 
480 | ////////////////////////////////////////////////////////////////////////////////
481 | // Utility Methods 
482 | ////////////////////////////////////////////////////////////////////////////////
483 | 
484 | /*******************************************************************************
485 |  *  Make a best guess as to the current location
486 |  *  since the real value is provided fairly often
487 |  *  don't need to be too precise about it
488 |  *
489 |  *  @param dt_dur how long since the last update
490 |  ******************************************************************************/
491 | void SpiriGo::updateLocation(ros::Duration dt_dur){
492 |     float dt = dt_dur.toSec();
493 |     location.position.x += velocity.linear.x*dt;
494 |     location.position.y += velocity.linear.y*dt;
495 |     location.position.z += velocity.linear.z*dt;
496 |     // TODO: deal with the angular part too
497 | }
498 | 
499 | ////////////////////////////////////////////////////////////////////////////////
500 | // Main loop
501 | ////////////////////////////////////////////////////////////////////////////////
502 | 
503 | void SpiriGo::Loop()
504 | {
505 |     ros::Rate pub_rate(10);
506 | 
507 |     ros::Time last_time = ros::Time::now();
508 |     while (nh.ok()){
509 |         ros::spinOnce();
510 |         if(flying){
511 |             ros::Duration dt =  ros::Time::now() - last_time;
512 |             last_time = ros::Time::now();
513 |             //updateLocation(dt);
514 |             //setENUVelocity();
515 |         }
516 | 
517 |         pub_rate.sleep();
518 |     }
519 | 
520 | }
521 | 
522 | int main(int argc, char **argv)
523 | {
524 | 
525 |     ros::init(argc, argv, "spiri_go");
526 | 
527 |     SpiriGo go_thing;
528 | 
529 |     //wait until apm has connected to mavros
530 |     /*while(!go_thing.isValid()) {
531 |         ;
532 |     }
533 |     ROS_INFO("connected to mavros");*/
534 |     //I found that the nodes not run in order has no effect
535 | 
536 |     //It's better to wait some time
537 |     ros::Duration(2).sleep();
538 | 
539 |     //change stream rate, important
540 |     while(!go_thing.setStreamRate(0, 10)) {   //-all  
541 |     }
542 | 
543 |     ROS_INFO("set action server!");
544 | 
545 |     // start action servers
546 |     go_thing.takeoff_as.start();
547 |     go_thing.land_here_as.start();
548 | 
549 |     go_thing.Loop();
550 | 
551 |     return 0;
552 | }
553 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/spiri_go_client.cpp~:
--------------------------------------------------------------------------------
 1 | /**
 2 |  * simpely takeoff and land
 3 |  */
 4 | 
 5 | #include "ros/ros.h"
 6 | #include <actionlib/client/simple_action_client.h>
 7 | #include <actionlib/client/terminal_state.h>
 8 | #include <spiri_go/TakeoffAction.h>
 9 | 
10 | int main(int argc, char **argv) {
11 | 	ros::init(argc, argv, "spiri_go_client");
12 | 	
13 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);  //first param: server name
14 | 
15 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
16 | 
17 | 	ac.waitForServer();	
18 | 	
19 | 	ROS_INFO("Action server started--Taking off!");
20 | 
21 | 	spiri_go::TakeoffGoal goal;
22 | 
23 | 	//set the target height to take off!
24 | 	goal.height = 10;
25 | 	ac.sendGoal(goal);
26 | 
27 | 	//wait for action(takeOff) to return
28 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
29 | 
30 | 	if(is_takeOff_finished ) {
31 | 		ROS_INFO("reached target height!");
32 | 	}
33 | 	else {
34 | 		ROS_INFO("should set mode Land!");
35 | 	}
36 | 
37 | 	return 0;
38 | 
39 | }
40 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/takeoff_and_land.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | /*******************************************************************************
 3 |  *  The Spiri Project
 4 |  *
 5 |  *  File: takeoff_and_land.cpp
 6 |  *
 7 |  *  Purpose: simpely takeoff and land for autonomous flights.
 8 |  *
 9 |  *  @author Peng Cheng
10 |  *  @version 0.1.0 2015/12/24
11 |  *  @version 0.1.1 2016/03/16
12 |  ******************************************************************************/
13 | 
14 | #include "ros/ros.h"
15 | #include <actionlib/client/simple_action_client.h>
16 | #include <actionlib/client/terminal_state.h>
17 | #include <spiri_go/TakeoffAction.h>
18 | #include <spiri_go/LandHereAction.h>
19 | 
20 | int main(int argc, char **argv) {
21 | 	ros::init(argc, argv, "spiri_go_client");
22 | 
23 | 	//It's better to wait sonme time, for this file has no NodeHandle, we should add ros::Time::init()
24 | 	ros::Time::init();
25 | 	ros::Duration(3).sleep();
26 | 	
27 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);      //first param: server name, second param: true means a new thread
28 | 	actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
29 | 
30 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
31 | 
32 | 	ac.waitForServer();	
33 | 	
34 | 	ROS_INFO("Action server started--Taking off!");
35 | 
36 | 	spiri_go::TakeoffGoal goal;
37 | 
38 | 	//set the target height to take off!
39 | 	goal.height = 3;
40 | 	ac.sendGoal(goal);
41 | 
42 | 	//wait for action(takeOff) to return
43 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
44 | 
45 | 	if(is_takeOff_finished ) {
46 | 		ROS_INFO("reached target height!");
47 | 
48 | 		//do something
49 | 		ros::Duration(20).sleep();
50 | 	}
51 | 	else {
52 | 		ROS_INFO("should set mode Land!");
53 | 	}
54 | 
55 | 	//land
56 | 	ROS_INFO("Waiting for action server(land) to start.");
57 | 	ac1.waitForServer();	
58 | 	ROS_INFO("Start to land!.");
59 | 	
60 | 	spiri_go::LandHereGoal goal_1;
61 | 	goal_1.height = 0;
62 | 	ac1.sendGoal(goal_1);
63 | 
64 | 	//wait for action(land) to return
65 | 	bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
66 | 	if(is_land_finished ) {
67 | 		ROS_INFO("Land success!");
68 | 	}
69 | 
70 | 	return 0;
71 | 
72 | }
73 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/takeoff_and_land.cpp~:
--------------------------------------------------------------------------------
 1 | 
 2 | /*******************************************************************************
 3 |  *  The Spiri Project
 4 |  *
 5 |  *  File: takeoff_and_land.cpp
 6 |  *
 7 |  *  Purpose: simpely takeoff and land for autonomous flights.
 8 |  *
 9 |  *  @author Peng Cheng
10 |  *  @version 0.1.0 2015/12/24
11 |  *  @version 0.1.1 2016/03/16
12 |  ******************************************************************************/
13 | 
14 | #include "ros/ros.h"
15 | #include <actionlib/client/simple_action_client.h>
16 | #include <actionlib/client/terminal_state.h>
17 | #include <spiri_go/TakeoffAction.h>
18 | #include <spiri_go/LandHereAction.h>
19 | 
20 | int main(int argc, char **argv) {
21 | 	ros::init(argc, argv, "spiri_go_client");
22 | 
23 | 	//It's better to wait sonme time, for this file has no NodeHandle, we should add ros::Time::init()
24 | 	ros::Time::init();
25 | 	ros::Duration(3).sleep();
26 | 	
27 | 	actionlib::SimpleActionClient<spiri_go::TakeoffAction> ac("spiri_take_off", true);      //first param: server name, second param: true means a new thread
28 | 	actionlib::SimpleActionClient<spiri_go::LandHereAction> ac1("spiri_land_here", true);  //first param: server name, second param: false means not to use threads
29 | 
30 | 	ROS_INFO("Waiting for action server(takeOff) to start.");
31 | 
32 | 	ac.waitForServer();	
33 | 	
34 | 	ROS_INFO("Action server started--Taking off!");
35 | 
36 | 	spiri_go::TakeoffGoal goal;
37 | 
38 | 	//set the target height to take off!
39 | 	goal.height = 3;
40 | 	ac.sendGoal(goal);
41 | 
42 | 	//wait for action(takeOff) to return
43 | 	bool is_takeOff_finished = ac.waitForResult(ros::Duration(60.0));
44 | 
45 | 	if(is_takeOff_finished ) {
46 | 		ROS_INFO("reached target height!");
47 | 
48 | 		//do something
49 | 		ros::Duration(30).sleep();
50 | 	}
51 | 	else {
52 | 		ROS_INFO("should set mode Land!");
53 | 	}
54 | 
55 | 	//land
56 | 	ROS_INFO("Waiting for action server(land) to start.");
57 | 	ac1.waitForServer();	
58 | 	ROS_INFO("Start to land!.");
59 | 	
60 | 	spiri_go::LandHereGoal goal_1;
61 | 	goal_1.height = 0;
62 | 	ac1.sendGoal(goal_1);
63 | 
64 | 	//wait for action(land) to return
65 | 	bool is_land_finished = ac1.waitForResult(ros::Duration(60.0));
66 | 	if(is_land_finished ) {
67 | 		ROS_INFO("Land success!");
68 | 	}
69 | 
70 | 	return 0;
71 | 
72 | }
73 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/test/spiri_go_test.cpp:
--------------------------------------------------------------------------------
 1 |  // Bring in spiri_go's API
 2 | #include "spiri_go.h"
 3 | // Bring in gtest
 4 | #include <gtest/gtest.h>
 5 | 
 6 | TEST(SpiriGoTests, Takeoff){
 7 | 
 8 | }
 9 | 
10 | // Run all the tests that were declared with TEST()
11 | int main(int argc, char **argv){
12 | 	testing::InitGoogleTest(&argc, argv);
13 | 	return RUN_ALL_TESTS();
14 | }
15 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/test/test_com.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | /*******************************************************************************
 3 |  *  The Spiri Project
 4 |  *
 5 |  *  File: leader.cpp
 6 |  *
 7 |  *  Purpose: Formation control of leaders with collision avoidance using releative position.
 8 |  *
 9 |  *  @author Peng Cheng
10 |  *  @version 0.1.0    2016/4/06
11 |  ******************************************************************************/
12 | 
13 | #include "ros/ros.h"
14 | #include <actionlib/client/simple_action_client.h>
15 | #include <actionlib/client/terminal_state.h>
16 | #include <spiri_go/TakeoffAction.h>
17 | #include "spiri_go.h"
18 | #include <std_msgs/String.h>
19 | #include <sensor_msgs/NavSatFix.h>
20 | #include <geometry_msgs/TwistStamped.h>
21 | #include <sstream>
22 | #include <px4ros/gpsPosition.h>
23 | #include <math.h>
24 | #include <fstream>
25 | 
26 | 
27 | //global variables
28 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
29 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
30 | 
31 | #define R  6371000         //meter
32 | #define PI 3.1415926535898
33 | 
34 | /*******************************************************************************
35 |  *  Callback function on the gps of the copter
36 |  *
37 |  *  @param gpsPositionConstPtr& the gps callback pointer
38 |  ******************************************************************************/
39 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
40 |    selfGlobalPos = *msg;
41 | }
42 | 
43 | /*******************************************************************************
44 |  *  Callback function on the gps of the neighbour copter
45 |  *
46 |  *  @param gpsPositionConstPtr& the gps callback pointer
47 |  ******************************************************************************/
48 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
49 | {
50 |     neigGlobalPos = *msg;
51 | }
52 | 
53 | /*******************************************************************************
54 |  *  Main function
55 |  *
56 |  *  @param 
57 |  ******************************************************************************/
58 | int main(int argc, char **argv) {
59 | 	ros::init(argc, argv, "spiri_go_client");
60 | 	ros::NodeHandle nh;
61 | 
62 | 	ros::Rate loop_rate(10.0);
63 | 
64 | 	// Subscriber to the Copter gps
65 | 	string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
66 | 	ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
67 | 
68 | 	// Subscriber to the neighbour gps
69 | 	string neigGps_ = nh.resolveName("neighbour_position");
70 | 	ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
71 | 
72 | 	//Publisher to the gps(toString)
73 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
74 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
75 | 
76 | 	while(nh.ok()) {
77 | 		//1.publish self gps(toString)
78 | 		std_msgs::String gps_string;
79 | 		std::stringstream ss;
80 | 		//ss << "01o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
81 | 		ss << "x1o"<<120.87654321<<"a"<<30.12345678<<"#";
82 | 		gps_string.data = ss.str();
83 | 		gps_string_pub.publish(gps_string);
84 | 
85 | 		ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
86 | 		ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
87 | 
88 | 		//2.read neighbour gps
89 | 		ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
90 | 		ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
91 | 		
92 | 		ros::spinOnce();
93 |     		loop_rate.sleep();
94 | 	}
95 | 
96 | 	return 0;
97 | 
98 | }
99 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/src/test/test_com.cpp~:
--------------------------------------------------------------------------------
 1 | 
 2 | /*******************************************************************************
 3 |  *  The Spiri Project
 4 |  *
 5 |  *  File: leader.cpp
 6 |  *
 7 |  *  Purpose: Formation control of leaders with collision avoidance using releative position.
 8 |  *
 9 |  *  @author Peng Cheng
10 |  *  @version 0.1.0    2016/4/06
11 |  ******************************************************************************/
12 | 
13 | #include "ros/ros.h"
14 | #include <actionlib/client/simple_action_client.h>
15 | #include <actionlib/client/terminal_state.h>
16 | #include <spiri_go/TakeoffAction.h>
17 | #include "spiri_go.h"
18 | #include <std_msgs/String.h>
19 | #include <sensor_msgs/NavSatFix.h>
20 | #include <geometry_msgs/TwistStamped.h>
21 | #include <sstream>
22 | #include <px4ros/gpsPosition.h>
23 | #include <math.h>
24 | #include <fstream>
25 | 
26 | 
27 | //global variables
28 | sensor_msgs::NavSatFix selfGlobalPos;    //self gps subscribed from mavros
29 | px4ros::gpsPosition neigGlobalPos;       //neighbour gps subscribed from leader_control_decode.py
30 | 
31 | #define R  6371000         //meter
32 | #define PI 3.1415926535898
33 | 
34 | /*******************************************************************************
35 |  *  Callback function on the gps of the copter
36 |  *
37 |  *  @param gpsPositionConstPtr& the gps callback pointer
38 |  ******************************************************************************/
39 | void gpsSubCb(const sensor_msgs::NavSatFixConstPtr& msg){
40 |    selfGlobalPos = *msg;
41 | }
42 | 
43 | /*******************************************************************************
44 |  *  Callback function on the gps of the neighbour copter
45 |  *
46 |  *  @param gpsPositionConstPtr& the gps callback pointer
47 |  ******************************************************************************/
48 | void neigGpsSubCb(const px4ros::gpsPositionConstPtr& msg)
49 | {
50 |     neigGlobalPos = *msg;
51 | }
52 | 
53 | /*******************************************************************************
54 |  *  Main function
55 |  *
56 |  *  @param 
57 |  ******************************************************************************/
58 | int main(int argc, char **argv) {
59 | 	ros::init(argc, argv, "spiri_go_client");
60 | 	ros::NodeHandle nh;
61 | 
62 | 	ros::Rate loop_rate(10.0);
63 | 
64 | 	// Subscriber to the Copter gps
65 | 	string gps_ = nh.resolveName("/mavros/global_position/raw/fix");
66 | 	ros::Subscriber gps = nh.subscribe(gps_, 5, gpsSubCb);
67 | 
68 | 	// Subscriber to the neighbour gps
69 | 	string neigGps_ = nh.resolveName("neighbour_position");
70 | 	ros::Subscriber neigGps = nh.subscribe(neigGps_, 5, neigGpsSubCb);	
71 | 
72 | 	//Publisher to the gps(toString)
73 | 	string gps_string_pub_ = nh.resolveName("serial_data_send");
74 | 	ros::Publisher gps_string_pub = nh.advertise<std_msgs::String>(gps_string_pub_, 5);
75 | 
76 | 	while(nh.ok()) {
77 | 		//1.publish self gps(toString)
78 | 		std_msgs::String gps_string;
79 | 		std::stringstream ss;
80 | 		//ss << "01o"<<selfGlobalPos.longitude<<"a"<<selfGlobalPos.latitude<<"#";
81 | 		ss << "x1o"<<120.87654321<<"a"<<30.12345678<<"#";
82 | 		gps_string.data = ss.str();
83 | 		gps_string_pub.publish(gps_string);
84 | 
85 | 		ROS_INFO("self_lat:%f", selfGlobalPos.latitude);
86 | 		ROS_INFO("self_lon:%f", selfGlobalPos.longitude);
87 | 
88 | 		//2.read neighbour gps
89 | 		ROS_INFO("Neigb_lat:%f", neigGlobalPos.lat);
90 | 		ROS_INFO("Neigb_lon:%f", neigGlobalPos.lon);
91 | 		
92 | 		ros::spinOnce();
93 |     		loop_rate.sleep();
94 | 	}
95 | 
96 | 	return 0;
97 | 
98 | }
99 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/srv/LastState.srv:
--------------------------------------------------------------------------------
1 | # No params
2 | ---
3 | bool connected
4 | bool armed
5 | bool guided
6 | string mode


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/srv/LocalPosition.srv:
--------------------------------------------------------------------------------
1 | # requires no input
2 | ---
3 | float64 x
4 | float64 y
5 | float64 z


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/test/api.py:
--------------------------------------------------------------------------------
 1 | import unittest
 2 | from spiripy import api, spiri_except
 3 | import os, subprocess
 4 | # Load specific service and action server definitions
 5 | from spiri_go.msg import *
 6 | from spiri_go.srv import *
 7 | 
 8 | class TestSpiriApi(unittest.TestCase):
 9 |     @classmethod
10 |     def setUpClass(self):
11 |         # print "setting the class up."
12 |         # roslaunch spiri_go
13 |         self.ros = subprocess.Popen(
14 |             ["roslaunch", "spiri_go", "sitl.launch"],
15 |             preexec_fn=os.setsid,
16 |             #stdout=subprocess.PIPE, stdin=subprocess.PIPE, stderr=subprocess.PIPE
17 |         )
18 | 
19 |     @classmethod
20 |     def tearDownClass(self):
21 |         #os.kill(self.ros.pid, 15)
22 |         os.killpg(os.getpgid(self.ros.pid), 15)
23 |         os.waitpid(self.ros.pid, os.WUNTRACED)
24 | 
25 |     def setUp(self):
26 |         self.spiri_go = api.SpiriGo()
27 | 
28 |     # TESTS OF ACTIONS
29 |     # verify that client getter fails when it should
30 |     def test_bogus_action_name(self):
31 |         with self.assertRaises(spiri_except.SpiriGoConnectionError):
32 |             self.spiri_go.getActionClient("bogus_action_name", TakeoffAction)
33 | 
34 |     # Use the client test as an example to base other's on
35 |     # any comments that apply to all may go only here
36 |     def test_takeoff_client(self):
37 |         action_name = "spiri_take_off"
38 |         # Attempt to create the client
39 |         self.spiri_go.getActionClient(action_name, TakeoffAction)
40 |         # Test that the goal can be created and has the right parameters
41 |         goal = TakeoffGoal()
42 |         self.assertEquals(goal.height, 0)
43 | 
44 |     # TESTS OF SERVICES
45 |     def test_bogus_service_name(self):
46 |         with self.assertRaises(spiri_except.SpiriGoConnectionError):
47 |             self.spiri_go.getServiceClient("bogus_service_name", LastState)
48 | 
49 |     # test getting the state
50 |     def test_get_state(self):
51 |         #service_name = 'spiri_state'
52 |         #state_sc = self.spiri_go.getServiceClient(service_name, LastState)
53 |         state = self.spiri_go.getState()
54 |         # check that the state is in it's expected starting configuration
55 |         self.assertFalse(state.connected)
56 | 
57 |     def test_get_local_position(self):
58 |         service_name = 'spiri_local_position'
59 |         state_sc = self.spiri_go.getServiceClient(service_name, LocalPosition)
60 | 
61 |     # Test the wait_for_fcu function's timeout feature
62 |     def test_wait_for_fcu_timeout(self):
63 |         with self.assertRaises(spiri_except.SpiriGoConnectionError):
64 |             # should throw an error since there's no sim to connect to
65 |             self.spiri_go.wait_for_fcu(1)
66 | 
67 | 
68 | if __name__ == "__main__":
69 |     unittest.main()
70 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/test/sim.py:
--------------------------------------------------------------------------------
 1 | import unittest
 2 | from spiripy import api, spiri_except
 3 | import os, subprocess
 4 | # Load specific service and action server definitions
 5 | from spiri_go.msg import *
 6 | from spiri_go.srv import *
 7 | 
 8 | # The setup and teardown of these is very slow (almost a minute worst case),
 9 | # therefore it is recomended to use large monolithic testcases to reduce
10 | # overhead. Generally though, do not test something here if it can be
11 | # tested just as well without the simulator. Note that mavros is on for
12 | # all test suits. This is the only one that uses the sitl sim though.
13 | class TestSpiriApiWithSim(unittest.TestCase):
14 |     def setUp(self):
15 |         # launch the simulator
16 |         home = os.path.expanduser("~")
17 |         ardu_path = os.path.join(home, "ardupilot", "ArduCopter")
18 |         #os.chdir(ardu_path)
19 |         self.sim = subprocess.Popen(
20 |             "sim_vehicle.sh --daemon",
21 |             cwd=ardu_path,
22 |             preexec_fn=os.setsid,
23 |             shell=True,
24 |             #stdin=subprocess.PIPE,stdout=subprocess.PIPE,stderr=subprocess.PIPE
25 |         )
26 |         # launch ROS
27 |         self.ros = subprocess.Popen(
28 |             ["roslaunch", "spiri_go", "sitl.launch"],
29 |             preexec_fn=os.setsid,
30 |             #stdin=subprocess.PIPE,stdout=subprocess.PIPE,stderr=subprocess.PIPE
31 |         )
32 |         self.spiri_go = api.SpiriGo()
33 | 
34 |     def tearDown(self):
35 |         # kill ros
36 |         os.killpg(os.getpgid(self.ros.pid), 15)
37 |         os.waitpid(self.ros.pid, os.WUNTRACED)
38 |         # kill the simulator
39 |         os.killpg(os.getpgid(self.sim.pid), 15)
40 |         os.waitpid(self.sim.pid, os.WUNTRACED)
41 |         # wait for the processes to end
42 | 
43 |     def test(self):
44 |         try:
45 |             print 'waiting 20 seconds for ROS'
46 |             self.spiri_go.wait_for_ros(20)
47 |         except:
48 |             # print an explanation, then reraise the error
49 |             print "Can't connect to ROS. Check your system setup."
50 |             raise
51 | 
52 |         try:
53 |             print 'waiting 20 seconds for the flight controller'
54 |             self.spiri_go.wait_for_fcu(20)
55 |         except:
56 |             # print an explanation, then reraise the error
57 |             print "Can't connect to simulator. Check your system setup."
58 |             raise
59 | 
60 |         print 'Test Takeoff'
61 |         to_height = 5
62 |         self.spiri_go.takeoff(to_height)
63 |         # This blocks until takeoff is over
64 |         local = self.spiri_go.get_local_position()
65 |         # ensure that you are close enough to the takeoff goal
66 |         self.assertGreater(local.z, to_height*0.96)
67 |         self.assertLess(local.z, to_height*1.04)
68 | 
69 |         # Test Landing
70 |         self.spiri_go.land()
71 |         # block while waiting to land
72 |         local = self.spiri_go.get_local_position()
73 |         # a assume it will be at least this low after it's landed
74 |         self.assertLess(local.z, 0.1)
75 | 
76 | 
77 | if __name__ == "__main__":
78 |     unittest.main()
79 | 


--------------------------------------------------------------------------------
/multiDrone-ROS/spiri_go/test/sitl.test:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<!-- First launch mavros -->
 3 | 
 4 | 	<arg name="fcu_url" default="udp://127.0.0.1:14550@" />
 5 | 	<arg name="gcs_url" default="udp://127.0.0.1:14551@" />
 6 | 	<arg name="tgt_system" default="1" />
 7 | 	<arg name="tgt_component" default="1" />
 8 | 	<arg name="log_output" default="screen" />
 9 | 
10 | 	<include file="$(find mavros)/launch/node.launch">
11 | 		<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
12 | 		<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
13 | 
14 | 		<arg name="fcu_url" value="$(arg fcu_url)" />
15 | 		<arg name="gcs_url" value="$(arg gcs_url)" />
16 | 		<arg name="tgt_system" value="$(arg tgt_system)" />
17 | 		<arg name="tgt_component" value="$(arg tgt_component)" />
18 | 		<arg name="log_output" value="$(arg log_output)" />
19 | 	</include>
20 | 
21 | 	<!-- Launch SpiriGo -->
22 | 
23 | 	<node pkg="spiri_go" name="spiri_go" type="spiri_go" output="screen"></node>
24 | 	<test pkg="spiri_go" name="spiri_go_test" test-name="spiri_go_test" type="spiri_go_test"></test>
25 | </launch>
26 | 


--------------------------------------------------------------------------------