mirror of
https://github.com/CopterExpress/clever-show.git
synced 2026-06-05 11:39:33 +00:00
drone: move watchdog from modules to the root dir
This commit is contained in:
Arthur Golubtsov
@@ -1,286 +0,0 @@
|
||||
import rospy
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import math
|
||||
import logging
|
||||
import threading
|
||||
|
||||
# for backward compatibility with clever
|
||||
try:
|
||||
from clever import SetAttitude
|
||||
except ImportError:
|
||||
from clover import SetAttitude
|
||||
|
||||
from sensor_msgs.msg import Range
|
||||
from mavros_msgs.msg import State, PositionTarget
|
||||
from mavros_msgs.srv import SetMode, CommandBool
|
||||
from std_msgs.msg import Bool
|
||||
from std_srvs.srv import Trigger, TriggerResponse
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
|
||||
import inspect # Add parent dir to PATH to import messaging_lib
|
||||
current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
|
||||
parent_dir = os.path.dirname(current_dir)
|
||||
sys.path.insert(0, parent_dir)
|
||||
|
||||
from config import ConfigManager
|
||||
|
||||
config = ConfigManager()
|
||||
config.load_config_and_spec("config/client.ini")
|
||||
|
||||
watchdog_is_enabled = config.position_watchdog_enabled
|
||||
log_state = config.position_watchdog_log_state
|
||||
vision_pose_delay_after_arm = config.position_watchdog_vision_pose_delay_after_arm
|
||||
visual_pose_timeout = config.position_watchdog_vision_pose_timeout
|
||||
pos_delta_max = config.position_watchdog_position_delta_max
|
||||
watchdog_action = config.position_watchdog_action
|
||||
timeout_to_disarm = config.position_watchdog_disarm_timeout
|
||||
emergency_land_thrust = config.emergency_land_thrust
|
||||
emergency_land_decrease_thrust_after = config.emergency_land_decrease_thrust_after
|
||||
|
||||
logging.basicConfig( # TODO all prints as logs
|
||||
level=logging.DEBUG, # INFO
|
||||
stream=sys.stdout,
|
||||
format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s",
|
||||
handlers=[
|
||||
logging.StreamHandler(sys.stdout),
|
||||
])
|
||||
|
||||
handler = logging.StreamHandler(sys.stdout)
|
||||
handler.setLevel(logging.DEBUG)
|
||||
formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s] %(message)s")
|
||||
handler.setFormatter(formatter)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger.setLevel(logging.DEBUG)
|
||||
logger.addHandler(handler)
|
||||
|
||||
set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
|
||||
arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
|
||||
set_attitude = rospy.ServiceProxy('/set_attitude', SetAttitude)
|
||||
|
||||
visual_pose_last_timestamp = 0
|
||||
armed = False
|
||||
mode = ''
|
||||
laser_range = 10
|
||||
emergency = False
|
||||
|
||||
local_pose = None
|
||||
setpoint_raw = None
|
||||
setpoint_position = None
|
||||
setpoint_pose = None
|
||||
|
||||
arm_start_time = None
|
||||
offboard_start_time = None
|
||||
offboard_disarmed_timeout = 3.
|
||||
|
||||
emergency_land_called = False
|
||||
|
||||
rospy.init_node('visual_pose_watchdog')
|
||||
logger.info('visual_pose_watchdog inited')
|
||||
logger.info('visual_pose_timeout = {} | position_delta_max = {} | watchdog_action = {}'.format(visual_pose_timeout, pos_delta_max, watchdog_action))
|
||||
logger.info('timeout_to_disarm = {}'.format(timeout_to_disarm))
|
||||
if watchdog_action == 'emergency_land':
|
||||
logger.info('emergency_land_thrust: {}'.format(emergency_land_thrust))
|
||||
|
||||
rate = rospy.Rate(10)
|
||||
|
||||
def get_distance(x1, y1, z1, x2, y2, z2):
|
||||
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
|
||||
|
||||
def get_pos_delta(PoseStamped1, PoseStamped2):
|
||||
if PoseStamped1 is None or PoseStamped2 is None:
|
||||
return float('nan')
|
||||
pos1 = PoseStamped1.pose.position
|
||||
pos2 = PoseStamped2.pose.position
|
||||
return get_distance(pos1.x, pos1.y, pos1.z, pos2.x, pos2.y, pos2.z)
|
||||
|
||||
def get_time_delta(PoseStamped1, PoseStamped2):
|
||||
if PoseStamped1 is None or PoseStamped2 is None:
|
||||
return float('nan')
|
||||
time1 = PoseStamped1.header.stamp.to_sec()
|
||||
time2 = PoseStamped2.header.stamp.to_sec()
|
||||
return time1 - time2
|
||||
|
||||
def visual_pose_callback(data):
|
||||
global visual_pose_last_timestamp
|
||||
visual_pose_last_timestamp = data.header.stamp.to_sec()
|
||||
|
||||
def local_pose_callback(data):
|
||||
global local_pose
|
||||
local_pose = data
|
||||
|
||||
def setpoint_raw_callback(data):
|
||||
global setpoint_raw, setpoint_position, setpoint_pose
|
||||
setpoint_raw_pose = PoseStamped()
|
||||
setpoint_raw_pose.header = data.header
|
||||
setpoint_raw_pose.pose.position = data.position
|
||||
setpoint_raw = setpoint_raw_pose
|
||||
setpoint_pose = get_current_setpoint_pose(setpoint_raw, setpoint_position)
|
||||
|
||||
def setpoint_position_callback(data):
|
||||
global setpoint_raw, setpoint_position, setpoint_pose
|
||||
setpoint_position = data
|
||||
setpoint_pose = get_current_setpoint_pose(setpoint_raw, setpoint_position)
|
||||
|
||||
def get_current_setpoint_pose(_setpoint_raw, _setpoint_position):
|
||||
if _setpoint_position is None and _setpoint_raw is None:
|
||||
return None
|
||||
elif _setpoint_position is not None and _setpoint_raw is None:
|
||||
return _setpoint_position
|
||||
elif _setpoint_raw is not None and _setpoint_position is None:
|
||||
return _setpoint_raw
|
||||
else:
|
||||
return _setpoint_raw if _setpoint_raw.header.stamp > _setpoint_position.header.stamp else _setpoint_position
|
||||
|
||||
def state_callback(data):
|
||||
global armed, mode
|
||||
armed = data.armed
|
||||
mode = data.mode
|
||||
|
||||
def laser_callback(data):
|
||||
global laser_range
|
||||
laser_range = data.range
|
||||
|
||||
def emergency_land(disarm_if_timeout = True):
|
||||
global emergency_land_thrust, laser_range
|
||||
current_thrust = emergency_land_thrust
|
||||
action_timestamp = time.time()
|
||||
while armed:
|
||||
logger.debug("Emergency land | range: {:.2f} | thrust: {:.2f}".format(laser_range, current_thrust))
|
||||
if current_thrust >= 0.03:
|
||||
try:
|
||||
set_attitude(thrust = current_thrust, yaw = 0, frame_id = 'body', auto_arm = True)
|
||||
except rospy.ServiceException as e:
|
||||
logger.info(e)
|
||||
delta = time.time() - action_timestamp
|
||||
if delta > timeout_to_disarm and disarm_if_timeout:
|
||||
try:
|
||||
arming(False)
|
||||
except rospy.ServiceException as e:
|
||||
logger.info(e)
|
||||
if (laser_range < 0.1 or delta > emergency_land_decrease_thrust_after) and current_thrust >= 0.:
|
||||
current_thrust -= 0.02
|
||||
if current_thrust <= 0.03:
|
||||
current_thrust = 0
|
||||
try:
|
||||
arming(False)
|
||||
except rospy.ServiceException as e:
|
||||
logger.info(e)
|
||||
rate.sleep()
|
||||
|
||||
def emergency_land_service(request):
|
||||
global emergency_land_called, armed
|
||||
responce = TriggerResponse()
|
||||
if armed:
|
||||
responce.success = True
|
||||
responce.message = "Start emergency landing"
|
||||
emergency_land_called = True
|
||||
else:
|
||||
responce.success = False
|
||||
responce.message = "Copter is disarmed, no need for emergency landing!"
|
||||
emergency_land_called = False
|
||||
return responce
|
||||
|
||||
def watchdog_callback(event):
|
||||
global visual_pose_last_timestamp, armed, mode, watchdog_action, laser_range
|
||||
global emergency, local_pose, setpoint_pose, emergency_land_called, log_state
|
||||
global offboard_start_time, arm_start_time, vision_pose_delay_after_arm
|
||||
pos_delta = get_pos_delta(local_pose, setpoint_pose)
|
||||
pos_dt = get_time_delta(local_pose, setpoint_pose)
|
||||
visual_pose_dt = abs(time.time() - visual_pose_last_timestamp)
|
||||
if log_state:
|
||||
logger.info("armed: {} | mode: {} | vis_dt: {:.2f} | pos_delta: {:.2f} | pos_dt: {:.2f} | range: {:.2f} | watchdog_action: {}".format(
|
||||
armed, mode, visual_pose_dt, pos_delta, pos_dt, laser_range, watchdog_action))
|
||||
if mode == 'OFFBOARD':
|
||||
if offboard_start_time is None:
|
||||
offboard_start_time = time.time()
|
||||
if armed:
|
||||
if arm_start_time is None:
|
||||
arm_start_time = time.time()
|
||||
arm_time = time.time() - arm_start_time
|
||||
logger.debug('arm time: {}'.format(arm_time))
|
||||
if arm_time > vision_pose_delay_after_arm and watchdog_is_enabled:
|
||||
if (visual_pose_dt > visual_pose_timeout and visual_pose_timeout != 0.) or (pos_delta > pos_delta_max and pos_delta_max != 0.):
|
||||
action_timestamp = time.time()
|
||||
emergency = True
|
||||
if watchdog_action not in ['land', 'emergency_land', 'disarm']:
|
||||
watchdog_action = 'land'
|
||||
if watchdog_action == 'land':
|
||||
logger.info('Visual pose data is too old, copter is armed, landing...')
|
||||
while mode != "AUTO.LAND":
|
||||
try:
|
||||
set_mode(custom_mode='AUTO.LAND')
|
||||
except rospy.ServiceException as e:
|
||||
logger.info(e)
|
||||
if time.time() - action_timestamp > timeout_to_disarm:
|
||||
break
|
||||
rate.sleep()
|
||||
else:
|
||||
logger.info('Land mode is set')
|
||||
while armed:
|
||||
if time.time() - action_timestamp > timeout_to_disarm:
|
||||
try:
|
||||
arming(False)
|
||||
except rospy.ServiceException as e:
|
||||
logger.info(e)
|
||||
rate.sleep()
|
||||
elif watchdog_action == 'disarm':
|
||||
logger.info('Visual pose data is too old, copter is armed, disarming...')
|
||||
while armed:
|
||||
try:
|
||||
arming(False)
|
||||
except rospy.ServiceException as e:
|
||||
logger.info(e)
|
||||
rate.sleep()
|
||||
elif watchdog_action == 'emergency_land':
|
||||
if visual_pose_dt > visual_pose_timeout:
|
||||
logger.info('Visual pose data is too old, copter is armed, emergency landing...')
|
||||
if pos_delta > pos_delta_max:
|
||||
logger.info('Position delta is {} m, copter is armed, emergency landing...'.format(pos_delta))
|
||||
emergency_land()
|
||||
logger.info('Disarmed')
|
||||
emergency = False
|
||||
if emergency_land_called:
|
||||
emergency = True
|
||||
logger.info('/emergency_land service was called, start emergency landing...')
|
||||
emergency_land()
|
||||
logger.info('Disarmed')
|
||||
emergency = False
|
||||
emergency_land_called = False
|
||||
else:
|
||||
arm_start_time = None
|
||||
if time.time() - offboard_start_time > offboard_disarmed_timeout:
|
||||
try:
|
||||
set_mode(custom_mode='AUTO.LAND')
|
||||
except rospy.ServiceException as e:
|
||||
logger.info(e)
|
||||
else:
|
||||
offboard_start_time = None
|
||||
if (abs(time.time() - visual_pose_last_timestamp) > visual_pose_timeout and visual_pose_timeout != 0.0):
|
||||
logger.info('Visual pose data is too old')
|
||||
|
||||
rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback)
|
||||
|
||||
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, local_pose_callback)
|
||||
|
||||
rospy.Subscriber('/mavros/setpoint_position/local', PoseStamped, setpoint_position_callback)
|
||||
|
||||
rospy.Subscriber('/mavros/setpoint_raw/local', PositionTarget, setpoint_raw_callback)
|
||||
|
||||
rospy.Subscriber('/mavros/state', State, state_callback)
|
||||
|
||||
rospy.Subscriber('/mavros/distance_sensor/rangefinder', Range, laser_callback)
|
||||
|
||||
emergency_pub = rospy.Publisher('/emergency', Bool, queue_size=10)
|
||||
|
||||
rospy.Service('emergency_land', Trigger, emergency_land_service)
|
||||
|
||||
rospy.Timer(rospy.Duration(0.5), watchdog_callback)
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
emergency_msg = Bool()
|
||||
emergency_msg.data = emergency
|
||||
emergency_pub.publish(emergency_msg)
|
||||
rate.sleep()
|
||||
Reference in New Issue
Block a user