def callback_depth(depth):
image_buffer = drone.bridge.imgmsg_to_cv2(depth,
desired_encoding="passthrough")
mean_left = 0
mean_right = 0
matrix = depth.height * depth.width
for row in range(depth.height):
for col in range(depth.width):
pix = image_buffer[row][col]
if pix < 1000:
if (col < depth.width / 2):
mean_left += pix
else:
mean_right += pix
mean_left /= matrix * 0.5
mean_right /= matrix * 0.5
message = PprzMessage("intermcu", "IMCU_LR_MEAN_DIST")
message.set_values([np.float(mean_left), np.float(mean_right)])
serial.interface.send(message, drone.id)
def parse_pprz_msg(callback, ivy_msg):
"""
Parse an Ivy message into a PprzMessage.
Basically parts/args in string are separated by space, but char array can also contain a space:
|f,o,o, ,b,a,r| in old format or "foo bar" in new format
:param callback: function to call with ac_id and parsed PprzMessage as params
:param ivy_msg: Ivy message string to parse into PprzMessage
"""
# first split on array delimiters
l = re.split('([|\"][^|\"]*[|\"])', ivy_msg)
# strip spaces and filter out emtpy strings
l = [str.strip(s) for s in l if str.strip(s) is not '']
data = []
for s in l:
# split non-array strings further up
if '|' not in s and '"' not in s:
data += s.split(' ')
else:
data.append(s)
# ignore ivy message with less than 3 elements
if len(data) < 3:
return
# normal format is "sender_name msg_name msg_payload..."
# advanced format has requests and answers (with request_id as 'pid_index')
# request: "sender_name request_id msg_name_REQ msg_payload..."
# answer: "request_id sender_name msg_name msg_payload..."
# check for request_id in first or second string (-> advanced format with msg_name in third string)
advanced = False
if re.search("[0-9]+_[0-9]+", data[0]) or re.search(
"[0-9]+_[0-9]+", data[1]):
advanced = True
msg_name = data[2] if advanced else data[1]
# check which message class it is
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + ivy_msg)
return
# pass non-telemetry messages with ac_id 0
if msg_class == "telemetry":
try:
ac_id = int(data[0])
except ValueError:
print("ignoring message " + ivy_msg)
sys.stdout.flush()
else:
ac_id = 0
payload = data[3:] if advanced else data[2:]
values = list(filter(None, payload))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
# finally call the callback, passing the aircraft id and parsed message
callback(ac_id, msg)
def parse_pprz_msg(callback, ivy_msg):
"""
Parse an Ivy message into a PprzMessage.
Basically parts/args in string are separated by space, but char array can also contain a space:
|f,o,o, ,b,a,r| in old format or "foo bar" in new format
:param callback: function to call with ac_id and parsed PprzMessage as params
:param ivy_msg: Ivy message string to parse into PprzMessage
"""
# first split on array delimiters
l = re.split('([|\"][^|\"]*[|\"])', ivy_msg)
# strip spaces and filter out emtpy strings
l = [str.strip(s) for s in l if str.strip(s) is not '']
data = []
for s in l:
# split non-array strings further up
if '|' not in s and '"' not in s:
data += s.split(' ')
else:
data.append(s)
# ignore ivy message with less than 3 elements
if len(data) < 3:
return
# normal format is "sender_name msg_name msg_payload..."
# advanced format has requests and answers (with request_id as 'pid_index')
# request: "sender_name request_id msg_name_REQ msg_payload..."
# answer: "request_id sender_name msg_name msg_payload..."
# check for request_id in first or second string (-> advanced format with msg_name in third string)
advanced = False
if re.search("[0-9]+_[0-9]+", data[0]) or re.search("[0-9]+_[0-9]+", data[1]):
advanced = True
msg_name = data[2] if advanced else data[1]
# check which message class it is
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + ivy_msg)
return
# pass non-telemetry messages with ac_id 0
if msg_class == "telemetry":
try:
ac_id = int(data[0])
except ValueError:
print("ignoring message " + ivy_msg)
sys.stdout.flush()
else:
ac_id = 0
payload = data[3:] if advanced else data[2:]
values = list(filter(None, payload))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
# finally call the callback, passing the aircraft id and parsed message
callback(ac_id, msg)
def _message_req(self, msg_name, cb, params=None):
bind_id = None
def _cb(sender, msg):
if bind_id is not None:
self._ivy.unsubscribe(bind_id)
cb(sender, msg)
req_id = self._get_req_id()
req_regex = '^{} ([^ ]* +{}( .*|$))'.format(req_id, msg_name)
bind_id = self._ivy.subscribe(_cb, req_regex)
req_msg = PprzMessage('ground', '{}_REQ'.format(msg_name))
if params is not None:
req_msg.set_values(params)
#FIXME we shouldn't use directly Ivy, but pprzlink python API is not supporting the request id for now
IvySendMsg('pprz_connect {} {} {}'.format(
req_id, req_msg.name, req_msg.payload_to_ivy_string()))
def on_ivy_msg(self, agent, *larg):
""" Split ivy message up into the separate parts
Basically parts/args in string are separated by space, but char array can also contain a space:
|f,o,o, ,b,a,r| in old format or "foo bar" in new format
"""
# return if no callback is set
if self.callback is None:
return
# first split on array delimiters
l = re.split('([|\"][^|\"]*[|\"])', larg[0])
# strip spaces and filter out emtpy strings
l = [str.strip(s) for s in l if str.strip(s) is not '']
data = []
for s in l:
# split non-array strings further up
if '|' not in s and '"' not in s:
data += s.split(' ')
else:
data.append(s)
# ignore ivy message with less than 3 elements
if len(data) < 3:
return
# check which message class it is
msg_name = data[1]
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + larg[0])
return
# pass non-telemetry messages with ac_id 0
if msg_class == "telemetry":
try:
ac_id = int(data[0])
except ValueError:
print("ignoring message " + larg[0])
sys.stdout.flush()
else:
ac_id = 0
values = list(filter(None, data[2:]))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
self.callback(ac_id, msg)
def on_ivy_msg(self, agent, *larg):
""" Split ivy message up into the separate parts
Basically parts/args in string are separated by space, but char array can also contain a space:
|f,o,o, ,b,a,r| in old format or "foo bar" in new format
"""
# return if no callback is set
if self.callback is None:
return
# first split on array delimiters
l = re.split('([|\"][^|\"]*[|\"])', larg[0])
# strip spaces and filter out emtpy strings
l = [str.strip(s) for s in l if str.strip(s) is not '']
data = []
for s in l:
# split non-array strings further up
if '|' not in s and '"' not in s:
data += s.split(' ')
else:
data.append(s)
# ignore ivy message with less than 3 elements
if len(data) < 3:
return
# check which message class it is
msg_name = data[1]
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + larg[0])
return
# pass non-telemetry messages with ac_id 0
if msg_class == "telemetry":
try:
ac_id = int(data[0])
except ValueError:
print("ignoring message " + larg[0])
sys.stdout.flush()
else:
ac_id = 0
values = list(filter(None, data[2:]))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
self.callback(ac_id, msg)
def callback(pose, quality):
dx_yaw, dy_yaw = xy_yaw_correction()
dy_pitch, dz_pitch = yz_pitch_correction()
dx_roll, dz_roll = xz_roll_correction()
new_x = -pose.pose.position.y * 100 + dx_yaw + dx_roll
new_y = pose.pose.position.x * 100 + dy_yaw + dy_pitch
new_z = pose.pose.position.z * 100 + dz_pitch + dz_roll
new_qx = pose.pose.orientation.x
new_qy = pose.pose.orientation.y
new_qz = pose.pose.orientation.z
new_qw = pose.pose.orientation.w
# Differentiate the position to get the speed
if drone.hasMoved(new_x, new_y, new_z, new_qx, new_qy, new_qz, new_qw):
drone.vel_x += new_x - drone.x
drone.vel_y += new_y - drone.y
drone.vel_z += new_z - drone.z
drone.vel_samples += 1
drone.x = new_x
drone.y = new_y
drone.z = new_z
drone.qx = new_qx
drone.qy = new_qy
drone.qz = new_qz
drone.qw = new_qw
# Calculate the derivative of the sum to get the correct velocity
drone.vel_transmit += 1
if drone.vel_samples >= min_velocity_samples:
sample_time = drone.vel_transmit / freq_transmit
drone.speed_x = drone.vel_x / sample_time
drone.speed_y = drone.vel_y / sample_time
drone.speed_z = drone.vel_z / sample_time
drone.vel_x = 0
drone.vel_y = 0
drone.vel_z = 0
drone.vel_samples = 0
drone.vel_transmit = 0
# Normalized rotations in rad
rotations = quaternionToEulerianAngle(drone.qx, drone.qy, drone.qz,
drone.qw)
drone.roll, drone.pitch, yaw = map(normRadAngle, rotations)
drone.heading = -yaw
# Generating tow
tow = np.uint32((int(time.strftime("%w")) - 1) * (24 * 60 * 60 * 1000) +
int(time.strftime("%H")) * (60 * 60 * 1000) +
int(time.strftime("%M")) * (60 * 1000) +
int(time.strftime("%S")) * 1000 +
int(datetime.now().microsecond / 1000))
try:
message = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
message.set_values([
np.uint8(drone.id),
np.int32(drone.x),
np.int32(drone.y), # x and y are swapped /!\
np.int32(drone.z),
np.int32(drone.speed_x),
np.int32(drone.speed_y),
np.int32(drone.speed_z),
np.uint32(tow),
np.int32(drone.heading * 10000000)
])
if (quality.quality.value != 0) and (quality.quality.value != 4):
if lights.hasStopped:
light_data = ThreadLight(3, 400, [
"front:left:blue", "front:right:blue", "rear:left:blue",
"rear:right:blue"
])
light_data.start()
lights.hasStopped = False
udp.interface.send(message, drone.id, drone.address)
elif not lights.hasStopped:
light_problem = ThreadLight(
1,
1500, [
"front:left:red", "front:right:red", "rear:left:red",
"rear:right:red"
],
sleepy="False")
light_problem.start()
lights.hasStopped = True
except KeyError as e:
pass
def callback(pose):
# print rospy.loginfo(mag)
# print rospy.loginfo(pose)
new_x = pose.pose.position.x * 100
new_y = pose.pose.position.y * 100
new_z = pose.pose.position.z * 100
new_qx = pose.pose.orientation.x
new_qy = pose.pose.orientation.y
new_qz = pose.pose.orientation.z
new_qw = pose.pose.orientation.w
# Differentiate the position to get the speed
if drone.hasMoved(new_x, new_y, new_z, new_qx, new_qy, new_qz, new_qw):
drone.vel_x += new_x - drone.x
drone.vel_y += new_y - drone.y
drone.vel_z += new_z - drone.z
drone.vel_samples += 1
drone.x = new_x
drone.y = new_y
drone.z = new_z
drone.qx = new_qx
drone.qy = new_qy
drone.qz = new_qz
drone.qw = new_qw
# Calculate the derivative of the sum to get the correct velocity
drone.vel_transmit += 1
if drone.vel_samples >= min_velocity_samples:
sample_time = drone.vel_transmit / freq_transmit
drone.speed_x = drone.vel_x / sample_time
drone.speed_y = drone.vel_y / sample_time
drone.speed_z = drone.vel_z / sample_time
drone.vel_x = 0
drone.vel_y = 0
drone.vel_z = 0
drone.vel_samples = 0
drone.vel_transmit = 0
# Normalized heading in rad
heading = quaternionToEulerianAngle(drone.qx, drone.qy, drone.qz,
drone.qw)[2]
# Generating tow
tow = np.uint32((int(time.strftime("%w")) - 1) * (24 * 60 * 60 * 1000) +
int(time.strftime("%H")) * (60 * 60 * 1000) +
int(time.strftime("%M")) * (60 * 1000) +
int(time.strftime("%S")) * 1000 +
int(datetime.now().microsecond / 1000))
message = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
message.set_values([
np.uint8(drone.id),
np.int32(drone.x),
np.int32(drone.y),
np.int32(drone.z),
np.int32(drone.speed_x),
np.int32(drone.speed_y),
np.int32(drone.speed_z),
np.uint32(tow),
np.int32(heading)
])
udp.interface.send(message, drone.id, drone.address)
if not lights.hasBlinked:
lights.blink(3, 400, [
"front:left:blue", "front:right:blue", "rear:left:blue",
"rear:right:blue"
])
lights.hasBlinked = True
