def __init__(self, joy_file, id=1):
self.id = id
self.ns = rospy.get_namespace()
self.state_url = 'mavros/state'
self.arming_url = 'mavros/cmd/arming'
self.rc_url = 'mavros/rc/override'
self.mavros_state = State()
self.joy_is_connected = False
self.joy_handle = JoyHandle(joy_file)
self.rc_simulation = RCSimulation(self.rc_url)
self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_callback)
self.sub_state = rospy.Subscriber(self.state_url, State,
self.state_callback)
rospy.wait_for_service(self.arming_url) # TODO(franreal): wait here?
self.arming_proxy = rospy.ServiceProxy(self.arming_url, CommandBool)
rospy.Timer(rospy.Duration(1), self.arming_callback) # 1Hz
def __init__(self, joy_file):
self.joy_handle = JoyHandle(joy_file)
take_off_url = 'ual/take_off'
land_url = 'ual/land'
velocity_url = 'ual/set_velocity'
rospy.wait_for_service(take_off_url)
rospy.wait_for_service(land_url)
self.take_off = rospy.ServiceProxy(take_off_url, TakeOff)
self.land = rospy.ServiceProxy(land_url, Land)
self.velocity_pub = rospy.Publisher(velocity_url,
TwistStamped,
queue_size=1)
self.ual_state = State()
self.headless = True
self.uav_yaw = 0.0
self.gains_table = [0.5, 0.8, 1.0, 1.3, 1.8, 2.1, 2.5]
self.gain_index = 2
def __init__(self, joy_name):
action_file = rospkg.RosPack().get_path('ual_teleop') + '/config/joy_teleop.yaml'
with open(action_file, 'r') as action_config:
action_map = yaml.load(action_config)['joy_actions']
self.joy_handle = JoyHandle(joy_name, action_map)
take_off_url = 'ual/take_off'
land_url = 'ual/land'
velocity_url = 'ual/set_velocity'
rospy.wait_for_service(take_off_url)
rospy.wait_for_service(land_url)
self.take_off = rospy.ServiceProxy(take_off_url, TakeOff)
self.land = rospy.ServiceProxy(land_url, Land)
self.velocity_pub = rospy.Publisher(velocity_url, TwistStamped, queue_size=1)
self.ual_state = State()
self.headless = False
self.uav_yaw = 0.0
self.gains_table = [0.5, 0.8, 1.0, 1.3, 1.8, 2.1, 2.5]
self.gain_index = 2
def __init__(self, joy_name, id=1):
self.id = id
self.ns = rospy.get_namespace()
self.state_url = 'mavros/state'
self.arming_url = 'mavros/cmd/arming'
self.rc_url = 'mavros/rc/override'
self.mavros_state = MavrosState()
self.joy_is_connected = False
action_file = rospkg.RosPack().get_path(
'ual_teleop') + '/config/simulate_safety_pilot.yaml'
with open(action_file, 'r') as action_config:
action_map = yaml.load(action_config)['joy_actions']
self.joy_handle = JoyHandle(joy_name, action_map)
self.rc_simulation = RCSimulation(self.rc_url)
self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_callback)
self.sub_state = rospy.Subscriber(self.state_url, MavrosState,
self.state_callback)
rospy.wait_for_service(self.arming_url) # TODO(franreal): wait here?
self.arming_proxy = rospy.ServiceProxy(self.arming_url, CommandBool)
rospy.Timer(rospy.Duration(1), self.arming_callback) # 1Hz
class JoyTeleop:
def __init__(self, joy_file):
self.joy_handle = JoyHandle(joy_file)
take_off_url = 'ual/take_off'
land_url = 'ual/land'
velocity_url = 'ual/set_velocity'
rospy.wait_for_service(take_off_url)
rospy.wait_for_service(land_url)
self.take_off = rospy.ServiceProxy(take_off_url, TakeOff)
self.land = rospy.ServiceProxy(land_url, Land)
self.velocity_pub = rospy.Publisher(velocity_url,
TwistStamped,
queue_size=1)
self.ual_state = State()
self.headless = True
self.uav_yaw = 0.0
self.gains_table = [0.5, 0.8, 1.0, 1.3, 1.8, 2.1, 2.5]
self.gain_index = 2
def state_callback(self, data):
self.ual_state = data
def pose_callback(self, data):
self.uav_yaw = 2.0 * math.atan2(data.pose.orientation.z,
data.pose.orientation.w)
def joy_callback(self, data):
self.joy_handle.update(data)
# print self.joy_handle # DEBUG
if self.joy_handle.get_button('left_shoulder'):
if self.joy_handle.get_button_state(
'x'
) is ButtonState.JUST_PRESSED and self.ual_state.state == State.LANDED_ARMED:
rospy.loginfo("Taking off")
self.take_off(2.0, False) # TODO(franreal): takeoff height?
if self.joy_handle.get_button_state(
'b'
) is ButtonState.JUST_PRESSED and self.ual_state.state == State.FLYING_AUTO:
rospy.loginfo("Landing")
self.land(False)
if self.headless == True and (
self.joy_handle.get_button_state('right_shoulder') is
ButtonState.JUST_PRESSED):
rospy.loginfo("Exiting headless mode")
self.headless = False
elif self.headless == False and (
self.joy_handle.get_button_state('right_shoulder') is
ButtonState.JUST_PRESSED):
rospy.loginfo("Entering headless mode")
self.headless = True
if self.joy_handle.get_button_state(
'left_trigger') is ButtonState.JUST_PRESSED:
self.gain_index = self.gain_index - 1 if self.gain_index > 0 else 0
rospy.loginfo("Speed level: %d", self.gain_index)
if self.joy_handle.get_button_state(
'right_trigger') is ButtonState.JUST_PRESSED:
max_index = len(self.gains_table) - 1
self.gain_index = self.gain_index + 1 if self.gain_index < max_index else max_index
rospy.loginfo("Speed level: %d", self.gain_index)
if self.ual_state.state == State.FLYING_AUTO:
vel_cmd = TwistStamped()
vel_cmd.header.stamp = rospy.Time.now()
# TODO: Use frame_id = 'uav_1' in not-headless mode?
vel_cmd.header.frame_id = 'map'
if self.headless:
vel_cmd.twist.linear.x = 0.5 * self.gains_table[
self.gain_index] * self.joy_handle.get_axis(
'right_analog_x')
vel_cmd.twist.linear.y = 0.5 * self.gains_table[
self.gain_index] * self.joy_handle.get_axis(
'right_analog_y')
vel_cmd.twist.linear.z = 0.2 * self.gains_table[
self.gain_index] * self.joy_handle.get_axis(
'left_analog_y')
vel_cmd.twist.angular.z = -self.joy_handle.get_axis(
'left_analog_x')
else:
x = 0.5 * self.gains_table[
self.gain_index] * self.joy_handle.get_axis(
'right_analog_y')
y = -0.5 * self.gains_table[
self.gain_index] * self.joy_handle.get_axis(
'right_analog_x')
vel_cmd.twist.linear.x = (x * math.cos(self.uav_yaw) -
y * math.sin(self.uav_yaw))
vel_cmd.twist.linear.y = (x * math.sin(self.uav_yaw) +
y * math.cos(self.uav_yaw))
vel_cmd.twist.linear.z = 0.2 * self.gains_table[
self.gain_index] * self.joy_handle.get_axis(
'left_analog_y')
vel_cmd.twist.angular.z = -self.joy_handle.get_axis(
'left_analog_x')
self.velocity_pub.publish(vel_cmd)
class SafetyPilot:
def __init__(self, joy_name, id=1):
self.id = id
self.ns = rospy.get_namespace()
self.state_url = 'mavros/state'
self.arming_url = 'mavros/cmd/arming'
self.rc_url = 'mavros/rc/override'
self.mavros_state = MavrosState()
self.joy_is_connected = False
action_file = rospkg.RosPack().get_path(
'ual_teleop') + '/config/simulate_safety_pilot.yaml'
with open(action_file, 'r') as action_config:
action_map = yaml.load(action_config)['joy_actions']
self.joy_handle = JoyHandle(joy_name, action_map)
self.rc_simulation = RCSimulation(self.rc_url)
self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_callback)
self.sub_state = rospy.Subscriber(self.state_url, MavrosState,
self.state_callback)
rospy.wait_for_service(self.arming_url) # TODO(franreal): wait here?
self.arming_proxy = rospy.ServiceProxy(self.arming_url, CommandBool)
rospy.Timer(rospy.Duration(1), self.arming_callback) # 1Hz
def arming_callback(self, event):
if self.joy_is_connected: # Disable auto-arming
return
if not self.mavros_state.armed and self.mavros_state.connected:
try:
rospy.loginfo(
"Safety pilot simulator for robot id [%d]: arming [%s]",
self.id, self.ns + self.arming_url)
self.arming_proxy(True)
except rospy.ServiceException as exc:
rospy.logerr("Service did not process request: %s", str(exc))
def state_callback(self, data):
self.mavros_state = data
def joy_callback(self, data):
if not self.joy_is_connected:
rospy.loginfo(
"Joystick detected, RC simulation is now enabled (and auto-arming disabled)"
)
self.joy_is_connected = True
self.joy_handle.update(data)
# print self.joy_handle # DEBUG
self.rc_simulation.set_channel(
1, 1500 + 600 * self.joy_handle.get_action_axis('throttle'))
self.rc_simulation.set_channel(
2, 1500 + 600 * self.joy_handle.get_action_axis('yaw'))
self.rc_simulation.set_channel(
3, 1500 + 600 * self.joy_handle.get_action_axis('pitch'))
self.rc_simulation.set_channel(
4, 1500 + 600 * self.joy_handle.get_action_axis('roll'))
if self.joy_handle.get_action_button('secure'):
fltmode_pwm = self.rc_simulation.get_channel(5)
if (self.joy_handle.get_action_button('set_mode_pwm_2100')):
fltmode_pwm = 2100
if (self.joy_handle.get_action_button('set_mode_pwm_1500')):
fltmode_pwm = 1500
if (self.joy_handle.get_action_button('set_mode_pwm_900')
): # This button has maximum priority (stabilized)
fltmode_pwm = 900
self.rc_simulation.set_channel(5, fltmode_pwm) # fltmode
class SafetyPilot:
def __init__(self, joy_file, id=1):
self.id = id
self.ns = rospy.get_namespace()
self.state_url = 'mavros/state'
self.arming_url = 'mavros/cmd/arming'
self.rc_url = 'mavros/rc/override'
self.mavros_state = State()
self.joy_is_connected = False
self.joy_handle = JoyHandle(joy_file)
self.rc_simulation = RCSimulation(self.rc_url)
self.sub_joy = rospy.Subscriber('/joy', Joy, self.joy_callback)
self.sub_state = rospy.Subscriber(self.state_url, State,
self.state_callback)
rospy.wait_for_service(self.arming_url) # TODO(franreal): wait here?
self.arming_proxy = rospy.ServiceProxy(self.arming_url, CommandBool)
rospy.Timer(rospy.Duration(1), self.arming_callback) # 1Hz
def arming_callback(self, event):
if self.joy_is_connected: # Disable auto-arming
return
if not self.mavros_state.armed and self.mavros_state.connected:
try:
rospy.loginfo(
"Safety pilot simulator for robot id [%d]: arming [%s]",
self.id, self.ns + self.arming_url)
self.arming_proxy(True)
except rospy.ServiceException as exc:
rospy.logerr("Service did not process request: %s", str(exc))
def state_callback(self, data):
self.mavros_state = data
def joy_callback(self, data):
if not self.joy_is_connected:
rospy.loginfo(
"Joystick detected, RC simulation is now enabled (and auto-arming disabled)"
)
self.joy_is_connected = True
self.joy_handle.update(data)
# print self.joy_handle # DEBUG
self.rc_simulation.set_channel(
1,
1500 + 600 * self.joy_handle.get_axis('left_analog_y')) # throttle
self.rc_simulation.set_channel(
2, 1500 + 600 * self.joy_handle.get_axis('left_analog_x')) # yaw
self.rc_simulation.set_channel(
3,
1500 + 600 * self.joy_handle.get_axis('right_analog_y')) # pitch
self.rc_simulation.set_channel(
4, 1500 + 600 * self.joy_handle.get_axis('right_analog_x')) # roll
if self.joy_handle.get_button('left_shoulder'):
fltmode_pwm = self.rc_simulation.get_channel(5)
if (self.joy_handle.get_button('x')):
fltmode_pwm = 2100
if (self.joy_handle.get_button('y')):
fltmode_pwm = 1500
if (self.joy_handle.get_button('b')
): # This button has maximum priority (stabilized)
fltmode_pwm = 900
self.rc_simulation.set_channel(5, fltmode_pwm) # fltmode
class JoyTeleop:
def __init__(self, joy_name):
action_file = rospkg.RosPack().get_path('ual_teleop') + '/config/joy_teleop.yaml'
with open(action_file, 'r') as action_config:
action_map = yaml.load(action_config)['joy_actions']
self.joy_handle = JoyHandle(joy_name, action_map)
take_off_url = 'ual/take_off'
land_url = 'ual/land'
velocity_url = 'ual/set_velocity'
rospy.wait_for_service(take_off_url)
rospy.wait_for_service(land_url)
self.take_off = rospy.ServiceProxy(take_off_url, TakeOff)
self.land = rospy.ServiceProxy(land_url, Land)
self.velocity_pub = rospy.Publisher(velocity_url, TwistStamped, queue_size=1)
self.ual_state = State()
self.headless = False
self.uav_yaw = 0.0
self.gains_table = [0.5, 0.8, 1.0, 1.3, 1.8, 2.1, 2.5]
self.gain_index = 2
def state_callback(self, data):
self.ual_state = data
def pose_callback(self, data):
self.uav_yaw = 2.0 * math.atan2(data.pose.orientation.z, data.pose.orientation.w)
def joy_callback(self, data):
self.joy_handle.update(data)
# print self.joy_handle # DEBUG
if self.joy_handle.get_action_button('secure'):
if self.joy_handle.get_action_button_state('take_off') is ButtonState.JUST_PRESSED and self.ual_state.state == State.LANDED_ARMED:
rospy.loginfo("Taking off")
self.take_off(2.0, False) # TODO(franreal): takeoff height?
if self.joy_handle.get_action_button_state('land') is ButtonState.JUST_PRESSED and self.ual_state.state == State.FLYING_AUTO:
rospy.loginfo("Landing")
self.land(False)
if self.headless == True and (self.joy_handle.get_action_button_state('toggle_headless') is ButtonState.JUST_PRESSED):
rospy.loginfo("Exiting headless mode")
self.headless = False
elif self.headless == False and (self.joy_handle.get_action_button_state('toggle_headless') is ButtonState.JUST_PRESSED):
rospy.loginfo("Entering headless mode")
self.headless = True
if self.joy_handle.get_action_button_state('speed_down') is ButtonState.JUST_PRESSED:
self.gain_index = self.gain_index - 1 if self.gain_index > 0 else 0
rospy.loginfo("Speed level: %d", self.gain_index)
if self.joy_handle.get_action_button_state('speed_up') is ButtonState.JUST_PRESSED:
max_index = len(self.gains_table) - 1
self.gain_index = self.gain_index + 1 if self.gain_index < max_index else max_index
rospy.loginfo("Speed level: %d", self.gain_index)
if self.ual_state.state == State.FLYING_AUTO:
vel_cmd = TwistStamped()
vel_cmd.header.stamp = rospy.Time.now()
# TODO: Use frame_id = 'uav_1' in not-headless mode?
vel_cmd.header.frame_id = 'map'
if self.headless:
vel_cmd.twist.linear.x = self.gains_table[self.gain_index] * self.joy_handle.get_action_axis('move_forward')
vel_cmd.twist.linear.y = self.gains_table[self.gain_index] * self.joy_handle.get_action_axis('move_right')
vel_cmd.twist.linear.z = self.gains_table[self.gain_index] * self.joy_handle.get_action_axis('move_up')
vel_cmd.twist.angular.z = self.joy_handle.get_action_axis('move_yaw')
else:
x = self.gains_table[self.gain_index] * self.joy_handle.get_action_axis('move_forward')
y = self.gains_table[self.gain_index] * self.joy_handle.get_action_axis('move_right')
vel_cmd.twist.linear.x = (x*math.cos(self.uav_yaw) - y*math.sin(self.uav_yaw))
vel_cmd.twist.linear.y = (x*math.sin(self.uav_yaw) + y*math.cos(self.uav_yaw))
vel_cmd.twist.linear.z = self.gains_table[self.gain_index] * self.joy_handle.get_action_axis('move_up')
vel_cmd.twist.angular.z = self.joy_handle.get_action_axis('move_yaw')
self.velocity_pub.publish(vel_cmd)