finned_uuv_teleop.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Copyright (c) 2016 The UUV Simulator Authors.
3 # All rights reserved.
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 from __future__ import print_function
17 import numpy
18 import rospy
19 import tf
20 import tf.transformations as trans
21 
22 from sensor_msgs.msg import Joy
23 from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
24 from uuv_thrusters.models import Thruster
25 from rospy.numpy_msg import numpy_msg
26 
27 
29  def __init__(self):
30  print('FinnedUUVControllerNode: initializing node')
31 
32  self._ready = False
33 
34  # Test if any of the needed parameters are missing
35  param_labels = ['n_fins', 'gain_roll', 'gain_pitch', 'gain_yaw',
36  'thruster_model', 'fin_topic_prefix',
37  'fin_topic_suffix', 'thruster_topic',
38  'axis_thruster', 'axis_roll', 'axis_pitch', 'axis_yaw']
39 
40  for label in param_labels:
41  if not rospy.has_param('~%s' % label):
42  raise rospy.ROSException('Parameter missing, label=%s' % label)
43 
44  # Number of fins
45  self._n_fins = rospy.get_param('~n_fins')
46 
47  # Thruster joy axis gain
49  if rospy.has_param('~thruster_joy_gain'):
50  self._thruster_joy_gain = rospy.get_param('~thruster_joy_gain')
51 
52  # Read the vector for contribution of each fin on the change on
53  # orientation
54  gain_roll = rospy.get_param('~gain_roll')
55  gain_pitch = rospy.get_param('~gain_pitch')
56  gain_yaw = rospy.get_param('~gain_yaw')
57 
58  if len(gain_roll) != self._n_fins or len(gain_pitch) != self._n_fins \
59  or len(gain_yaw) != self._n_fins:
60  raise rospy.ROSException('Input gain vectors must have length '
61  'equal to the number of fins')
62 
63  # Create the command angle to fin angle mapping
64  self._rpy_to_fins = numpy.vstack((gain_roll, gain_pitch, gain_yaw)).T
65 
66  # Read the joystick mapping
67  self._joy_axis = dict(axis_thruster=rospy.get_param('~axis_thruster'),
68  axis_roll=rospy.get_param('~axis_roll'),
69  axis_pitch=rospy.get_param('~axis_pitch'),
70  axis_yaw=rospy.get_param('~axis_yaw'))
71 
72  # Subscribe to the fin angle topics
73  self._pub_cmd = list()
74  self._fin_topic_prefix = rospy.get_param('~fin_topic_prefix')
75  self._fin_topic_suffix = rospy.get_param('~fin_topic_suffix')
76  for i in range(self._n_fins):
77  topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix
78  self._pub_cmd.append(
79  rospy.Publisher(topic, FloatStamped, queue_size=10))
80 
81  # Create the thruster model object
82  try:
83  self._thruster_topic = rospy.get_param('~thruster_topic')
84  self._thruster_params = rospy.get_param('~thruster_model')
85  if 'max_thrust' not in self._thruster_params:
86  raise rospy.ROSException('No limit to thruster output was given')
87  self._thruster_model = Thruster.create_thruster(
88  self._thruster_params['name'], 0,
89  self._thruster_topic, None, None,
90  **self._thruster_params['params'])
91  except:
92  raise rospy.ROSException('Thruster model could not be initialized')
93 
94  # Subscribe to the joystick topic
95  self.sub_joy = rospy.Subscriber('joy', numpy_msg(Joy),
96  self.joy_callback)
97 
98  self._ready = True
99 
100  def joy_callback(self, msg):
101  """Handle callbacks with joystick state."""
102 
103  if not self._ready:
104  return
105 
106  try:
107  thrust = max(0, msg.axes[self._joy_axis['axis_thruster']]) * \
108  self._thruster_params['max_thrust'] * \
109  self._thruster_joy_gain
110 
111  cmd_roll = msg.axes[self._joy_axis['axis_roll']]
112  if abs(cmd_roll) < 0.2:
113  cmd_roll = 0.0
114 
115  cmd_pitch = msg.axes[self._joy_axis['axis_pitch']]
116  if abs(cmd_pitch) < 0.2:
117  cmd_pitch = 0.0
118 
119  cmd_yaw = msg.axes[self._joy_axis['axis_yaw']]
120  if abs(cmd_yaw) < 0.2:
121  cmd_yaw = 0.0
122 
123  rpy = numpy.array([cmd_roll, cmd_pitch, cmd_yaw])
124  fins = self._rpy_to_fins.dot(rpy)
125 
126  self._thruster_model.publish_command(thrust)
127 
128  for i in range(self._n_fins):
129  cmd = FloatStamped()
130  cmd.data = fins[i]
131  self._pub_cmd[i].publish(cmd)
132 
133  if not self._ready:
134  return
135  except Exception as e:
136  print('Error occurred while parsing joystick input, check '
137  'if the joy_id corresponds to the joystick '
138  'being used. message={}'.format(e))
139 
140 
141 if __name__ == '__main__':
142  print('starting FinnedUUVControllerNode.py')
143  rospy.init_node('finned_uuv_teleop')
144 
145  try:
147  rospy.spin()
148  except rospy.ROSInterruptException:
149  print('caught exception')
150  print('exiting')


uuv_teleop
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:41