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 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 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 __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 selected_box_cb(msg, points_msg):
rospy.loginfo("selected_box_cb driven")
if not auto_set_mode:
return
try:
resp1 = send_icp_srv(points=points_msg, box=msg)
erase_all_marker()
x = resp1.dim.x
y = resp1.dim.y
z = resp1.dim.z
r = resp1.dim.radius
sr = resp1.dim.small_radius
midi_feedback = []
if (resp1.dim.type == 0):
insert_marker(shape_type=TransformableMarkerOperate.BOX,
name='box1',
description='')
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 (resp1.dim.type == 1):
insert_marker(shape_type=TransformableMarkerOperate.CYLINDER,
name='cylinder1',
description='')
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)))
else:
insert_marker(shape_type=TransformableMarkerOperate.TORUS,
name='torus1',
description='')
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)))
midi_feedback_pub.publish(midi_feedback)
color = ColorRGBA(r=1.0, g=0.0, b=0.0, a=0.6)
set_color_pub.publish(color)
set_pose_srv(pose_stamped=resp1.pose_stamped)
set_dim_srv(dimensions=resp1.dim)
rospy.loginfo("icp succeeded")
return
except rospy.ServiceException, e:
print "ICP Service call failed: %s" % e
def marker_info_cb(msg):
erase_all_marker()
x = msg.marker_dim.x
y = msg.marker_dim.y
z = msg.marker_dim.z
r = msg.marker_dim.radius
sr = msg.marker_dim.small_radius
midi_feedback = []
if (msg.marker_dim.type == 0):
insert_marker(shape_type=TransformableMarkerOperate.BOX,
name='box1',
description='')
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 (msg.marker_dim.type == 1):
insert_marker(shape_type=TransformableMarkerOperate.CYLINDER,
name='cylinder1',
description='')
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)))
else:
insert_marker(shape_type=TransformableMarkerOperate.TORUS,
name='torus1',
description='')
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)))
midi_feedback_pub.publish(midi_feedback)
color = ColorRGBA(r=1.0, g=0.0, b=0.0, a=0.6)
set_color_pub.publish(color)
set_pose_srv(pose_stamped=msg.marker_pose_stamped)
set_dim_srv(dimensions=msg.marker_dim)
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 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
