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 joy_feedback_array(cls, msg, obj):
for i in range(msg['_length']):
jf = JoyFeedback()
jf.type = msg['%s_type' % i]
jf.id = msg['%s_id' % i]
jf.intensity = msg['%s_intensity' % i]
obj.array.append(jf)
return(obj)
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 selected_only_box_cb(msg):
pose_stamped_msg = PoseStamped()
pose_stamped_msg.header = msg.header
pose_stamped_msg.pose = msg.pose
set_pose_pub.publish(pose_stamped_msg)
# set_pose_srv(pose_stamped=pose_stamped_msg)
# dimensions
x = msg.dimensions.x
y = msg.dimensions.y
z = msg.dimensions.z
shape_type = get_type_srv()
midi_feedback = []
if shape_type.type == TransformableMarkerOperate.BOX:
set_dim_srv(dimensions=MarkerDimensions(x=x, y=y, z=z))
midi_feedback.append(
JoyFeedback(id=0, intensity=(x - x_min) / (x_max - x_min)))
midi_feedback.append(
JoyFeedback(id=1, intensity=(y - y_min) / (y_max - y_min)))
midi_feedback.append(
JoyFeedback(id=2, intensity=(z - z_min) / (z_max - z_min)))
elif shape_type.type == TransformableMarkerOperate.CYLINDER:
r = min(x, y)
set_dim_srv(dimensions=MarkerDimensions(radius=r, z=z))
midi_feedback.append(
JoyFeedback(id=0, intensity=(r - r_min) / (r_max - r_min)))
midi_feedback.append(
JoyFeedback(id=1, intensity=(z - z_min) / (z_max - z_min)))
elif shape_type.type == TransformableMarkerOperate.TORUS:
r = min(x, y) * 0.5
sr = min(x, y) * 0.5 * 0.1
set_dim_srv(dimensions=MarkerDimensions(radius=r, small_radius=sr))
midi_feedback.append(
JoyFeedback(id=0, intensity=(r - r_min) / (r_max - r_min)))
midi_feedback.append(
JoyFeedback(id=1, intensity=(sr - sr_min) / (sr_max - sr_min)))
color = ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.6)
set_color_pub.publish(color)
midi_feedback_pub.publish(midi_feedback)
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)
pass
elif data.buttons[5]==False and past.buttons[5]==True:
#LED OFF
pass
print(siita)
pub.publish(msg)
past=data
rospy.Subscriber('/wiimote/state', State, callback, queue_size=10)
pub = rospy.Publisher('/joy/set_feedback', JoyFeedbackArray, queue_size=10)
#stm = rospy.Publisher('/stm', stm_carrot, 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
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 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))
