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()
Example #2
0
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()
Example #3
0
 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