Hello, I’m trying to make an iterative offboard flight environment, like if drone reaches certain condition, then it is respawned and continue offboard flight.
For example, if I order drone go to (0,0,10) and when its altitude is over 5, I want drone spawn at (0,0,0) and continue to go to (0,0,10).
I could make drone respawn but as it is respawned, it was still armed and tried to go to target position. So, I tried to turn to land mode so drone could be disarmed, and then restart offboard flight. However, it didn’t worked.
The log is here and I don’t know if I can upload the code, but it looks like this: If I’m not allowed to upload the code, I’ll take it down as soon as I check it.
1. #! /usr/bin/env python
2. import rospy
3. import time
4. from geometry_msgs.msg import PoseStamped, Vector3Stamped
5. from mavros_msgs.msg import *
6. from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest, CommandTOL
7. from std_srvs.srv import Empty
8. from sensor_msgs.msg import NavSatFix
9. current_state = State()
10. current_pose = PoseStamped()
11. def state_cb(msg):
12. global current_state
13. current_state = msg
14. def pose_cb(msg):
15. global current_pose
16. current_pose = msg
17. if __name__ == "__main__":
18. rospy.init_node("offb_node_py")
19. state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
20. local_pos_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped, callback = pose_cb)
21. local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
22. target_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
23. rospy.wait_for_service("/mavros/cmd/arming")
24. arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
25. rospy.wait_for_service("/mavros/set_mode")
26. set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
27. land_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
28. # Setpoint publishing MUST be faster than 2Hz
29. rate = rospy.Rate(20)
30. # Wait for Flight Controller connection
31. while(not rospy.is_shutdown() and not current_state.connected):
32. rate.sleep()
33. pose = PoseStamped()
34. pose.pose.position.x = 0
35. pose.pose.position.y = 0
36. pose.pose.position.z = 10
37. # Send a few setpoints before starting
38. for i in range(100):
39. if(rospy.is_shutdown()):
40. break
41. local_pos_pub.publish(pose)
42. rate.sleep()
43. offb_set_mode = SetModeRequest()
44. offb_set_mode.custom_mode = 'OFFBOARD'
45. land_set_mode = SetModeRequest()
46. land_set_mode.custom_mode = 'AUTO.LAND'
47. land_cmd = CommandTOL()
48. land_cmd._request_class.altitude = 0
49. land_cmd._request_class.longitude = 0
50. land_cmd._request_class.latitude = 0
51. land_cmd._request_class.min_pitch = 0.0
52. land_cmd._request_class.yaw = 0.0
53. arm_cmd = CommandBoolRequest()
54. arm_cmd.value = True
55. last_req = rospy.Time.now()
56. while(not rospy.is_shutdown()):
57. if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
58. if(set_mode_client.call(offb_set_mode).mode_sent == True):
59. rospy.loginfo("OFFBOARD enabled")
60. last_req = rospy.Time.now()
61. else:
62. if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
63. if(arming_client.call(arm_cmd).success == True):
64. rospy.loginfo("Vehicle armed")
65. last_req = rospy.Time.now()
66.
67. if(current_pose.pose.position.z > 5.0):
68. rospy.wait_for_service('/gazebo/reset_world')
69. try:
70. reset_sim = rospy.ServiceProxy('/gazebo/reset_world', Empty)
71. reset_sim()
72. except rospy.ServiceException as e:
73. rospy.logerr("Service call failed: " + str(e))
74. arm_cmd.value = False
75. rospy.wait_for_service("/mavros/set_mode")
76. if(current_state.mode != "AUTO.LAND" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
77. if(set_mode_client.call(land_set_mode).mode_sent==True):
78. rospy.loginfo("AUTO.LAND enabled")
79. land_client.call(land_set_mode)
80. rate.sleep()
81. last_req = rospy.Time.now()
82. rospy.wait_for_service("/mavros/cmd/arming")
83. if(current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
84. if(arming_client.call(arm_cmd).success == True):
85. rospy.loginfo("Vehicle disarmed")
86. last_req = rospy.Time.now()
87. arm_cmd.value = True
88. local_pos_pub.publish(pose)
89. rate.sleep()