def sendSetpoint():
# Input data
global setpoint, yawSetPoint, run, position_control
# Output data
global setPointsCount
setPointsCount = 0
#local_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)
local_setpoint_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=1)
rate = rospy.Rate(30)
while run:
q = quaternion_from_euler(0, 0, deg2radf(yawSetPoint), axes="sxyz")
msg = PoseStamped()
msg.header.stamp = rospy.Time.now()
msg.header.seq=setPointsCount
msg.pose.position.x = float(setpoint.x)
msg.pose.position.y = float(setpoint.y)
msg.pose.position.z = float(setpoint.z)
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
local_setpoint_pub.publish(msg)
setPointsCount = setPointsCount + 1
rate.sleep()
def sendSetpoint():
# Input data
global setpoint, yawSetPoint, run, position_control
# Output data
global setPointsCount
setPointsCount = 0
#local_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)
local_setpoint_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=1)
rate = rospy.Rate(30)
while run:
q = quaternion_from_euler(0, 0, deg2radf(yawSetPoint), axes="sxyz")
msg = PoseStamped()
msg.header.stamp = rospy.Time.now()
msg.header.seq = setPointsCount
msg.pose.position.x = float(setpoint.x)
msg.pose.position.y = float(setpoint.y)
msg.pose.position.z = float(setpoint.z)
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
local_setpoint_pub.publish(msg)
setPointsCount = setPointsCount + 1
rate.sleep()
def __init__(self, name, pointXYZ, yaw = 0, precisionXY = 0.05, precisionZ = 0.05, precisionYAW = 1):
super(target, self).__init__("target", name)
self.target = pointXYZ
self.orientation = deg2radf(yaw)
self.precision = Point(precisionXY, precisionXY, precisionZ)
self.precisionYAW = precisionYAW
self.sent = False