def offboard(self,
ignore_thrust=False,
ignore_attitude=False,
ignore_bodyrate=True,
ignore_position=False,
ignore_velocity=True,
ignore_acceleration_force=True):
rate = rospy.Rate(10) # 10hz
mode = offboard_control_mode()
mode.ignore_thrust = ignore_thrust
mode.ignore_attitude = ignore_attitude
mode.ignore_bodyrate_x = ignore_bodyrate
mode.ignore_bodyrate_y = ignore_bodyrate
mode.ignore_bodyrate_z = ignore_bodyrate
mode.ignore_position = ignore_position
mode.ignore_velocity = ignore_velocity
mode.ignore_acceleration_force = ignore_acceleration_force
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("setting offboard mode")
time = rospy.get_rostime().now()
mode.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_off.publish(mode)
rate.sleep()
count = count + 1
# triggers offboard
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 3
pos.return_switch = 3
pos.posctl_switch = 3
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 1
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("triggering offboard")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
def pubRC(self, roll, pitch, thrust, yaw_rate): pub = manual_control_setpoint() pub.timestamp = 0 pub.x = -pitch pub.y = roll pub.z = thrust pub.r = yaw_rate pub.mode_switch = manual_control_setpoint.SWITCH_POS_OFF pub.return_switch = manual_control_setpoint.SWITCH_POS_OFF pub.posctl_switch = manual_control_setpoint.SWITCH_POS_OFF pub.loiter_switch = manual_control_setpoint.SWITCH_POS_OFF pub.offboard_switch = manual_control_setpoint.SWITCH_POS_OFF pub.acro_switch = manual_control_setpoint.SWITCH_POS_NONE self.px4_manual_control.publish(pub)
def pubRC(self, roll, pitch, thrust, yaw_rate):
pub = manual_control_setpoint()
pub.timestamp = 0
pub.x = -pitch
pub.y = roll
pub.z = thrust
pub.r = yaw_rate
pub.mode_switch = manual_control_setpoint.SWITCH_POS_OFF
pub.return_switch = manual_control_setpoint.SWITCH_POS_OFF
pub.posctl_switch = manual_control_setpoint.SWITCH_POS_OFF
pub.loiter_switch = manual_control_setpoint.SWITCH_POS_OFF
pub.offboard_switch = manual_control_setpoint.SWITCH_POS_OFF
pub.acro_switch = manual_control_setpoint.SWITCH_POS_NONE
self.px4_manual_control.publish(pub)
def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True,
ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True):
rate = rospy.Rate(10) # 10hz
mode = offboard_control_mode()
mode.ignore_thrust = ignore_thrust
mode.ignore_attitude = ignore_attitude
mode.ignore_bodyrate = ignore_bodyrate
mode.ignore_position = ignore_position
mode.ignore_velocity = ignore_velocity
mode.ignore_acceleration_force = ignore_acceleration_force
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("setting offboard mode")
time = rospy.get_rostime().now()
mode.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_off.publish(mode)
rate.sleep()
count = count + 1
# triggers offboard
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 3
pos.return_switch = 3
pos.posctl_switch = 3
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 1
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("triggering offboard")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
def arm(self):
rate = rospy.Rate(10) # 10hz
att = CommandAttitudeThrust()
att.header = Header()
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 3
pos.return_switch = 3
pos.posctl_switch = 3
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 3
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("zeroing")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
# Fake input to iris commander
self.pubAtt.publish(att)
rate.sleep()
count = count + 1
pos.r = 1
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("arming")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
rate.sleep()
count = count + 1
def arm(self):
rate = rospy.Rate(10) # 10hz
att = CommandAttitudeThrust()
att.header = Header()
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 3
pos.return_switch = 3
pos.posctl_switch = 3
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 3
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("zeroing")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
# Fake input to iris commander
self.pubAtt.publish(att)
rate.sleep()
count = count + 1
pos.r = 1
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("arming")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
rate.sleep()
count = count + 1
def posctl(self):
rate = rospy.Rate(10) # 10hz
# triggers posctl
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 2
pos.return_switch = 3
pos.posctl_switch = 1
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 3
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("triggering posctl")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
rate.sleep()
count = count + 1
def posctl(self):
rate = rospy.Rate(10) # 10hz
# triggers posctl
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 2
pos.return_switch = 3
pos.posctl_switch = 1
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 3
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("triggering posctl")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1