def param_callback_final(self, **kwargs): api_callback( self.loop, self.module["maverick_api.modules.api.mavros.ParamSchema"]. update_parameter, **kwargs, )
def rel_alt_callback(self, data): kwargs = {"relativeAltitude": data.data} api_callback( self.loop, self.module["maverick_api.modules.api.mavros.VfrHudSchema"]. set_vfr_hud_message, **kwargs, )
def mission_callback(self, data): # application_log.debug(f"mission_callback: {data}") api_callback( self.loop, self.module["maverick_api.modules.api.mavros.MissionSchema"]. update_mission, data=data, )
def mode_state_callback(self, data): kwargs = { "uuid": "test", "modeString": data.mode, "updateTime": int(time.time()), } api_callback( self.loop, self.module["maverick_api.modules.api.mavros.ModeSchema"]. update_mode, **kwargs, )
def vehicle_params(self, meta_string=None): """Called once on API start up""" # TODO: make vehicle dynamic and choose between px4 and ardupilot # TODO: If this fails try to run it some time in the future param_received, param_list = param_get_all(True) # TODO move this to a background thread or make it an async call (when ROS supports it) application_log.debug(f"Parameters received: {param_received}") param_meta_vehicle = {} for param in param_list: param_meta_vehicle[param.param_id] = { "group": param.param_id.split("_")[0].strip().rstrip("_").upper() } # TODO: handle IOError when mavlink-router is not connected to the AP but mavros is running start_time = time.time() application_log.debug("starting parameter meta fetch") param_meta_server = get_param_meta(meta_string) application_log.debug("finished parameter meta fetch") application_log.debug( f"parameter meta fetch took {time.time() - start_time}s") param_meta = {**param_meta_vehicle, **param_meta_server} # sort the bitmask and values by the keys # at the same time turn the sorted dict into a string for param_key in param_meta: param_meta[param_key]["type"] = "NUMBER" values = param_meta[param_key].get("values", None) if values: sorted_values = {values[k]: k for k in sorted(values)} param_meta[param_key]["values"] = json.dumps(sorted_values) bitmask = param_meta[param_key].get("bitmask", None) if bitmask: sorted_bitmask = {bitmask[k]: k for k in sorted(bitmask)} param_meta[param_key]["bitmask"] = json.dumps(sorted_bitmask) param_meta[param_key]["type"] = "BITMASK" # application_log.info(f"self.param_meta {self.param_meta}") api_callback( self.loop, self.module["maverick_api.modules.api.mavros.ParamSchema"]. update_parameter_meta, **param_meta, ) for param in param_list: kwargs = {"id": param.param_id, "value": param.param_value} self.param_callback_final(**kwargs)
def nav_sat_fix_callback(self, data): kwargs = { "seq": data.header.seq, "secs": data.header.stamp.secs, "nsecs": data.header.stamp.nsecs, "frameId": data.header.frame_id, "statusStatus": data.status.status, "statusService": data.status.service, "latitude": data.latitude, "longitude": data.longitude, "altitude": data.altitude, "positionCovarianceType": data.position_covariance_type, } api_callback( self.loop, self.module["maverick_api.modules.api.mavros.NavSatFixSchema"]. set_nav_sat_fix_message, **kwargs)
def statustext_callback(self, data): if data.name == "/mavros": # application_log.debug("statustext: {0}:{1}".format(data.level, data.msg)) kwargs = { "seq": data.header.seq, "secs": data.header.stamp.secs, "nsecs": data.header.stamp.nsecs, "frameId": data.header.frame_id, "level": data.level, "message": data.msg, } api_callback( self.loop, self.module["maverick_api.modules.api.mavros.StatusTextSchema"] .set_status_text_message, **kwargs, )
def state_callback(self, data): kwargs = { "seq": data.header.seq, "frameId": data.header.frame_id, "guided": data.guided, "nsecs": data.header.stamp.nsecs, "systemStatus": data.system_status, "secs": data.header.stamp.secs, "connected": data.connected, "mode": data.mode, "armed": data.armed, } api_callback( self.loop, self.module["maverick_api.modules.api.mavros.VehicleStateSchema"]. set_state_message, **kwargs, )
def vfr_hud_callback(self, data): kwargs = { "seq": data.header.seq, "secs": data.header.stamp.secs, "nsecs": data.header.stamp.nsecs, "frameId": data.header.frame_id, "airspeed": data.airspeed, "groundspeed": data.groundspeed, "heading": data.heading, "throttle": data.throttle, "altitude": data.altitude, "climb": data.climb, } api_callback( self.loop, self.module["maverick_api.modules.api.mavros.VfrHudSchema"]. set_vfr_hud_message, **kwargs, )
def pose_stamped_callback(self, data): kwargs = { "seq": data.header.seq, "secs": data.header.stamp.secs, "nsecs": data.header.stamp.nsecs, "frameId": data.header.frame_id, "posePositionX": data.pose.position.x, "posePositionY": data.pose.position.y, "posePositionZ": data.pose.position.z, "poseOrientationX": data.pose.orientation.x, "poseOrientationY": data.pose.orientation.y, "poseOrientationZ": data.pose.orientation.z, "poseOrientationW": data.pose.orientation.w, } api_callback( self.loop, self.module["maverick_api.modules.api.mavros.PoseStampedSchema"]. set_pose_stamped_message, **kwargs, )
def imu_callback(self, data): kwargs = { "seq": data.header.seq, "secs": data.header.stamp.secs, "nsecs": data.header.stamp.nsecs, "frameId": data.header.frame_id, "orientationX": data.orientation.x, "orientationY": data.orientation.y, "orientationZ": data.orientation.z, "orientationW": data.orientation.w, "angularVelocityX": data.angular_velocity.x, "angularVelocityY": data.angular_velocity.y, "angularVelocityZ": data.angular_velocity.z, "linearAccelerationX": data.linear_acceleration.x, "linearAccelerationY": data.linear_acceleration.y, "linearAccelerationZ": data.linear_acceleration.z, } api_callback( self.loop, self.module["maverick_api.modules.api.mavros.ImuSchema"]. set_imu_message, **kwargs, )
def run(self): try: vehicle_info = self.get_ros_vehicle_info() application_log.info( f"Obtained info from {len(vehicle_info.vehicles)} vehicles") update_time = int(time.time()) for vehicle in vehicle_info.vehicles: # application_log.debug(f"{self.vehicle_info}") (autopilot_string, type_string, parameter_string) = get_vehicle_strings(vehicle) application_log.debug( f"{autopilot_string} {type_string}\n{vehicle_info}") vehicle_uuid = self.get_vehicle_uuid() data = { "uuid": vehicle_uuid, "update_time": update_time, "info": vehicle, "autopilot_string": autopilot_string, "type_string": type_string, "parameter_string": parameter_string, } api_callback( self.loop, self. module["maverick_api.modules.api.mavros.VehicleInfoSchema"] .update_vehicle_info, data=data, ) self.vehicle_info[data["uuid"]] = data except rospy.ServiceException as ex: application_log.error( f"An error occurred while retrieving vehicle info via ROS: {ex}" )