diff --git a/communication/multirotor_communication.py b/communication/multirotor_communication.py
index b44f9e6..14d89c0 100644
--- a/communication/multirotor_communication.py
+++ b/communication/multirotor_communication.py
@@ -141,28 +141,36 @@ class Communication:
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
def cmd_vel_flu_callback(self, msg):
+ self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
if self.hover_flag == 0:
self.coordinate_frame = 8
self.motion_type = 1
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)
def cmd_vel_enu_callback(self, msg):
+ self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
if self.hover_flag == 0:
self.coordinate_frame = 1
self.motion_type = 1
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)
def cmd_accel_flu_callback(self, msg):
+ self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
if self.hover_flag == 0:
self.coordinate_frame = 8
self.motion_type = 2
self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
def cmd_accel_enu_callback(self, msg):
+ self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
if self.hover_flag == 0:
self.coordinate_frame = 1
self.motion_type = 2
- self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.x,afz=msg.linear.x,yaw_rate=msg.angular.z)
+ self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
+
+ def hover_state_transition(self,x,y,z,w):
+ if abs(x) > 0.005 or abs(y) > 0.005 or abs(z) > 0.005 or abs(w) > 0.005:
+ self.hover_flag = 0
def cmd_callback(self, msg):
if msg.data == '':
@@ -218,7 +226,6 @@ class Communication:
self.hover()
print(self.vehicle_type+'_'+self.vehicle_id+":"+self.flight_mode)
elif self.flightModeService(custom_mode=self.flight_mode):
- self.hover_flag = 0
print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode)
return True
else:
diff --git a/control/multirotor_keyboard_multi_control.py b/control/multirotor_keyboard_control.py
similarity index 96%
rename from control/multirotor_keyboard_multi_control.py
rename to control/multirotor_keyboard_control.py
index 0f2654f..70e25a6 100755
--- a/control/multirotor_keyboard_multi_control.py
+++ b/control/multirotor_keyboard_control.py
@@ -30,8 +30,7 @@ r : return home
t/y : arm/disarm
v/n : takeoff/land
b : offboard
-s : hover(offboard mode) and remove the mask of keyboard control
-k : hover(hover mode) and remove the mask of keyboard control
+s/k : hover and remove the mask of keyboard control
0~9 : extendable mission(eg.different formation configuration)
this will mask the keyboard control
g : control the leader
@@ -55,7 +54,7 @@ r : return home
t/y : arm/disarm
v/n : takeoff(disenabled now)/land
b : offboard
-s or k : hover and remove the mask of keyboard control
+s/k : hover and remove the mask of keyboard control
0~9 : extendable mission(eg.different formation configuration)
this will mask the keyboard control
g : control all drones
@@ -208,12 +207,7 @@ if __name__=="__main__":
elif key == 'g':
ctrl_leader = not ctrl_leader
print_msg()
- elif key == 'k':
- cmd = 'HOVER'
- cmd_vel_mask = False
- print_msg()
- print('Hover')
- elif key == 's' :
+ elif key == 'k' or key == 's':
cmd_vel_mask = False
forward = 0.0
leftward = 0.0
diff --git a/control/plane_keyboard_multi_control.py b/control/plane_keyboard_control.py
similarity index 100%
rename from control/plane_keyboard_multi_control.py
rename to control/plane_keyboard_control.py
diff --git a/control/rover_keyboard_multi_control.py b/control/rover_keyboard_control.py
similarity index 100%
rename from control/rover_keyboard_multi_control.py
rename to control/rover_keyboard_control.py
diff --git a/sitl_config/launch/2d_motion_planning.launch b/sitl_config/launch/2d_motion_planning.launch
deleted file mode 100644
index 998648c..0000000
--- a/sitl_config/launch/2d_motion_planning.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/sitl_config/launch/Drone_with_arm.launch b/sitl_config/launch/Drone_with_arm.launch
deleted file mode 100644
index b947d26..0000000
--- a/sitl_config/launch/Drone_with_arm.launch
+++ /dev/null
@@ -1,53 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/sitl_config/launch/move_base.launch b/sitl_config/launch/move_base.launch
deleted file mode 100644
index c566ad2..0000000
--- a/sitl_config/launch/move_base.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/sitl_config/launch/outdoor3.launch b/sitl_config/launch/outdoor3.launch
index dd84bdf..94a37f5 100644
--- a/sitl_config/launch/outdoor3.launch
+++ b/sitl_config/launch/outdoor3.launch
@@ -12,8 +12,8 @@
-
-
+
+
diff --git a/sitl_config/worlds/outdoor3.world b/sitl_config/worlds/outdoor3.world
new file mode 100644
index 0000000..326d538
--- /dev/null
+++ b/sitl_config/worlds/outdoor3.world
@@ -0,0 +1,40 @@
+
+
+
+
+
+ model://sun
+
+
+ model://sonoma_raceway
+ -295 150 -7 0 0 0
+
+
+ EARTH_WGS84
+ 38.161479
+ -122.454630
+ 488.0
+
+
+ 0 0 -9.8066
+
+
+ quick
+ 10
+ 1.3
+ 0
+
+
+ 0
+ 0.2
+ 100
+ 0.001
+
+
+ 0.004
+ 1
+ 250
+ 6.0e-6 2.3e-5 -4.2e-5
+
+
+