Skip to content

Commit

Permalink
Fixing errors in keyboard teleop. Trying python keyboard launch
Browse files Browse the repository at this point in the history
  • Loading branch information
AVSurfer123 committed Jan 18, 2021
1 parent 6a64390 commit ba5c396
Show file tree
Hide file tree
Showing 3 changed files with 36 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,10 @@
</node>
</group>

<include file="$(find-pkg_share uuv_teleop)/launch/uuv_keyboard_teleop.launch">
<include file="$(find-pkg-share uuv_teleop)/launch/uuv_keyboard_teleop.launch">
<arg name="uuv_name" value="$(var uuv_name)"/>
<arg name="output_topic" value="cmd_vel"/>
<arg name="message_type" value="twist"/>
</include>

</launch>
24 changes: 24 additions & 0 deletions uuv_teleop/launch/keyboard_teleop.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():

uuv_name_action = DeclareLaunchArgument('uuv_name', description="Name for the UUV to use for namespace")
uuv_name = LaunchConfiguration('uuv_name')

return LaunchDescription([uuv_name_action,
Node(
package='uuv_teleop',
node_executable='vehicle_keyboard_teleop.py',
node_name='keyboard_teleop',
node_namespace=[uuv_name],
output='screen',
emulate_tty=True,
remappings=[('output', ['/', uuv_name, '/cmd_vel'])]
)
])
20 changes: 10 additions & 10 deletions uuv_teleop/scripts/vehicle_keyboard_teleop.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,10 @@ def __init__(self, name, **kwargs):

# Speed setting
self.speed = 1 # 1 = Slow, 2 = Fast
self.l = Vector3(0, 0, 0) # Linear Velocity for Publish
self.a = Vector3(0, 0, 0) # Angular Velocity for publishing
self.l = Vector3(x=0., y=0., z=0.) # Linear Velocity for Publish
self.a = Vector3(x=0., y=0., z=0.) # Angular Velocity for publishing
self.linear_increment = 0.05 # How much to increment linear velocities by, to avoid jerkyness
self.linear_limit = 1 # Linear velocity limit = self.linear_limit * self.speed
self.linear_limit = 1. # Linear velocity limit = self.linear_limit * self.speed
self.angular_increment = 0.05
self.angular_limit = 0.5
# User Interface
Expand Down Expand Up @@ -119,9 +119,9 @@ def _parse_keyboard(self):

# Set Vehicle Speed #
if key_press == "1":
self.speed = 1
self.speed = 1.
if key_press == "2":
self.speed = 2
self.speed = 2.

# Choose ros message accordingly
if self._msg_type == 'twist':
Expand Down Expand Up @@ -173,8 +173,8 @@ def _parse_keyboard(self):

else:
# If no button is pressed reset velocities to 0
self.l = Vector3(x=0, y=0, z=0)
self.a = Vector3(x=0, y=0, z=0)
self.l = Vector3(x=0., y=0., z=0.)
self.a = Vector3(x=0., y=0., z=0.)

# Store velocity message into Twist format
cmd.angular = self.a
Expand All @@ -186,8 +186,8 @@ def _parse_keyboard(self):
self.get_logger().info('Shutting down [%s] node' % name)

# Set twists to 0
cmd.angular = Vector3(x=0, y=0, z=0)
cmd.linear = Vector3(x=0, y=0, z=0)
cmd.angular = Vector3(x=0., y=0., z=0.)
cmd.linear = Vector3(x=0., y=0., z=0.)
self._output_pub.publish(cmd)

exit(-1)
Expand All @@ -207,7 +207,7 @@ def main(args=None):
teleop = KeyBoardVehicleTeleop(name, parameter_overrides=[sim_time_param])
teleop.get_logger().info('Starting [%s] node' % name)

termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, teleop.settings)

rclpy.spin(teleop)
teleop.get_logger().info('Shutting down [%s] node' % name)
Expand Down

0 comments on commit ba5c396

Please sign in to comment.