def set_led_gesture(self, gesture, leds, wave, period, length, mirror, mask): period = max(-32678, min(32678, int(period * 1000))) data = [gesture, leds, wave, period, length, mirror] + mask[:8] data += [1] * (14 - len(data)) self.aseba_led_gesture_publisher.publish( AsebaEvent(rospy.get_rostime(), 0, data))
def callback(msg: ColorRGBA) -> None: r = int(msg.r * 32) g = int(msg.g * 32) b = int(msg.b * 32) aseba_msg = AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[r, g, b]) publisher.publish(aseba_msg)
def set_led_gesture(self, gesture: int, leds: int, wave: int, period: float, length: int, mirror: int, mask: List[int]) -> None: period = max(-32678, min(32678, int(period * 1000))) data = [gesture, leds, wave, period, length, mirror] + mask[:8] data += [1] * (14 - len(data)) self.aseba_led_gesture_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=data))
def on_led_off(self, msg): for i in LED_NUMBER.keys(): print 'off ', i self.aseba_led_publisher.publish( AsebaEvent(rospy.get_rostime(), 0, [i] + 8 * [0])) # sleep to avoid that aseba or ros do not process all messages. # could be improved by having 6 separate aseba topics where to send messages rospy.sleep(0.005)
def on_led(self, msg): i = msg.id num = LED_NUMBER.get(i, 0) if num <= len(msg.values): data = [i] + [int(32 * v) for v in msg.values[:8]] data += [0] * (9 - len(data)) self.aseba_led_publisher.publish( AsebaEvent(rospy.get_rostime(), 0, data))
def on_sound_threshold(self, msg): value = msg * 255 if value < 0: value = 1 if value > 255: value = 0 self.sound_threshold_publisher.publish( AsebaEvent(rospy.get_rostime(), 0, [value]))
def on_sound_threshold(self, msg: AsebaEvent) -> None: value = msg * 255 if value < 0: value = 1 if value > 255: value = 0 self.sound_threshold_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[value]))
def on_sound_play(self, msg: Sound) -> None: freq = max(1, int(msg.frequency)) duration = rclpy.duration.Duration.from_msg( msg.duration).nanoseconds / 1e9 duration = max(1, int(duration * 60)) self.aseba_play_sound_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[freq, duration]))
def on_led(self, msg: Led) -> None: i = msg.id num = LED_NUMBER.get(i, 0) if num <= len(msg.values): data = [i] + [int(32 * v) for v in msg.values[:8]] data += [0] * (9 - len(data)) self.aseba_led_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=data))
def on_led_off(self, msg: Empty) -> None: for i in LED_NUMBER.keys(): self.aseba_led_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[i] + 8 * [0])) # sleep to avoid that aseba or ros do not process all messages. # could be improved by having 6 separate aseba topics where to send # messages time.sleep(0.005)
def on_led(self, msg: Led) -> None: # if Led.CIRCLE != msg.id: # self.get_logger('warn')('The e-puck only support CIRCLE leds.') # return num = 8 if num <= len(msg.values): data = [0] + [1 if v > 0 else 0 for v in msg.values[:8]] data += [0] * (9 - len(data)) self.aseba_led_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=data))
def motor_speed(self, value: int) -> None: if self._motor_speed == value: return self._motor_speed = value if self.motor == 'right': data = [0, value] else: data = [value, 0] self.pub.publish(AsebaEvent(data=data)) c = 8 * value / 500 leds = [max(0, min(1, c - i)) for i in range(8)] self.led_pub.publish(Led(id=Led.CIRCLE, values=leds))
def run(self): standing_wheel = 'right' if self.motor == 'left' else 'left' rospy.loginfo( 'Place the robot with the %s wheel at the center of the T ' 'and press the central button when the robot is ready', standing_wheel) self.ping() rospy.wait_for_message('buttons/center', std_msgs.msg.Bool) rospy.sleep(3) msg = rospy.wait_for_message('aseba/events/ground', AsebaEvent) self.ths = [d - self.gap for d in msg.data] # count(3) rospy.loginfo('Start calibrating %s motor using thresholds %s', self.motor, self.ths) _n = 1 self.pick = None for motor_speed in self.motor_speeds: rospy.loginfo('Set motor speed to %s', motor_speed) _n += self.target_number_of_samples self.motor_speed = motor_speed if self.motor == 'right': data = [0, self.motor_speed] else: data = [self.motor_speed, 0] self.pub.publish(AsebaEvent(data=data)) while len(self._samples) < _n: rospy.sleep(0.05) self.pub.publish(AsebaEvent(data=[0, 0])) self.sub.unregister() rospy.logdebug('Calculate with samples %s', self._samples) self.save_samples() samples = np.array(self._samples) results = calibrate(samples) rospy.loginfo('Calibration of motor %s done: %s', self.motor, results) self.save_calibration(results)
def on_comm_tx_payload(self, msg: Int16) -> None: msg = AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[msg.data]) self.aseba_set_comm_tx_payload_publisher.publish(msg)
def set_speed(self, values): self.aseba_pub.publish(AsebaEvent(rospy.get_rostime(), 0, values))
def callback(msg): r = int(msg.r * 32) g = int(msg.g * 32) b = int(msg.b * 32) aseba_msg = AsebaEvent(rospy.get_rostime(), 0, [r, g, b]) publisher.publish(aseba_msg)
def on_set_comm(self, msg): self.aseba_set_comm_publisher.publish( AsebaEvent(rospy.get_rostime(), 0, [enabled, payload]))
def on_shutdown_msg(self, msg: Empty) -> None: self.aseba_shutdown_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[]))
def play_system_sound(self, sound: int) -> None: self.aseba_play_system_sound_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[sound]))
def on_comm_enable(self, msg: Bool) -> None: msg = AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=[msg.data]) self.aseba_enable_comm_publisher.publish(msg)
def on_sound_play(self, msg): freq = max(1, int(msg.frequency)) duration = max(1, int(msg.duration.to_sec() * 60)) self.aseba_play_sound_publisher.publish( AsebaEvent(rospy.get_rostime(), 0, [freq, duration]))
def play_system_sound(self, sound): self.aseba_play_system_sound_publisher.publish( AsebaEvent(rospy.get_rostime(), 0, [sound]))
def cb(msg: Bool) -> None: data = [index, 1 if msg.data else 0] self.aseba_led_publisher.publish( AsebaEvent(stamp=self.clock.now().to_msg(), source=0, data=data))
def on_shutdown_msg(self, msg): self.aseba_shutdown_publisher.publish( AsebaEvent(rospy.get_rostime(), 0, []))