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)