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