def cb_joy_feedback(self, msg):
"""
Callback method for sensor_msgs/JoyFeedbackArray
The message contains the following feedback:
LED0: red
LED1: green
LED2: blue
RUMBLE0: rumble small
RUMBLE1: rumble big
:param msg:
:type msg: JoyFeedbackArray
:return:
"""
feedback = Feedback()
for jf in msg.array:
if jf.type == JoyFeedback.TYPE_LED:
feedback.set_led = True
if jf.id == 0:
feedback.led_r = jf.intensity
elif jf.id == 1:
feedback.led_g = jf.intensity
elif jf.id == 2:
feedback.led_b = jf.intensity
elif jf.type == JoyFeedback.TYPE_RUMBLE:
feedback.set_rumble = True
if jf.id == 0:
feedback.rumble_small = jf.intensity
elif jf.id == 1:
feedback.rumble_big = jf.intensity
feedback.rumble_duration = 1.0
self.cb_feedback(feedback)
def __init__(self):
self._stamped = rospy.get_param('~stamped', False)
if self._stamped:
self._cls = TwistStamped
self._frame_id = rospy.get_param('~frame_id', 'base_link')
else:
self._cls = Twist
self._inputs = rospy.get_param('~inputs')
self._scales = rospy.get_param('~scales')
self.buttonpressed = False
self.counter = 0
self._attrs = []
for attr in Status.__slots__:
if attr.startswith('axis_') or attr.startswith('button_'):
self._attrs.append(attr)
self._pub = rospy.Publisher('cmd_vel/joystick',
self._cls,
queue_size=1)
self._pub_squ = rospy.Publisher('/joystick/square',
Bool,
queue_size=1,
latch=True)
self._pub_triangle = rospy.Publisher('/joystick/triangle',
Bool,
queue_size=1,
latch=True)
self._pub_circle = rospy.Publisher('/soft_estop/reset',
Bool,
queue_size=1) # , latch =True)
self._pub_cross = rospy.Publisher('/soft_estop/enable',
Bool,
queue_size=1) # , latch =True)
self._pub_trim = rospy.Publisher('/trim_increment',
Float32,
queue_size=1)
self._trim_incre_value = rospy.get_param('~trim_increment_value', 0.05)
self._pub_feedback = rospy.Publisher("set_feedback",
Feedback,
queue_size=1)
rospy.Subscriber('status', Status, self.cb_status, queue_size=1)
rospy.loginfo("Linear Scale is at %f", self._scales["linear"]["x"])
self._feedback = Feedback()
self._feedback.set_rumble = True
def cb_status(self, msg):
"""
:type msg: Status
"""
now = rospy.Time.now()
if (now - self._last_pub_time).to_sec() < self._min_interval:
return
feedback = Feedback()
feedback.set_rumble = True
feedback.rumble_small = abs(msg.axis_left_y)
feedback.rumble_big = abs(msg.axis_right_y)
# Set LED using the touchpad
touch = msg.touch0
if touch.active and msg.button_circle:
feedback.set_led = True
self._led['r'] = touch.x
if touch.active and msg.button_triangle:
feedback.set_led = True
self._led['g'] = touch.x
if touch.active and msg.button_cross:
feedback.set_led = True
self._led['b'] = touch.x
feedback.led_r = self._led['r']
feedback.led_g = self._led['g']
feedback.led_b = self._led['b']
# Turn on/off flash with PS button
if not self._prev.button_ps and msg.button_ps:
feedback.set_led_flash = True
if self._led['flashing']:
feedback.led_flash_off = 0
else:
feedback.led_flash_on = 0.2
feedback.led_flash_off = 0.2
self._led['flashing'] = not self._led['flashing']
self._pub_feedback.publish(feedback)
self._prev = msg
self._last_pub_time = now
def cb_status(self, msg):
"""
:type msg: Status
"""
now = self._node.get_clock().now()
# Commenting out this check for now because to_sec() does not seem to work for rclpy Duration objects
# This checks to see if the min interval for a callback has been violated.
# if (now - self._last_pub_time).to_sec() < self._min_interval:
# return
feedback = Feedback()
feedback.set_rumble = True
feedback.rumble_small = abs(msg.axis_left_y)
feedback.rumble_big = abs(msg.axis_right_y)
# Set LED using the touchpad
touch = msg.touch0
if touch.active and msg.button_circle:
feedback.set_led = True
self._led["r"] = touch.x
if touch.active and msg.button_triangle:
feedback.set_led = True
self._led["g"] = touch.x
if touch.active and msg.button_cross:
feedback.set_led = True
self._led["b"] = touch.x
feedback.led_r = float(self._led["r"])
feedback.led_g = float(self._led["g"])
feedback.led_b = float(self._led["b"])
# Turn on/off flash with PS button
if not self._prev.button_ps and msg.button_ps:
feedback.set_led_flash = True
if self._led["flashing"]:
feedback.led_flash_off = 0.0
else:
feedback.led_flash_on = 0.2
feedback.led_flash_off = 0.2
self._led["flashing"] = not self._led["flashing"]
self._pub_feedback.publish(feedback)
self._prev = msg
self._last_pub_time = now