Skip to content

Commit

Permalink
Joystick relay for ROS 2 (#43)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Noel215
Noel215 authored Sep 18, 2023
1 parent c50e8bd commit a3a6547
Show file tree
Hide file tree
Showing 5 changed files with 364 additions and 55 deletions.
13 changes: 6 additions & 7 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,11 +47,10 @@ install(
RUNTIME DESTINATION lib/${PROJECT_NAME}
)

# Pending to test ROS2 migration
#install(
# PROGRAMS scripts/joystick_relay.py
# DESTINATION lib/${PROJECT_NAME}
#)
install(
PROGRAMS scripts/joystick_relay.py
DESTINATION lib/${PROJECT_NAME}
)

foreach(dir launch config)
install(DIRECTORY ${dir}/
Expand All @@ -64,8 +63,8 @@ install(
)

if(BUILD_TESTING)
# find_package(launch_testing_ament_cmake)
# add_launch_test(test/system.test.py)
find_package(launch_testing_ament_cmake)
add_launch_test(test/test_joystick_relay.py)
endif()

ament_export_include_directories(include)
Expand Down
15 changes: 10 additions & 5 deletions launch/twist_mux_launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,10 +55,8 @@ def generate_launch_description():
remappings={('/cmd_vel_out', LaunchConfiguration('cmd_vel_out'))},
parameters=[
LaunchConfiguration('config_locks'),
LaunchConfiguration('config_topics'),
LaunchConfiguration('config_joy')]
LaunchConfiguration('config_topics')]
),

Node(
package='twist_mux',
executable='twist_marker',
Expand All @@ -67,5 +65,12 @@ def generate_launch_description():
parameters=[{
'frame_id': 'base_link',
'scale': 1.0,
'vertical_position': 2.0}])
])
'vertical_position': 2.0}]),
Node(
package='twist_mux',
executable='joystick_relay.py',
output='screen',
remappings={('joy_vel_in', 'input_joy/cmd_vel'),
('joy_vel_out', 'joy_vel')},
parameters=[LaunchConfiguration('config_joy')])
])
2 changes: 2 additions & 0 deletions package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,8 @@
<depend>visualization_msgs</depend>
<depend>diagnostic_updater</depend>

<exec_depend>twist_mux_msgs</exec_depend>

<test_depend>ament_lint_auto</test_depend>

<test_depend>ament_cmake_xmllint</test_depend>
Expand Down
98 changes: 55 additions & 43 deletions scripts/joystick_relay.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@

from geometry_msgs.msg import Twist
from rclpy.action import ActionServer
from rclpy.executors import ExternalShutdownException
from rclpy.qos import QoSProfile, QoSDurabilityPolicy
from rclpy.node import Node
from std_msgs.msg import Bool
from twist_mux_msgs.action import JoyPriority, JoyTurbo
Expand Down Expand Up @@ -75,48 +77,48 @@ def __init__(self, node, action_name, action_type, callback):
def _cb(self, goal):
self._callback()
goal.succeed()
return self._action_type().Result()
return self._action_type.Result()


class VelocityControl:

def __init__(self, node):
self._node = node
self._num_steps = self._node.declare_parameter('turbo/steps', 1)
self._num_steps = self._node.declare_parameter('turbo.steps', 1)

forward_min = self._node.declare_parameter(
'turbo/linear_forward_min', 1.0)
'turbo.linear_forward_min', 1.0)
forward_max = self._node.declare_parameter(
'turbo/linear_forward_max', 1.0)
self._forward = Velocity(forward_min, forward_max, self._num_steps)
'turbo.linear_forward_max', 1.0)
self._forward = Velocity(forward_min.value, forward_max.value, self._num_steps.value)

backward_min = self._node.declare_parameter(
'turbo/linear_backward_min', forward_min)
'turbo.linear_backward_min', forward_min.value)
backward_max = self._node.declare_parameter(
'turbo/linear_backward_max', forward_max)
self._backward = Velocity(backward_min, backward_max, self._num_steps)
'turbo.linear_backward_max', forward_max.value)
self._backward = Velocity(backward_min.value, backward_max.value, self._num_steps.value)

lateral_min = self._node.declare_parameter(
'turbo/linear_lateral_min', 1.0)
'turbo.linear_lateral_min', 1.0)
lateral_max = self._node.declare_parameter(
'turbo/linear_lateral_max', 1.0)
self._lateral = Velocity(lateral_min, lateral_max, self._num_steps)
'turbo.linear_lateral_max', 1.0)
self._lateral = Velocity(lateral_min.value, lateral_max.value, self._num_steps.value)

angular_min = self._node.declare_parameter('turbo/angular_min', 1.0)
angular_max = self._node.declare_parameter('turbo/angular_max', 1.0)
self._angular = Velocity(angular_min, angular_max, self._num_steps)
angular_min = self._node.declare_parameter('turbo.angular_min', 1.0)
angular_max = self._node.declare_parameter('turbo.angular_max', 1.0)
self._angular = Velocity(angular_min.value, angular_max.value, self._num_steps.value)

default_init_step = np.floor((self._num_steps + 1)/2.0)
default_init_step = np.floor((self._num_steps.value + 1)/2.0)
init_step = self._node.declare_parameter(
'turbo/init_step', default_init_step)
'turbo.init_step', default_init_step)

if init_step < 0 or init_step > self._num_steps:
if init_step.value < 0 or init_step.value > self._num_steps.value:
self._init_step = default_init_step
self._node.get_logger().warn('Initial step %d outside range [1, %d]!'
' Falling back to default %d' %
(init_step, self._num_steps, default_init_step))
self._node.get_logger().warn(
'Initial step %d outside range [1, %d]! Falling back to default %d' %
(init_step.value, self._num_steps.value, default_init_step))
else:
self._init_step = init_step
self._init_step = init_step.value

self.reset_turbo()

Expand Down Expand Up @@ -148,7 +150,7 @@ def scale_twist(self, cmd):
return twist

def increase_turbo(self):
if self._current_step < self._num_steps:
if self._current_step < self._num_steps.value:
self._current_step += 1
self.increase_angular_turbo()

Expand All @@ -158,7 +160,7 @@ def decrease_turbo(self):
self.decrease_angular_turbo()

def increase_angular_turbo(self):
if self._current_angular_step < self._num_steps:
if self._current_angular_step < self._num_steps.value:
self._current_angular_step += 1

def decrease_angular_turbo(self):
Expand All @@ -173,8 +175,11 @@ def reset_turbo(self):
class TextMarker(object):

def __init__(self, node, scale=1.0, z=0.0):
# TODO latch
self._pub = node.create_publisher(Marker, 'text_marker', 1)
self._pub = node.create_publisher(
Marker, 'text_marker', QoSProfile(
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
depth=1)
)

self._scale = scale
self._z = z
Expand Down Expand Up @@ -214,21 +219,25 @@ class JoystickRelay(Node):
def __init__(self):
super().__init__('joystick_relay')

self._current_priority = self.declare_parameter('priority', True)
self._velocity_control = VelocityControl()
self._current_priority = Bool()
self._current_priority.data = self.declare_parameter('priority', True).value
self._velocity_control = VelocityControl(self)

self._marker = TextMarker(self, 0.5, 2.0)

self._pub_cmd = self.create_publisher(Twist, 'joy_vel_out', 1)
self._subscriber = self.create_subscription(
Twist, 'joy_vel_in', self._forward_cmd, 1)

# TODO Latch
self._pub_priority = self.create_publisher(Bool, 'joy_priority', 1)
self._pub_priority = self.create_publisher(
Bool, 'joy_priority', QoSProfile(
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
depth=1)
)

# Wait for subscribers and publish initial joy_priority:
self._pub_priority.publish(self._current_priority)
self._marker.update(self._current_priority)
self._marker.update(self._current_priority.data)

# Marker timer (required to update the marker when the robot doesn't receive velocities):
self._timer = self.create_timer(1.0, self._timer_callback)
Expand All @@ -255,35 +264,38 @@ def __init__(self):
self._velocity_control.reset_turbo)

def _forward_cmd(self, cmd):
if self._current_priority:
if self._current_priority.data:
self._pub_cmd.publish(self._velocity_control.scale_twist(cmd))

self._marker.update(self._current_priority)
self._marker.update(self._current_priority.data)

def _toggle_priority(self):
self._current_priority = not self._current_priority
self.get_logger().info("Toggled joy_priority, current status is: %s",
self._current_priority)
self._current_priority.data = not self._current_priority.data
self.get_logger().info("Toggled joy_priority, current status is: %s" %
(self._current_priority.data))
self._pub_priority.publish(self._current_priority)
self._marker.update(self._current_priority)
self._marker.update(self._current_priority.data)

# Reset velocity to 0:
if self._current_priority:
if self._current_priority.data:
self._pub_cmd.publish(Twist())

def _timer_callback(self, event):
self._marker.update(self._current_priority)
def _timer_callback(self):
self._marker.update(self._current_priority.data)


def main(args=None):
rclpy.init(args=args)

node = JoystickRelay()

rclpy.spin(node)

node.destroy_node()
rclpy.shutdown()
try:
rclpy.spin(node)
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
node.destroy_node()
rclpy.try_shutdown()


if __name__ == '__main__':
Expand Down
Loading

0 comments on commit a3a6547

Please sign in to comment.