joy_cmd_vel.py
Go to the documentation of this file.
1 from jsk_teleop_joy.joy_plugin import JSKJoyPlugin
2 
3 import imp
4 try:
5  imp.find_module("geometry_msgs")
6 except:
7  import roslib; roslib.load_manifest('jsk_teleop_joy')
8 
9 
10 from geometry_msgs.msg import Twist
11 import tf
12 import rospy
13 import numpy
14 import math
15 import tf
16 import numpy
17 import time
18 
20  #def __init__(self, name='JoyPose6D', publish_pose=True):
21  def __init__(self, name, args):
22  JSKJoyPlugin.__init__(self, name, args)
23  self.cmd_vel = Twist()
24  self.publish_cmd_vel = self.getArg('publish_cmd_vel', True)
25  self.max_vel = self.getArg('max_vel', 0.2)
26  self.max_omega = self.getArg('max_omega', 0.17) # 10[deg]
27  self.orthogonal_axis_mode = self.getArg('orthogonal_axis_mode', True)
28  self.prev_time = rospy.Time.now()
29  if self.publish_cmd_vel:
30  self.twist_pub = rospy.Publisher(self.getArg('cmd_vel', 'cmd_vel'), Twist, queue_size = 1)
31 
32  def joyCB(self, status, history):
33  if history.length() > 0:
34  latest = history.latest()
35  if status.R3 and status.L2 and status.R2 and not (latest.R3 and latest.L2 and latest.R2):
36  self.followView(not self.followView())
37  cmd_vel = Twist()
38  # currently only support 2D plane movement
39  if not status.R3:
40  # xy
41  dist = numpy.sqrt(status.left_analog_y * status.left_analog_y + status.left_analog_x * status.left_analog_x) # dist is assumed to be 0 < dist < 1
42  scale_v = self.max_vel * dist
43  if self.orthogonal_axis_mode:
44  if abs(status.left_analog_y) - abs(status.left_analog_x) > 0.2:
45  x_diff = status.left_analog_y * scale_v
46  y_diff = 0.0
47  elif abs(status.left_analog_y) - abs(status.left_analog_x) < -0.2:
48  x_diff = 0.0
49  y_diff = status.left_analog_x * scale_v
50  else:
51  x_diff = 0.0
52  y_diff = 0.0
53  else:
54  x_diff = status.left_analog_y * scale_v
55  y_diff = status.left_analog_x * scale_v
56  else:
57  x_diff = 0.0
58  y_diff = 0.0
59 
60  cmd_vel.linear.x = x_diff
61  cmd_vel.linear.y = y_diff
62  cmd_vel.linear.z = 0.0
63 
64  dyaw = 0.0
65  if not status.R3:
66  if status.L1:
67  dyaw = self.max_omega
68  elif status.R1:
69  dyaw = -self.max_omega
70 
71  cmd_vel.angular.x = 0.0
72  cmd_vel.angular.y = 0.0
73  cmd_vel.angular.z = dyaw
74 
75  # publish at 10hz
76  if self.publish_cmd_vel:
77  now = rospy.Time.from_sec(time.time())
78  # placement.time_from_start = now - self.prev_time
79  if (now - self.prev_time).to_sec() > 1 / 30.0:
80  self.twist_pub.publish(cmd_vel)
81  self.prev_time = now
jsk_teleop_joy.joy_plugin.JSKJoyPlugin.getArg
def getArg(self, key, default=None)
Definition: joy_plugin.py:19
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.publish_cmd_vel
publish_cmd_vel
Definition: joy_cmd_vel.py:24
jsk_teleop_joy.joy_plugin
Definition: joy_plugin.py:1
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.prev_time
prev_time
Definition: joy_cmd_vel.py:28
jsk_teleop_joy.joy_plugin.JSKJoyPlugin
Definition: joy_plugin.py:5
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.orthogonal_axis_mode
orthogonal_axis_mode
Definition: joy_cmd_vel.py:27
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.max_vel
max_vel
Definition: joy_cmd_vel.py:25
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.cmd_vel
cmd_vel
Definition: joy_cmd_vel.py:23
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.twist_pub
twist_pub
Definition: joy_cmd_vel.py:30
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.__init__
def __init__(self, name, args)
Definition: joy_cmd_vel.py:21
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel
Definition: joy_cmd_vel.py:19
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.max_omega
max_omega
Definition: joy_cmd_vel.py:26
jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel.joyCB
def joyCB(self, status, history)
Definition: joy_cmd_vel.py:32


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:49