def setPS3LED(self):
led0 = JoyFeedback()
led0.type = JoyFeedback.TYPE_LED
led0.id = 0
led0.intensity = 1.
led1 = JoyFeedback()
led1.type = JoyFeedback.TYPE_LED
led1.id = 1
led1.intensity = 0.
led2 = JoyFeedback()
led2.type = JoyFeedback.TYPE_LED
led2.id = 2
led2.intensity = 0.
led3 = JoyFeedback()
led3.type = JoyFeedback.TYPE_LED
led3.id = 3
led3.intensity = 0.
rum0 = JoyFeedback()
rum0.type = JoyFeedback.TYPE_RUMBLE
rum0.id = 0
rum0.intensity = 0.
rum1 = JoyFeedback()
rum1.type = JoyFeedback.TYPE_RUMBLE
rum1.id = 1
rum1.intensity = 0.
feedback = JoyFeedbackArray()
feedback.array = [led0,led1,led2,led3,rum0,rum1]
self.ps3joy_pub.publish(feedback)
rospy.loginfo("setPS3LED: "+str(feedback))
def set_feedback(value): msg_f, vib = JoyFeedbackArray(), JoyFeedback() # id = 0, 1, 2 type of vibration vib.type, vib.id, vib.intensity = JoyFeedback.TYPE_RUMBLE, 0, value msg_f.array = [vib] pub.publish(msg_f)
def handle_wiimote_state(self, msg):
with self._lock:
if self.vz == -msg.angular_velocity_zeroed.z and \
self.vx == msg.angular_velocity_zeroed.x and \
self.vy == -msg.angular_velocity_zeroed.y:
self.stale_count += 1
else:
self.stale_count = 0
if self.stale_count == MAX_STALE_COUNT:
rospy.logwarn('Attempting to reset a stale device')
self.stale_handler()
return
self.vz = -msg.angular_velocity_zeroed.z
self.vx = msg.angular_velocity_zeroed.x
self.vy = -msg.angular_velocity_zeroed.y
self.last_state = msg
if not msg.LEDs[0]:
feedback_array_msg = JoyFeedbackArray()
feedback_msg = JoyFeedback()
feedback_msg.intensity = 1.0
feedback_array_msg.array.append(feedback_msg)
self.feedback_pub.publish(feedback_array_msg)
def cb_rum(self, event):
if self.gripperL._state.force > 20 and self.rumFlag == True:
rum = JoyFeedback()
rum.type = JoyFeedback.TYPE_RUMBLE
rum.id = 0
rum.intensity = 0.51
msg = JoyFeedbackArray()
msg.array = [rum]
self.pub.publish(msg)
rospy.sleep(0.3)
rum.intensity = 0.0
msg.array = [rum]
self.pub.publish(msg)
self.rumFlag = False
if self.gripperL._state.force == 0:
self.rumFlag = True
def __init__(self):
# Setup parameters
rospy.loginfo("wii interface initialized")
self.automode = False
self.deadman = Bool(False)
self.linear = 0.0
self.angular = 0.0
self.next_state_change = rospy.Time.now() + rospy.Duration(1)
self.rumble_on = False
self.warning = False
self.filter = rospy.get_param("~filter", 10)
self.pitch = [0.0] * self.filter
self.roll = [0.0] * self.filter
self.ptr = 0
self.twist = TwistStamped()
self.fb = JoyFeedbackArray(array=[
JoyFeedback(type=JoyFeedback.TYPE_LED, intensity=0, id=0),
JoyFeedback(type=JoyFeedback.TYPE_LED, intensity=0, id=1),
JoyFeedback(type=JoyFeedback.TYPE_LED, intensity=0, id=2),
JoyFeedback(type=JoyFeedback.TYPE_LED, intensity=0, id=3),
JoyFeedback(type=JoyFeedback.TYPE_RUMBLE, intensity=0, id=0)
])
# Callbacks
self.button_A_cb = self.no_callback_registered
self.button_up_cb = self.no_callback_registered
self.button_down_cb = self.no_callback_registered
self.button_home_cb = self.no_callback_registered
# Get parameters
self.reduced_range = rospy.get_param("~reduced_range",
40) # Given in percent
self.reduced_range = self.reduced_range / 100.0 # Convert to ratio
self.deadband = rospy.get_param("~deadband", 5) # Given in percent
self.deadband = self.deadband / 100.0 # Convert to rati
self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 2)
self.max_angular_velocity = rospy.get_param("~max_angular_velocity", 4)
self.publish_frequency = rospy.get_param("~publish_frequency", 10)
# Get topic names
self.deadman_topic = rospy.get_param("~deadman_topic", 'deadman')
self.automode_topic = rospy.get_param("~automode_topic", 'automode')
self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", 'cmd_vel')
self.feedback_topic = rospy.get_param("~feedback_topic",
'joy/set_feedback')
self.joy_topic = rospy.get_param("~joy_topic", 'joy')
self.status_topic = rospy.get_param("~all_ok_topic",
'/fmSafety/all_ok')
# Setup topics
self.deadman_pub = rospy.Publisher(self.deadman_topic, Bool)
self.automode_pub = rospy.Publisher(self.automode_topic, Bool)
self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
self.fb_pub = rospy.Publisher(self.feedback_topic, JoyFeedbackArray)
self.joy_sub = rospy.Subscriber(self.joy_topic, Joy, self.onJoy)
self.status_sub = rospy.Subscriber(self.status_topic, BoolStamped,
self.onAllOk)
def talker():
pub = rospy.Publisher('/joy/set_feedback', JoyFeedbackArray)
rospy.init_node('ledControlTester', anonymous=True)
led0 = JoyFeedback()
led0.type = JoyFeedback.TYPE_LED
led0.id = 0
led1 = JoyFeedback()
led1.type = JoyFeedback.TYPE_LED
led1.id = 1
led2 = JoyFeedback()
led2.type = JoyFeedback.TYPE_LED
led2.id = 2
led3 = JoyFeedback()
led3.type = JoyFeedback.TYPE_LED
led3.id = 3
rum = JoyFeedback()
rum.type = JoyFeedback.TYPE_RUMBLE
rum.id = 0
while not rospy.is_shutdown():
msg = JoyFeedbackArray()
msg.array = [led0, led1, led2, led3, rum]
rum.intensity = 0.6
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
rum.intensity = 0
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
def __init__(self):
self.stat = None
self.ps3 = JoyFeedbackArray()
self.led1 = JoyFeedback()
self.led1.type = TYPE_LED
self.led1.id = PS3_LED_1
self.led1.intensity = 0.0
self.led2 = JoyFeedback()
self.led2.type = TYPE_LED
self.led2.id = PS3_LED_2
self.led2.intensity = 0.0
self.led3 = JoyFeedback()
self.led3.type = TYPE_LED
self.led3.id = PS3_LED_3
self.led3.intensity = 0.0
self.led4 = JoyFeedback()
self.led4.type = TYPE_LED
self.led4.id = PS3_LED_4
self.led4.intensity = 0.0
self.ps3.array.append(self.led1)
self.ps3.array.append(self.led2)
self.ps3.array.append(self.led3)
self.ps3.array.append(self.led4)
self.rumble1 = JoyFeedback()
self.rumble1.type = TYPE_RUMBLE
self.rumble1.id = PS3_RUMBLE_1
self.rumble1.intensity = 0.0
self.ps3.array.append(self.rumble1)
print("=== ps3 ===")
print(self.ps3)
self.pub = rospy.Publisher('joy/set_feedback',
JoyFeedbackArray,
queue_size=2)
def light_thread():
global lights, max_delay
on = True
delay = 0.5
all_blink = True
while not rospy.is_shutdown():
set_lights = lights[:-1]
msg = JoyFeedbackArray()
for i in range(4):
unit_msg = JoyFeedback()
unit_msg.type = 0
unit_msg.id = i
if set_lights[i] == True:
unit_msg.intensity = 30.0
if set_lights[i] == "blinking" and on:
unit_msg.intensity = 30.0
if set_lights[i] == False:
unit_msg.intensity = 0.0
if set_lights[i] == "blinking" and not on:
unit_msg.intensity = 0.0
msg.array.append(unit_msg)
light_pub.publish(msg)
sleep(delay)
on = not on
def talker():
pub = rospy.Publisher('/joy/set_feedback', JoyFeedbackArray, queue_size=1)
rospy.init_node('ledControlTester', anonymous=True)
led0 = JoyFeedback()
led0.type = JoyFeedback.TYPE_LED
led0.id = 0
led1 = JoyFeedback()
led1.type = JoyFeedback.TYPE_LED
led1.id = 1
led2 = JoyFeedback()
led2.type = JoyFeedback.TYPE_LED
led2.id = 2
led3 = JoyFeedback()
led3.type = JoyFeedback.TYPE_LED
led3.id = 3
rum = JoyFeedback()
rum.type = JoyFeedback.TYPE_RUMBLE
rum.id = 0
while not rospy.is_shutdown():
msg = JoyFeedbackArray()
msg.array = [led0, led1, led2, led3, rum]
led0.intensity = 0.2
led3.intensity = 0.2
rum.intensity = 0.49
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
led0.intensity = 1.0
rum.intensity = 0.51
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
led0.intensity = 0.0
led1.intensity = 1.0
rum.intensity = 0.0
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
led1.intensity = 0.0
led2.intensity = 1.0
rum.intensity = 0.7
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
led2.intensity = 0.0
led3.intensity = 1.0
rum.intensity = 0.49
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
led1.intensity = 1.0
led2.intensity = 1.0
rum.intensity = 1.0
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
led0.intensity = 1.0
led1.intensity = 0.4
led2.intensity = 0.4
msg.array = [led0, led1, led2]
if msg is not None:
rospy.logdebug("Msg: " + str(msg))
pub.publish(msg)
rospy.sleep(INTER_PATTERN_SLEEP_DURATION)
def execute_rumble(self):
if self.rumble_type is not None:
self.rumble_pub.publish(
JoyFeedbackArray([JoyFeedback(type=1, id=0, intensity=0.0)]))
if self.rumble_type == 'long':
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=1.0)]))
t_end = rospy.Time.now() + rospy.Duration(2.0)
while rospy.Time.now(
) < t_end and self.rumble_type is not None:
rospy.sleep(0.1)
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=0.0)]))
elif self.rumble_type == 'short':
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=1.0)]))
t_end = rospy.Time.now() + rospy.Duration(1.0)
while rospy.Time.now(
) < t_end and self.rumble_type is not None:
rospy.sleep(0.1)
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=0.0)]))
elif self.rumble_type == 'shortx3':
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=1.0)]))
t_end = rospy.Time.now() + rospy.Duration(0.4)
while rospy.Time.now(
) < t_end and self.rumble_type is not None:
rospy.sleep(0.1)
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=0.0)]))
if self.rumble_type is None:
return
t_end = rospy.Time.now() + rospy.Duration(0.2)
while rospy.Time.now(
) < t_end and self.rumble_type is not None:
rospy.sleep(0.1)
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=1.0)]))
t_end = rospy.Time.now() + rospy.Duration(0.4)
while rospy.Time.now(
) < t_end and self.rumble_type is not None:
rospy.sleep(0.1)
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=0.0)]))
if self.rumble_type is None:
return
t_end = rospy.Time.now() + rospy.Duration(0.2)
while rospy.Time.now(
) < t_end and self.rumble_type is not None:
rospy.sleep(0.1)
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=1.0)]))
t_end = rospy.Time.now() + rospy.Duration(0.4)
while rospy.Time.now(
) < t_end and self.rumble_type is not None:
rospy.sleep(0.1)
self.rumble_pub.publish(
JoyFeedbackArray(
[JoyFeedback(type=1, id=0, intensity=0.0)]))
else:
rospy.logerr('user_prompt > unrecognized rumble_type=%s' %
rumble_type)
led0 = JoyFeedback()
led0.type = JoyFeedback.TYPE_LED
led0.id = 0
led1 = JoyFeedback()
led1.type = JoyFeedback.TYPE_LED
led1.id = 1
led2 = JoyFeedback()
led2.type = JoyFeedback.TYPE_LED
led2.id = 2
led3 = JoyFeedback()
led3.type = JoyFeedback.TYPE_LED
led3.id = 3
rum = JoyFeedback()
rum.type = JoyFeedback.TYPE_RUMBLE
rum.id = 0
msg = JoyFeedbackArray()
msg.array = [led0, led1, led2, led3, rum]
led0.intensity=1
led1.intensity=1
if __name__ == '__main__':
try:
rospy.spin()
except KeyboardInterrupt:
ser.close()
sys.exit()
import rospy
from sensor_msgs.msg import JoyFeedbackArray, JoyFeedback
rumbler = rospy.Publisher("/joy/set_feedback", JoyFeedbackArray, queue_size=1)
data1 = JoyFeedback
data2 = JoyFeedback
intensity = 0.0
data1.type = 1
data1.id = 1
data1.intensity = intensity
data2.type = 1
data2.id = 1
data2.intensity = intensity
datas = JoyFeedbackArray([data1, data2])
rospy.init_node('rumblerrrr')
for i in range(100000):
rumbler.publish(datas)
