Esempio n. 1
0
def pythontask_to_rostask(python_task):
    if (python_task.getType() == "target"):
        return Task(python_task.name, Task.TYPE_TARGET, python_task.target,
                    rad2degf(python_task.orientation), [
                        python_task.precision.x, python_task.precision.z,
                        python_task.precisionYAW
                    ], python_task.ID)

    elif (python_task.getType() == "loiter"):
        return Task(python_task.name, Task.TYPE_LOITER, Point(0, 0, 0), 0,
                    [python_task.waitTime], python_task.ID)

    elif (python_task.getType() == "takeoff"):
        return Task(python_task.name, Task.TYPE_TAKEOFF, Point(0, 0, 0), 0,
                    [python_task.precision], python_task.ID)

    elif (python_task.getType() == "land"):
        return Task(python_task.name, Task.TYPE_LAND, Point(0, 0, 0), 0,
                    [python_task.precision], python_task.ID)

    elif (python_task.getType() == "grab"):
        return Task(python_task.name, Task.TYPE_GRAB, Point(0, 0, 0), 0,
                    [python_task.state], python_task.ID)

    elif (python_task.getType() == "init_UAV"):
        return Task(python_task.name, Task.TYPE_INIT_UAV, Point(0, 0, 0), 0,
                    [float(python_task.sleep)], python_task.ID)

    elif (python_task.getType() == "arm"):
        return Task(python_task.name, Task.TYPE_ARM, Point(0, 0, 0), 0,
                    [float(python_task.timeout)], python_task.ID)

    elif (python_task.getType() == "disarm"):
        return Task(python_task.name, Task.TYPE_DISARM, Point(0, 0, 0), 0,
                    [float(python_task.timeout)], python_task.ID)
    else:
        return Task("null", Task.TYPE_NULL, Point(0, 0, 0), 0, [],
                    python_task.ID)
Esempio n. 2
0
def InterfaceKeyboard():
    # Input data
    global pose, laser_position_count
    # Output data
    global setpoint, yawSetPoint, run, position_control
    # Publishers
    global arming_client, set_mode_client

    what = getch()
    if what == "t":
        setpoint.x = setpoint.x - 0.1
    if what == "g":
        setpoint.x = setpoint.x + 0.1
    if what == "f":
        setpoint.y = setpoint.y - 0.1
    if what == "h":
        setpoint.y = setpoint.y + 0.1
    if what == "i":
        setpoint.z = setpoint.z + 0.1
    if what == "k":
        setpoint.z = setpoint.z - 0.1
    if what == "b":
        yawSetPoint = yawSetPoint + 1
    if what == "n":
        yawSetPoint = yawSetPoint - 1
    if what == "c":
        setpoint.x = pose.pose.position.x
        setpoint.y = pose.pose.position.y
        setpoint.z = pose.pose.position.z

    if what == "p":
        setpoint.x = pose.pose.position.x
        setpoint.y = pose.pose.position.y
        position_control = True
    if what == "l":
        position_control = False
    if what == "q":
        arming_client(False)
    if what == "a":
        arming_client(True)
    if what == "e":
        set_mode_client(custom_mode="OFFBOARD")
    if what == "m":
        run = False
        time.sleep(1)
        exit()

    Q = (pose.pose.orientation.x, pose.pose.orientation.y,
         pose.pose.orientation.z, pose.pose.orientation.w)
    euler = tf.transformations.euler_from_quaternion(Q)

    rospy.loginfo("MODE POSITION: ")
    rospy.loginfo("true" if position_control else "false")
    rospy.loginfo("Positions sent : %i", )
    rospy.loginfo("Position x: %s y: %s z: %s", pose.pose.position.x,
                  pose.pose.position.y, pose.pose.position.z)
    rospy.loginfo("Setpoint is now x:%s, y:%s, z:%s", setpoint.x, setpoint.y,
                  setpoint.z)
    rospy.loginfo("IMU :")
    rospy.loginfo("roll : %s", rad2degf(euler[0]))
    rospy.loginfo("pitch : %s", rad2degf(euler[1]))
    rospy.loginfo("yaw : %s", rad2degf(euler[2]))
    rospy.loginfo("wanted yaw : %s", yawSetPoint)
def velocityCB(data):
    global velocity
    velocity = [str(data.twist.linear.x),str(data.twist.linear.y),str(data.twist.linear.z),str(rad2degf(data.twist.angular.z))]
def XkCB(data):
    global Xk 
    Xk = [str(data.x), str(data.y),str(data.z),str(rad2degf(data.w))]
def XkpCB(data):
    global Xkp 
    Xkp = [str(data.x), str(data.y),str(data.z),str(rad2degf(data.w))]
def poseStamped2Array(data, RollPitch=False):
    quaternion = (data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w)
    
    roll, pitch, yaw = euler_from_quaternion(quaternion, axes="sxyz")
    if RollPitch:
        return [ str(data.pose.position.x), str(data.pose.position.y), str(data.pose.position.z), str(rad2degf(roll)), str(rad2degf(pitch)), str(rad2degf(yaw))]
    return [ str(data.pose.position.x), str(data.pose.position.y), str(data.pose.position.z), str(rad2degf(yaw))]
def InterfaceKeyboard():
    # Input data
    global pose, laser_position_count
    # Output data
    global setpoint, yawSetPoint, run, position_control
    # Publishers
    global arming_client, set_mode_client

    what = getch()
    if what == "t":
        setpoint.x = setpoint.x - 0.1
    if what == "g":
        setpoint.x = setpoint.x + 0.1
    if what == "f":
        setpoint.y = setpoint.y - 0.1
    if what == "h":
        setpoint.y = setpoint.y + 0.1
    if what == "i":
        setpoint.z = setpoint.z + 0.1
    if what == "k":
        setpoint.z = setpoint.z - 0.1
    if what == "b":
        yawSetPoint = yawSetPoint + 1
    if what == "n":
        yawSetPoint = yawSetPoint - 1
    if what == "c":
        setpoint.x = pose.pose.position.x
        setpoint.y = pose.pose.position.y
        setpoint.z = pose.pose.position.z

    if what == "p":
        setpoint.x = pose.pose.position.x
        setpoint.y = pose.pose.position.y
        position_control = True
    if what == "l":
        position_control = False
    if what == "q":
        arming_client(False)
    if what == "a":
        arming_client(True)
    if what == "e":
        set_mode_client(custom_mode = "OFFBOARD")
    if what == "m":
        run = False
        time.sleep(1)
        exit()

    Q = (
        pose.pose.orientation.x,
        pose.pose.orientation.y,
        pose.pose.orientation.z,
        pose.pose.orientation.w)
    euler = tf.transformations.euler_from_quaternion(Q)

    rospy.loginfo("MODE POSITION: ")
    rospy.loginfo("true" if position_control else "false")
    rospy.loginfo("Positions sent : %i",  )
    rospy.loginfo("Position x: %s y: %s z: %s", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
    rospy.loginfo("Setpoint is now x:%s, y:%s, z:%s", setpoint.x, setpoint.y, setpoint.z)
    rospy.loginfo("IMU :")
    rospy.loginfo("roll : %s", rad2degf(euler[0]))
    rospy.loginfo("pitch : %s", rad2degf(euler[1]))
    rospy.loginfo("yaw : %s", rad2degf(euler[2]))
    rospy.loginfo("wanted yaw : %s", yawSetPoint)
Esempio n. 8
0
def poseStamped2Array(data, RollPitch=False):
    quaternion = (data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w)

    roll, pitch, yaw = euler_from_quaternion(quaternion, axes="sxyz")
    if RollPitch:
        return [ str(data.pose.position.x), str(data.pose.position.y), str(data.pose.position.z), str(rad2degf(roll)), str(rad2degf(pitch)), str(rad2degf(yaw))]
    return [ str(data.pose.position.x), str(data.pose.position.y), str(data.pose.position.z), str(rad2degf(yaw))]
Esempio n. 9
0
def velocityCB(data):
    global velocity
    velocity = [str(data.twist.linear.x),str(data.twist.linear.y),str(data.twist.linear.z),str(rad2degf(data.twist.angular.z))]