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 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 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