def mission_callback(self, data):
# application_log.debug(f"mission_callback: {data}")
api_callback(
self.loop,
self.module["modules.api.mavros.MissionSchema"].update_mission,
data=data,
)
def run(self):
try:
self.vehicle_info = self.get_ros_vehicle_info()
application_log.info(
f"Obtained info from {len(self.vehicle_info.vehicles)} vehicles"
)
update_time = int(time.time())
for vehicle in self.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{self.vehicle_info}"
)
api_callback(
self.loop,
self.module[
"modules.api.mavros.VehicleInfoSchema"
].update_vehicle_info,
data={
"update_time": update_time,
"info": vehicle,
"autopilot_string": autopilot_string,
"type_string": type_string,
},
)
# FIXME: get params for this vehicle type and store it somewhere accessable
# FIXME: clean up self.params() before we use it again...
# self.params(parameter_string=parameter_string)
except rospy.ServiceException as ex:
application_log.error(
f"An error occurred while retrieving vehicle info via ROS: {ex}"
)
def rel_alt_callback(self, data):
kwargs = {"relativeAltitude": data.data}
api_callback(
self.loop,
self.module["modules.api.mavros.VfrHudSchema"].set_vfr_hud_message,
**kwargs,
)
def mode_state_callback(self, data):
kwargs = {
"uuid": "test",
"modeString": data.mode,
"updateTime": int(time.time()),
}
api_callback(
self.loop,
self.module["modules.api.mavros.ModeSchema"].update_mode,
**kwargs,
)
def statustext_callback(self, data):
if data.name == "/mavros":
logging.debug("statustext: {0}:{1}".format(data.level, data.msg))
kwargs = {
"seq": data.header.seq,
"secs": data.header.stamp.secs,
"nsecs": data.header.stamp.nsecs,
"frame_id": data.header.frame_id,
"level": data.level,
"message": data.msg,
}
api_callback(self.loop,
self.module[__name__].set_status_text_message,
**kwargs)
def state_callback(self, data):
kwargs = {
"seq": data.header.seq,
"frame_id": data.header.frame_id,
"guided": data.guided,
"nsecs": data.header.stamp.nsecs,
"system_status": data.system_status,
"secs": data.header.stamp.secs,
"connected": data.connected,
"mode": data.mode,
"armed": data.armed,
}
api_callback(self.loop, self.module[__name__].set_state_message,
**kwargs)
def nav_sat_fix_callback(self, data):
kwargs = {
"seq": data.header.seq,
"secs": data.header.stamp.secs,
"nsecs": data.header.stamp.nsecs,
"frame_id": data.header.frame_id,
"status_status": data.status.status,
"status_service": data.status.service,
"latitude": data.latitude,
"longitude": data.longitude,
"altitude": data.altitude,
"position_covariance_type": data.position_covariance_type,
}
api_callback(self.loop, self.module[__name__].set_nav_sat_fix_message,
**kwargs)
def vfr_hud_callback(self, data):
kwargs = {
"seq": data.header.seq,
"secs": data.header.stamp.secs,
"nsecs": data.header.stamp.nsecs,
"frame_id": 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[__name__].set_vfr_hud_message,
**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["modules.api.mavros.NavSatFixSchema"].
set_nav_sat_fix_message, **kwargs)
def pose_stamped_callback(self, data):
kwargs = {
"seq": data.header.seq,
"secs": data.header.stamp.secs,
"nsecs": data.header.stamp.nsecs,
"frame_id": data.header.frame_id,
"pose_position_x": data.pose.position.x,
"pose_position_y": data.pose.position.y,
"pose_position_z": data.pose.position.z,
"pose_orientation_x": data.pose.orientation.x,
"pose_orientation_y": data.pose.orientation.y,
"pose_orientation_z": data.pose.orientation.z,
"pose_orientation_w": data.pose.orientation.w,
}
api_callback(self.loop, self.module[__name__].set_pose_stamped_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["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["modules.api.mavros.VehicleStateSchema"].
set_state_message,
**kwargs,
)
def imu_callback(self, data):
kwargs = {
"seq": data.header.seq,
"secs": data.header.stamp.secs,
"nsecs": data.header.stamp.nsecs,
"frame_id": data.header.frame_id,
"orientation_x": data.orientation.x,
"orientation_y": data.orientation.y,
"orientation_z": data.orientation.z,
"orientation_w": data.orientation.w,
"angular_velocity_x": data.angular_velocity.x,
"angular_velocity_y": data.angular_velocity.y,
"angular_velocity_z": data.angular_velocity.z,
"linear_acceleration_x": data.linear_acceleration.x,
"linear_acceleration_y": data.linear_acceleration.y,
"linear_acceleration_z": data.linear_acceleration.z,
}
api_callback(self.loop, self.module[__name__].set_imu_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["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["modules.api.mavros.ImuSchema"].set_imu_message,
**kwargs,
)
