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