def __init__(self, state=5):
self.is_killswitch_on = False
self.pub = rospy.Publisher('height_control', HControl, queue_size=100)
self.state = state
self.h_control = HControl()
self.h_control.state = self.state
self.h_control.depth = 0
self.h_control.power = 0
def __init__(self, waypoint=None):
self.pub_h_nav = rospy.Publisher(
'height_control', HControl, queue_size=100)
self.pub_r_nav = rospy.Publisher(
'rotation_control', RControl, queue_size=100)
self.pub_m_nav = rospy.Publisher(
'movement_control', MControl, queue_size=100)
rospy.Subscriber('rotation_control_status', RControl,
self.r_status_callback, queue_size=100)
rospy.Subscriber('movement_control_status', MControl,
self.m_status_callback, queue_size=100)
rospy.Subscriber('height_control_status', HControl,
self.h_status_callback, queue_size=100)
self.h_control = HControl()
self.r_control = RControl()
self.m_control = MControl()
# used for HControl (int state, float depth, int power) #######################################
self.hStates = {
'down': 0,
'staying': 1,
'up': 2,
'unlock': 4,
'lock': 5
}
# used for RControl (int state, float rotation, int power) ####################################
self.rStates = {
'left': 0, # rotate left
'staying': 1,
'right': 2, # rotate right
'rotate_front_cam_dist': 3, # rotate with fcd
'keep_rotate_front_cam_dist': 4 # keeping rotating with fcd
}
# used for MControl (int state, int mDirection, float power, float distance) #######
self.mStates = {
'off': 0,
'power': 1, # adjust with power
'distance': 2, # ajust with distance
'front_cam_center': 3, # centered with front camera
'bot_cam_center': 4, # centered with bottom camera
'motor_time': 5 # turn on motor with specific time
}
self.directions = {
'none': 0,
'forward': 1,
'right': 2,
'backward': 3,
'left': 4
}
# waypoint variables
if waypoint:
self.waypoint = waypoint
else:
self.waypoint = Waypoint()
self.is_running_waypoint_rotation = False
self.is_running_waypoint_movement = False
self.is_busy_waypoint = False
self.w_distance_m = 0
self.w_power_m = 140
self.r_power = 85
self.h_power = 100
self.m_power = 140
self.waypoint_state = 0
self.thread_w = None
self.exit_waypoints = False
# vars dealing with movement break time
self.waypoint_m_time = 0
# self.waypoint_m_time_max = 2.14
self.waypoint_m_time_max = 1.0
# vars dealing with height checking
self.depth_threshold = 0.3
self.depth_assignment = 0.5
self.current_waypoint_x = 0
self.current_waypoint_y = 0
self.depth_cap = 3.5
# var for saved heading
self.saved_heading = None
self.saved_heading_path1 = None
def __init__(self, wp=None):
self.is_killswitch_on = False
self.pub_h_nav = rospy.Publisher('height_control',
HControl,
queue_size=100)
self.pub_r_nav = rospy.Publisher('rotation_control',
RControl,
queue_size=100)
self.pub_m_nav = rospy.Publisher('movement_control',
MControl,
queue_size=100)
# rospy.init_node('navigation_node', anonymous=True)
rospy.Subscriber('rotation_control_status',
RControl,
self.r_status_callback,
queue_size=100)
rospy.Subscriber('movement_control_status',
MControl,
self.m_status_callback,
queue_size=100)
rospy.Subscriber('height_control_status',
HControl,
self.h_status_callback,
queue_size=100)
self.h_control = HControl()
self.r_control = RControl()
self.m_control = MControl()
# used for HControl (int state, float depth, int power) #######################################
self.hStates = {
'down': 0,
'staying': 1,
'up': 2,
'unlock': 4,
'lock': 5
}
self.hState = None # state
self.depth = None # depth (nonstop moving: -1, moving distance: x)
self.hPower = 100 # power
# used for RControl (int state, float rotation, int power) ####################################
self.rStates = {
'left': 0, # rotate left
'staying': 1,
'right': 2, # rotate right
'rotate_front_cam_dist': 3, # rotate with fcd
'keep_rotate_front_cam_dist': 4 # keeping rotating with fcd
}
self.rState = None # state
self.rotation = None # rotation (nonstop rotating: -1, rotate degree: x)
self.rPower = 90 # power
# used for MControl (int state, int mDirection, float power, float distance) #######
self.mStates = {
'off': 0,
'power': 1, # adjust with power
'distance': 2, # ajust with distance
'front_cam_center': 3, # centered with front camera
'bot_cam_center': 4, # centered with bottom camera
'motor_time': 5 # turn on motor with specific time
}
self.mState = None # state
self.directions = {
'none': 0,
'forward': 1,
'right': 2,
'backward': 3,
'left': 4
}
self.mDirection = None # mDirection
self.mPower = None # power (none: 0, motor power: x)
self.distance = None # distance (distance away from the object: x)
self.runningTime = None # runningTime (time for the motor to turn on)
#waypoint variables
if wp:
self.waypoint = wp
else:
self.waypoint = Waypoint()
self.is_running_waypoint_rotation = False
self.is_running_waypoint_movement = False
self.is_busy_waypoint = False
self.w_distance_m = 0
self.w_power_m = 140
self.r_power = 90
self.h_power = 100
self.m_power = 140
self.waypoint_state = 0
self.thread_w = None
self.exit_waypoints = False
#vars dealing with movement break time
self.waypoint_m_time = 0
# self.waypoint_m_time_max = 2.14
self.waypoint_m_time_max = 1.0
#vars dealing with height checking
self.depth_threshold = 0.3
self.depth_assignment = 0.5
self.current_waypoint_x = 0
self.current_waypoint_y = 0
self.depth_cap = 3.5
#var for saved heading
self.saved_heading = 0