con = Commander()
time.sleep(2)
# waiting for core to be ready
while not con.flag_core_ready:
print("waiting for the vehicle to be armed and in offboard")
time.sleep(1)
# request to transit to multicopter
while not con.flyingrover_mode == "MC":
print("request to transit to multicopter mode")
con.transit_to_mc()
time.sleep(2)
然后,飞车将执行为多旋翼模式设置的航路点任务。为安全考虑,任务结束后将锁定飞车。
# multicopter mission round
con.move(5, 0, 5)
time.sleep(5)
con.move(5, 5, 5)
time.sleep(5)
con.move(0, 5, 5)
time.sleep(5)
con.move(0, 0, 5)
time.sleep(5)
con.land()
time.sleep(5)
#disarm for safety at the end
con.disarm()
rospy.init_node("offboard_node")
rate = rospy.Rate(30) # 30Hz
for i in range(10):
if self.current_heading is not None:
break
else:
print("Waiting for initialization.")
time.sleep(0.5)
self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
self.local_pose.pose.position.y,
self.local_pose.pose.position.z,
self.current_heading)
仿真模式下(暂未支持),程序将解锁飞车并设置为offboard模式。
'''arm and set offboard automatically in simulation mode'''
if flag_simulation_mode:
print("setting arm and offboard in simulation mode...")
for i in range(100):
self.local_target_pub.publish(self.cur_target_pose)
self.arm_state = self.arm()
self.offboard_state = self.offboard()
if self.arm_state and self.offboard_state:
break
time.sleep(0.05)
while not rospy.is_shutdown():
# publications
self.local_target_pub.publish(self.cur_target_pose)
self.flyingrover_mode_pub.publish(self.current_fr_mode)
self.landedstate_pub.publish(String(self.landed_state))
# publish flag to indicate the vehicle is armed and in offboard
if self.arm_state and self.offboard_state:
self.core_ready_pub.publish(True)
else:
self.core_ready_pub.publish(False)
......
# launch a terminal
cd ~/src/flyingrover_workspace/catkinws_offboard
source devel/setup.bash
roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"
# launch a new terminal
cd ~/src/flyingrover_workspace/offboard_demo_python
python3 px4_mavros_run.py
# launch a new terminal
cd ~/src/flyingrover_workspace/offboard_demo_python
python3 commander.py