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