Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add a toggle option to Topic-type commands #66

Open
wants to merge 4 commits into
base: foxy-devel
Choose a base branch
from
Open

Add a toggle option to Topic-type commands #66

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
234f18c
Allow a `value` type within an axis mapping. Useful for frame data.
Mar 8, 2021
6d97ecd
Add a Toggle option to Topic-type commands
Mar 9, 2021
b8d6e10
Add yaml example
Mar 9, 2021
bc8c16d
Remove debug statement
Mar 9, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion joy_teleop/config/joy_teleop_example.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,10 @@ joy_teleop:

walk:
type: topic
interface_type: geometry_msgs/msg/Twist
interface_type: geometry_msgs/msg/TwistStamped
topic_name: cmd_vel
deadman_buttons: [4]
toggle_buttons: [0]
axis_mappings:
linear-x:
axis: 1
Expand All @@ -20,6 +21,8 @@ joy_teleop:
linear-z:
button: 2
scale: 3.0
header-frame_id:
value: 'my_tf_frame'

force_push:
type: topic
Expand Down
67 changes: 52 additions & 15 deletions joy_teleop/joy_teleop/joy_teleop.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,17 +75,22 @@ def set_member(msg: typing.Any, member: str, value: typing.Any) -> None:
class JoyTeleopCommand:

def __init__(self, name: str, config: typing.Dict[str, typing.Any],
button_name: str, axes_name: str) -> None:
button_name: str, axes_name: str, toggle_buttons: str) -> None:
self.buttons: typing.List[str] = []
self.toggle_buttons: typing.List[str] = []
if button_name in config:
self.buttons = config[button_name]
self.axes: typing.List[str] = []
if axes_name in config:
self.axes = config[axes_name]
if toggle_buttons in config:
self.toggle_buttons = config[toggle_buttons]

if len(self.buttons) == 0 and len(self.axes) == 0:
if len(self.buttons) == 0 and len(self.axes) == 0 and len(self.toggle_buttons) == 0:
raise JoyTeleopException("No buttons or axes configured for command '{}'".format(name))

self.prev_joy_state = sensor_msgs.msg.Joy()

# Used to short-circuit the run command if there aren't enough buttons in the message.
self.min_button = 0
if len(self.buttons) > 0:
Expand Down Expand Up @@ -127,12 +132,24 @@ def update_active_from_buttons_and_axes(self, joy_state: sensor_msgs.msg.Joy) ->
class JoyTeleopTopicCommand(JoyTeleopCommand):

def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None:
super().__init__(name, config, 'deadman_buttons', 'deadman_axes')
super().__init__(name, config, 'deadman_buttons', 'deadman_axes', 'toggle_buttons')

self.name = name

self.topic_type = get_interface_type(config['interface_type'], 'msg')

self.toggle_buttons = []
if 'toggle_buttons' in config:
self.toggle_buttons = config['toggle_buttons']
self.toggle_enabled = True
# Need 2 rising or falling edges for a change in toggle state (button pressed and released)
self.toggle_press_count = 0

# For this control mode, self.buttons are deadman_buttons
self.have_deadman = False
if len(self.buttons) > 0:
self.have_deadman = True

# A 'message_value' is a fixed message that is sent in response to an activation. It is
# mutually exclusive with an 'axis_mapping'.
self.msg_value = None
Expand All @@ -154,16 +171,18 @@ def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node)
self.axis_mappings = config['axis_mappings']
# Now check that the mappings have all of the required configuration.
for mapping, values in self.axis_mappings.items():
if 'axis' not in values and 'button' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have an axis or button"
.format(name))
if 'offset' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have an offset"
if 'axis' not in values and 'button' not in values and 'value' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have an axis, button, or value"
.format(name))

if 'scale' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have a scale"
.format(name))
if 'axis' in values:
if 'offset' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have an offset"
.format(name))

if 'scale' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have a scale"
.format(name))

if self.msg_value is None and not self.axis_mappings:
raise JoyTeleopException("No 'message_value' or 'axis_mappings' "
Expand Down Expand Up @@ -192,11 +211,26 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None:

last_active = self.active
self.update_active_from_buttons_and_axes(joy_state)
if not self.active:
return
# This is where the return due to deadman switch happens
if self.have_deadman:
if not self.active:
return
if self.msg_value is not None and last_active == self.active:
return

# Check toggle status for this command
if len(self.prev_joy_state.buttons) > 0:
for toggle_button in self.toggle_buttons:
if joy_state.buttons[int(toggle_button)] != self.prev_joy_state.buttons[toggle_button]:
# Need 2 rising or falling edges for a change in toggle state (button pressed and released)
self.toggle_press_count = self.toggle_press_count + 1
if self.toggle_press_count == 2:
self.toggle_enabled = not self.toggle_enabled
self.toggle_press_count = 0
self.prev_joy_state = joy_state
if not self.toggle_enabled:
return

if self.msg_value is not None:
# This is the case for a static message.
msg = self.msg_value
Expand All @@ -223,6 +257,9 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None:
'but #{} was referenced in config.'.format(
len(joy_state.buttons), values['button']))
val = 0.0
elif 'value' in values:
# Pass on the value as its Python-implicit type
val = values.get('value')
else:
node.get_logger().error(
'No Supported axis_mappings type found in: {}'.format(mapping))
Expand All @@ -240,7 +277,7 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None:
class JoyTeleopServiceCommand(JoyTeleopCommand):

def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None:
super().__init__(name, config, 'buttons', 'axes')
super().__init__(name, config, 'buttons', 'axes', 'toggle_buttons')

self.name = name

Expand Down Expand Up @@ -289,7 +326,7 @@ def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None:
class JoyTeleopActionCommand(JoyTeleopCommand):

def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None:
super().__init__(name, config, 'buttons', 'axes')
super().__init__(name, config, 'buttons', 'axes', 'toggle_buttons')

self.name = name

Expand Down