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 self.cb_feedback(feedback)
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
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