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}"
)