class MAV:
def __init__(self):
self.setpoint_velocity_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=5)
self.setpoint_position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=5)
self.setpoint_attitude_pub = rospy.Publisher('/mavros/setpoint_attitude', PoseStamped, queue_size=5)
# rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.drone_pose_callback)
rospy.Subscriber('/mavros/state', State, self.state_callback)
self.setpoint_velocity = TwistStamped()
self.setpoint_position = PoseStamped()
self.current_pose = Pose()
self.current_state = State()
self.takeoff_state = False
self.auto_state = False
def set_position(self, position):
self.setpoint_position = TwistStamped()
self.setpoint_position.pose.position.x = position[0]
self.setpoint_position.pose.position.y = position[1]
self.setpoint_position.pose.position.z = position[2]
self.setpoint_position.header.stamp = rospy.Time.now()
self.setpoint_position.publish(self.setpoint_position)
print("Setpoint Position Sent: {}, {}, {}".format(position[0], position[1], position[2]) )
def set_velocity(self, velocity):
self.setpoint_velocity = TwistStamped()
self.setpoint_velocity.twist.linear.x = velocity[0]
self.setpoint_velocity.twist.linear.y = velocity[1]
self.setpoint_velocity.twist.linear.z = velocity[2]
self.setpoint_velocity.header.stamp = rospy.Time.now()
self.setpoint_velocity_pub.publish(self.setpoint_velocity)
print("Setpoint Velocity Sent: {}, {}, {}".format(self.setpoint_velocity.twist.linear.x,
self.setpoint_velocity.twist.linear.y,
self.setpoint_velocity.twist.linear.z ) )
def set_velocity_twist(self, velocity):
self.setpoint_velocity = TwistStamped()
self.setpoint_velocity.twist = velocity
self.setpoint_velocity.header.stamp = rospy.Time.now()
self.setpoint_velocity_pub.publish(self.setpoint_velocity)
print("Setpoint Velocity Sent: {}, {}, {}".format(self.setpoint_velocity.twist.linear.x,
self.setpoint_velocity.twist.linear.y,
self.setpoint_velocity.twist.linear.z ) )
def setMode(self, mode):
if self.current_state.mode != mode:
rospy.wait_for_service('/mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
isModeChanged = flightModeService(custom_mode=mode) #return true or false
except rospy.ServiceException, e:
print "service set_mode call failed: %s. %s Mode could not be set. Check that GPS is enabled"% (e, mode)
