def __load_map(self, map_name): logger.info('Loading Map: {}'.format(map_name)) # Set as the current map self.__map_name = str(map_name) try: map_obj = Map.objects(name=self.__map_name).get() # type: Map self.__active_map_pub.publish(map_obj.get_map_info_msg()) grid = map_obj.get_occupancy_grid_msg() self.__og_pub.publish(grid) if map_obj.map_data: self.__data_pub.publish( UInt8MultiArray(data=map_obj.map_data.read())) else: self.__data_pub.publish(UInt8MultiArray()) self.__marker_pub.publish(build_marker_array(map_obj)) except Exception as e: logger.error('Exception publishing data: {} - {}'.format( e, traceback.format_exc()))
def test_std_msgs_MultiArray(self): # multiarray is good test of embed plus array type from std_msgs.msg import Int32MultiArray, MultiArrayDimension, MultiArrayLayout, UInt8MultiArray dims = [MultiArrayDimension('foo', 1, 2), MultiArrayDimension('bar', 3, 4),\ MultiArrayDimension('foo2', 5, 6), MultiArrayDimension('bar2', 7, 8)] for d in dims: self.assertEquals(d, d) # there was a bug with UInt8 arrays, so this is a regression # test. the buff was with the uint8[] type consistency buff = StringIO() self.assertEquals(UInt8MultiArray(), UInt8MultiArray()) self.assertEquals('', UInt8MultiArray().data) UInt8MultiArray().serialize(buff) self.assertEquals(UInt8MultiArray(layout=MultiArrayLayout()), UInt8MultiArray()) UInt8MultiArray(layout=MultiArrayLayout()).serialize(buff) data = ''.join([chr(i) for i in range(0, 100)]) v = UInt8MultiArray(data=data) self._test_ser_deser(UInt8MultiArray(data=data), UInt8MultiArray()) self.assertEquals(Int32MultiArray(), Int32MultiArray()) self.assertEquals(Int32MultiArray(layout=MultiArrayLayout()), Int32MultiArray()) self.assertEquals( Int32MultiArray(layout=MultiArrayLayout(), data=[1, 2, 3]), Int32MultiArray(data=[1, 2, 3])) self.assertEquals(Int32MultiArray(layout=MultiArrayLayout(), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(),data=[1, 2, 3])) self.assertEquals(Int32MultiArray(layout=MultiArrayLayout(dim=[]), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(),data=[1, 2, 3])) self.assertEquals(Int32MultiArray(layout=MultiArrayLayout([], 0), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(),data=[1, 2, 3])) self.assertEquals(Int32MultiArray(layout=MultiArrayLayout(dim=[], data_offset=0), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(),data=[1, 2, 3])) self.assertEquals(Int32MultiArray(layout=MultiArrayLayout(dim=dims, data_offset=0), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(dim=dims),data=[1, 2, 3])) self.assertEquals(Int32MultiArray(layout=MultiArrayLayout(dims, 10), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(dim=dims,data_offset=10),data=[1, 2, 3])) self.assertNotEquals(Int32MultiArray(data=[1, 2, 3]), Int32MultiArray(data=[4, 5, 6])) self.assertNotEquals(Int32MultiArray(layout=MultiArrayLayout([], 1), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout([], 0),data=[1, 2, 3])) self.assertNotEquals(Int32MultiArray(layout=MultiArrayLayout([], 1), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(dim=[]),data=[1, 2, 3])) self.assertNotEquals(Int32MultiArray(layout=MultiArrayLayout(dims, 10), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(dim=dims,data_offset=11),data=[1, 2, 3])) self.assertNotEquals(Int32MultiArray(layout=MultiArrayLayout(dim=dims, data_offset=10), data=[1, 2, 3]),\ Int32MultiArray(layout=MultiArrayLayout(dim=dims[1:],data_offset=10),data=[1, 2, 3])) self._test_ser_deser(Int32MultiArray(), Int32MultiArray()) self._test_ser_deser(Int32MultiArray(layout=MultiArrayLayout()), Int32MultiArray()) self._test_ser_deser(Int32MultiArray(data=[1, 2, 3]), Int32MultiArray())
def buildFormationMessage(self, formation): # formation-related matrices pts = self.getPoints(formation) adjmat = np.array(formation['adjmat'], dtype=np.uint8) msg = Formation() msg.name = formation['name'] # formation points for pt in pts: msg.points.append(Point(*pt)) # adjacency matrix for this formation group msg.adjmat = UInt8MultiArray() msg.adjmat.data = adjmat.flatten().tolist() msg.adjmat.layout.dim.append(MultiArrayDimension()) msg.adjmat.layout.dim.append(MultiArrayDimension()) msg.adjmat.layout.dim[0].label = "rows" msg.adjmat.layout.dim[0].size = adjmat.shape[0] msg.adjmat.layout.dim[0].stride = adjmat.size msg.adjmat.layout.dim[1].label = "cols" msg.adjmat.layout.dim[1].size = adjmat.shape[1] msg.adjmat.layout.dim[1].stride = adjmat.shape[1] # should we include formation gains in our message? if self.send_gains: # pre-calculated gains may have been provided, but not required. # Store the gains so we do not need to redo work. if 'gains' not in formation: formation['gains'] = createGainMatrix(adjmat, pts, method='original') # # Print gain matrix for easy copying into formations.yaml # np.set_printoptions(linewidth=500, threshold=sys.maxsize) # print(np.array2string(formation['gains'], separator=', ', # formatter={'float_kind':lambda x: "% 0.4f" % x})) A = formation['gains'] # pack up the gains into the message gains = np.array(A, dtype=np.float32) msg.gains = UInt8MultiArray() msg.gains.data = gains.flatten().tolist() msg.gains.layout.dim.append(MultiArrayDimension()) msg.gains.layout.dim.append(MultiArrayDimension()) msg.gains.layout.dim[0].label = "rows" msg.gains.layout.dim[0].size = gains.shape[0] msg.gains.layout.dim[0].stride = gains.size msg.gains.layout.dim[1].label = "cols" msg.gains.layout.dim[1].size = gains.shape[1] msg.gains.layout.dim[1].stride = gains.shape[1] msg.header.stamp = rospy.Time.now() return msg
def __init__(self): super(InterfaceController, self).__init__() rospy.init_node(NODE_NAME) self.port = rospy.get_param("~port", DEFAULT_PORT) self.baud = rospy.get_param("~baud", DEFAULT_BAUD) self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) self.serial_device = serial.Serial(port=self.port, baudrate=self.baud) self.control_subscriber = rospy.Subscriber( CONTROL_TOPIC, UInt8MultiArray, self.control_requested_callback) self.command_queue = [] temp_arary = [] for _ in range(18): temp_arary.append(chr(255)) temp_arary.append(chr(165)) temp_arary.append(chr(0)) self.command_queue.append(UInt8MultiArray(data=temp_arary)) # ########### Start class ########## self.run()
def __exit__(self, type, value, traceback): if type is None: # do the drawing onto the device pixels = UInt8MultiArray() bits = list(self.image.convert("1").getdata()) vals = list() w, h = self.image.size for i in range(0, h / 8): for idx in reversed(range(0, w)): stop = (i + 1) * 8 * w - 1 start = (stop + 1) - 8 * w + idx vals.append( int( ''.join([str(mli) for mli in bits[start:stop:w]])[::-1], 2)) pixels.layout.dim.append(MultiArrayDimension()) pixels.layout.dim[0].label = "image" pixels.layout.dim[0].size = (w * h) / 8 pixels.layout.dim[0].stride = (w * h) / 8 pixels.layout.data_offset = 0 pixels.data = vals self.pub.publish(pixels) del self.draw # Tidy up the resources return False # Never suppress exceptions
def main(): #angles = rospy.getParam("joint_angles") angles = [0] * 6 pub = rospy.Publisher('joint_array', UInt8MultiArray, queue_size=10) rospy.init_node('publish_angle_node') rate = rospy.Rate(0.2) # 10hz a = 0 while not rospy.is_shutdown(): array = UInt8MultiArray() array.data = [] angles[0] = 90 #random.randrange(1,179) angles[1] = 90 #random.randrange(16,164) angles[2] = 90 #random.randrange(1,179) angles[3] = 90 #random.randrange(1,179) angles[4] = 90 #random.randrange(1,179) if a % 2 == 0: angles[5] = 20 #random.randrange(11,72) else: angles[5] = 60 #random.randrange(11,72) for i in range(6): array.data.append(angles[i]) pub.publish(array) rate.sleep() a += 1
def _j1939_callback(self, msg): data_bytes = bytearray(msg.data) sa_array = (0x2e, 0xd0, 0x1e, 0x80, 0x81, 0x82, 0x83) estop_send = UInt8MultiArray() index = 0 for sa in sa_array: if sa == msg.source: self.estop_array[index] = data_bytes[1] estop_send.data = self.estop_array self._publisher.publish(estop_send) index += 1 if 1 in self.estop_array and self.reset_flag == 0: estop_str = 'ESTOP ACTIVE' emstop_int = 0x01 elif 1 in self.estop_array and self.reset_flag == 1: estop_str = 'RESET ACTIVE' emstop_int = 0x02 else: estop_str = 'ALL CLEAR' self.reset_flag = 0 emstop_int = 0x00 self._publisher2.publish(estop_str) self.transmit_estop_j1939(emstop_int)
def sendAssignmentCb(self, event=None): """ This is only used for comparison to distributed ACLswarm assignment. Note: Although this callback has its own period, assignments will note take effect until every `autoauction_dt`. See coordination.launch. """ # do we have all the data we need? if None in self.poses or self.formidx == -1: return # construct matrix of swarm positions (3xn) q = np.array([(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) for msg in self.poses]).T p = self.getPoints(self.formations['formations'][self.formidx]).T # use global swarm state to find optimal assignment via Hungarian self.P, _ = find_optimal_assignment(q, p, last=self.P) # for n = 15, takes 5-10 ms # Publish to the swarm msg = UInt8MultiArray() msg.data = self.P self.pub_assignment.publish(msg)
def handle_face_recognize(req): rsp = face_recognizeResponse() bridge = CvBridge() ro_msg = UInt8MultiArray() try: for im in req.images_in: cv_image = bridge.imgmsg_to_cv2(im, "bgr8") # Leave image preprocess to client result = ps.process(cv_image) name_id_msg = UInt8 if result[1] > 0.9: name_id_msg.data = result[0] + 1 # known face id start with 1 else: name_id_msg.data = 0 # unknown face has id 0 ro_msg.data.append(name_id_msg) # establish a dict to find the name refereed by the id rsp.face_label_ids = ro_msg return rsp except CvBridgeError as e: print(e) return rsp
def publish(data): """Publishes PWM values to the microcontroller""" dataOut = UInt8MultiArray() dataOut.data = [data[0],data[1],data[2],data[3]] rospy.loginfo("PWM published: %s\n ", dataOut.data) pub.publish(dataOut)
def request_robot_status(self): data_out = pack_rcmd_bytes() rsqvel_msg = UInt8MultiArray() rsqvel_msg.layout.dim = len(data_out) rsqvel_msg.data = data_out self.pub_cmd_to_slave.publish(rsqvel_msg)
def pwmControl(): data1 = int(e1.get()) data2 = int(e2.get()) data3 = int(e3.get()) data4 = int(e4.get()) dataOut = UInt8MultiArray() dataOut.data = [data1, data2, data3, data4] rospy.loginfo("PWM published: %s\n ", dataOut) pwm.publish(dataOut)
def set_rgbled(self, value): """ Value is a 3-part tuple representing the RGB values of the color, each value being an integer in the range 0-254. """ assert len(value) == 3 for i, v in enumerate(value): assert 0 <= v <= 254 #get_service_proxy(c.HEAD, c.ID_LED)(i, v) self.head_rgb_set_pub.publish(UInt8MultiArray(data=value))
def send_wheel_cmdvel(self, cmdvel_x, cmdvel_y): self.cmdvel_x = cmdvel_x self.cmdvel_y = cmdvel_y data_out = pack_wcmd_bytes(self.cmdvel_x, self.cmdvel_y) cmdvel_msg = UInt8MultiArray() cmdvel_msg.layout.dim = len(data_out) cmdvel_msg.data = data_out self.pub_cmd_to_slave.publish(cmdvel_msg)
def show_color(self, color): message = UInt8MultiArray() values = [] for _ in range(18): values.append(color[0]) values.append(color[1]) values.append(color[2]) message.data = values self.led_controller_publisher.publish(message)
def _j1939_callback(self, msg): data_bytes = bytearray(msg.data) array = UInt8MultiArray() array.data = [0, 1] if data_bytes[1] == 0: self._publisher.publish(array) else: self._publisher.publish(array)
def test_byte_array(self): msg = UInt8MultiArray(data=[0, 1, 2]) extracted = extract_cbor_values(msg) data = extracted['data'] self.assertEqual(type(data), bytes) for i, val in enumerate(msg.data): if PYTHON2: self.assertEqual(ord(data[i]), val) else: self.assertEqual(data[i], val)
def talker(): pub = rospy.Publisher('/joint_array', UInt8MultiArray, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): msg = UInt8MultiArray() msg.layout = [] msg.data = [5, 74, 28, 6, 26, 73] rospy.loginfo(msg) pub.publish(msg) rate.sleep()
def nrfPublisher(): nrfSetup() nrfPub = rospy.Publisher('radioControlSignal', UInt8MultiArray, queue_size=5) rospy.init_node('nrfPublisher', anonymous=True) rate = rospy.Rate(20) # 20hz radioMsg = UInt8MultiArray() radioMsg.layout.data_offset = 4 while not rospy.is_shutdown(): radioReceive() radioMsg.data = [genericAngle,throttle,flareFlag,autonomousFlag] nrfPub.publish(radioMsg) rate.sleep()
def callback(self, data): rate = rospy.Rate(10) self.axe = data.axes self.button = data.buttons self.joy = self.axe + self.button seg_axes = [] enc_axes = [] ## processing axes for i in range(0, 8): tmp = self.joy[i] if tmp >= self.boundary: tmp = 1 elif tmp < self.boundary and tmp > -self.boundary: tmp = 0 elif tmp <= -self.boundary: tmp = -1 seg_axes.append(tmp) if seg_axes[2] <= 1 and seg_axes[2] > -1: seg_axes[2] = 0 elif seg_axes[2] == -1: seg_axes[2] = 1 if seg_axes[5] <= 1 and seg_axes[5] > -1: seg_axes[5] = 0 elif seg_axes[5] == -1: seg_axes[5] = 1 tmp2 = 0 for i in range(0, 8): if seg_axes[i] == 0: tmp2 = 0 elif seg_axes[i] == 1: tmp2 = 1 elif seg_axes[i] == -1: tmp2 = 2 enc_axes.append(tmp2) ## encoding axes self.sum_axe_front = enc_axes[0] * 64 + enc_axes[1] * 16 + enc_axes[ 3] * 4 + enc_axes[4] * 1 self.sum_axe_below = enc_axes[2] * 64 + enc_axes[5] * 16 + enc_axes[ 6] * 4 + enc_axes[7] * 1 self.sum_button = self.joy[8] * 64 + self.joy[9] * 32 + self.joy[ 10] * 16 + self.joy[11] * 8 + self.joy[15] * 4 + self.joy[ 14] * 2 + self.joy[16] * 1 joy_cmddd = UInt8MultiArray() joy_cmddd.data = [ self.sum_button, self.sum_axe_front, self.sum_axe_below ] self.pub_joycmd.publish(joy_cmddd) rate.sleep()
def main(): global sensor_array_24 global sensor_array_22 sensor_array_24 = UInt8MultiArray() sensor_array_22 = UInt8MultiArray() sensor_array_24.data = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] #24 sensor_array_22.data = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] #22 rate = rospy.Rate(10) #10hz time.sleep(1) while not rospy.is_shutdown(): try: trigger_test() rate.sleep() except Exception: rospy.logerr(sys.exc_info()) rate.sleep() return
def __init__(self): ## neopixel mode ## self.neopixel_mode_msg = UInt8() self.neopixel_mode_publisher = rospy.Publisher("neopixel_mode", UInt8, queue_size=10) ## neopixel rgb : [r0,g0,b0, r1,g1,b1, r2,g2,b2] ## self.neopixel_rgb_msg = UInt8MultiArray() self.neopixel_rgb_publisher = rospy.Publisher("neopixel_rgb", UInt8MultiArray, queue_size=10) rospy.init_node("last_neopixel")
def __init__(self): rospy.init_node('gimbal_aiming', anonymous=True) self.init_detection() rospy.Subscriber('fiducial_transforms', FiducialTransformArray, self._transform_callback) self._gimbal_pub = rospy.Publisher('enemy_pos', EnemyPos, queue_size=1) self._shoot_pub = rospy.Publisher('shoot', UInt8MultiArray, queue_size=1) rate = rospy.Rate(20) # 10hz self._cnt = 0 self._out_cnt = 0 self._last_id = 0 self._shoot_dict = Counter() self.enemy_dist = 0 self.enemy_pitch = 0 self.enemy_yaw = 0 self.enable_shoot = False print 'init...' self._init_cnt = 0 while not rospy.is_shutdown(): enemy_pos = EnemyPos() shoot_msg = UInt8MultiArray(data=[0, 0, 1, 100]) if self._init_cnt < 50 and self.enable_shoot: self._init_cnt += 1 self._gimbal_pub.publish(enemy_pos) self._shoot_pub.publish(shoot_msg) rate.sleep() continue if self.enemy_id == 0: self._cnt = 0 self._out_cnt += 1 if self._out_cnt > 20: self.enemy_dist = 0 self.enemy_pitch = 0 self.enemy_yaw = 0 else: self._cnt = self._cnt + 1 if self.enemy_z > 5.0 or self.enemy_z == 0 or self._cnt < 5 or self._shoot_dict[str(self.enemy_id)] > 10: pass else: self._out_cnt = 0 #self._shoot_dict[str(self.enemy_id)] += 1 self.enemy_dist, self.enemy_pitch, self.enemy_yaw = self._calc_gimbal(self.enemy_x, self.enemy_y, self.enemy_z) shoot_msg.data = [1, 0, 1, 100] enemy_pos.enemy_dist = float(self.enemy_dist) enemy_pos.enemy_pitch = float(self.enemy_pitch) enemy_pos.enemy_yaw = float(self.enemy_yaw) print enemy_pos, shoot_msg self._gimbal_pub.publish(enemy_pos) if not self.enable_shoot: shoot_msg.data = [ 0, 0, 0, 0] self._shoot_pub.publish(shoot_msg) rate.sleep()
def updateValue(self): """Check the Pobject values for changes (since the previous simulation step) and publish a message if changes have occured NOTE: the function considers all pObject values >= 1 to be 1 and otherwise 0""" if (self.isNewValue()): newValue = UInt8MultiArray() newValue.data = [0] * 10 # process only LEDs that have been included in 'cmd_led' group for i, led in enumerate(self.ledNumbers): newValue.data[led] = 1 if (self.pObjects[i].value >= 1) else 0 #publish the newly constructed value self.publisher.publish(newValue) Effector.updateValue(self)
def callback(data): joy_cmddd = UInt8MultiArray() axe = data.axes button = data.buttons #joy_cmddd = UInt8MultiArray() joy = axe + button seg_axes = [] enc_axes = [] boundary = 0.5 for i in range(0, 8): tmp = joy[i] if tmp >= boundary: tmp = 1 elif tmp < boundary and tmp > -boundary: tmp = 0 elif tmp <= -boundary: tmp = -1 seg_axes.append(tmp) if seg_axes[2] <= 1 and seg_axes[2] > -1: seg_axes[2] = 0 elif seg_axes[2] == -1: seg_axes[2] = 1 if seg_axes[5] <= 1 and seg_axes[5] > -1: seg_axes[5] = 0 elif seg_axes[5] == -1: seg_axes[5] = 1 for i in range(0, 8): if seg_axes[i] == 0: tmp2 = 0 elif seg_axes[i] == 1: tmp2 = 1 elif seg_axes[i] == -1: tmp2 = 2 enc_axes.append(tmp2) sum_axe_front = enc_axes[0] * 64 + enc_axes[1] * 16 + enc_axes[ 3] * 4 + enc_axes[4] * 1 sum_axe_below = enc_axes[2] * 64 + enc_axes[5] * 16 + enc_axes[ 6] * 4 + enc_axes[7] * 1 sum_button = joy[8] * 64 + joy[9] * 32 + joy[10] * 16 + joy[11] * 8 + joy[ 15] * 4 + joy[14] * 2 + joy[16] * 1 joy_cmddd.data = [sum_button, sum_axe_front, sum_axe_below] pub = rospy.Publisher('joy_commands', UInt8MultiArray, queue_size=10) pub.publish(joy_cmddd)
def detect_and_publish(self, image): shapes_list = self.species_detector.process(image, image_scaling_factor=0.6, debug=False) species_counts = [x * 0 for x in range(0, 4)] for shape_info in shapes_list: shape_type = shape_info[1][0] if shape_type == species_detector.ShapeType.triangle: species_counts[0] += 1 elif shape_type == species_detector.ShapeType.rectangle: species_counts[1] += 1 elif shape_type == species_detector.ShapeType.square: species_counts[2] += 1 elif shape_type == species_detector.ShapeType.circle: species_counts[3] += 1 species_counts = list(map(lambda count: UInt8(count), species_counts)) species_msg = UInt8MultiArray(data=species_counts) species_pub.publish(species_msg)
def timer_callback(self): if self.start_timer is not None and not self.first_time: print('first') self.first_time = True elif self.first_time: print('second') self.start_timer.cancel() self.start_timer = None self.timer = self.create_timer(UPDATE_PERIOD, self.timer_callback) self.tf_viz_tim = self.create_timer(0.1, self.tf_viz_callback) start = self.get_clock().now() # diff = start.nanoseconds - self.prev_time.nanoseconds # point_i = int(SERVO_FREQ * diff / (10 ** 9)) # print(diff/10**9, point_i) # if point_i >= len(self.prev_trajectory): # point_i = len(self.prev_trajectory) - 1 # print(point_i) current_point = self.prev_trajectory[-1] new_trajectory = self.trajectory_generator.generate_trajectory(current_point) inversed_trajectory = [ self.inv_kin_calc.calc_point(point) for point in new_trajectory ] data = self.trajectory_encoder.encode_trajectory(inversed_trajectory) cmd = self.SET_SERVOS_NOW if self.first_time else self.SET_SERVOS_LOOP data = bytearray([cmd]) + data spi_msg = UInt8MultiArray(data=data) self.spi_bridge.publish(spi_msg) self.prev_trajectory.extend(new_trajectory) front_point = self.path_proxy.first_unused() triangle = self.inv_kin_calc.triangle(front_point, left_side=False) self.publish_transform('odom', 'front_point', front_point) self.publish_transform('odom', 'tri_a', triangle[0]) self.publish_transform('odom', 'tri_b', triangle[1]) self.publish_transform('odom', 'tri_c', triangle[2]) elapsed = self.get_clock().now() - start print(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s') if self.start_timer is None: self.first_time = False
def __init__(self): ### Neopixel (read manual at https://docs.google.com/spreadsheets/d/1sDBpzWSkR-TJ_CMlEihfyFtKzg1GNOJjjiK5VN6d-MU/edit#gid=0) ### ## neopixel mode ## self.neopixel_mode_msg = UInt8() self.neopixel_mode_publisher = rospy.Publisher("neopixel_mode", UInt8, queue_size=10) ## neopixel rgb : [r0,g0,b0, r1,g1,b1, r2,g2,b2] ## self.neopixel_rgb_msg = UInt8MultiArray() self.neopixel_rgb_publisher = rospy.Publisher("neopixel_rgb", UInt8MultiArray, queue_size=10) ## neopixel time ## self.neopixel_time_msg = UInt16MultiArray() self.neopixel_time_publisher = rospy.Publisher("neopixel_time_ms", UInt16MultiArray, queue_size=10) ## neopixel number of led ## self.neopixel_num_msg = UInt16MultiArray() self.neopixel_num_publisher = rospy.Publisher("neopixel_num", UInt16MultiArray, queue_size=10) ## neopixel number of step in run mode ## self.neopixel_step_msg = UInt16() self.neopixel_step_publisher = rospy.Publisher("neopixel_step", UInt16, queue_size=10) ### ### ### self.joy_sub = None self.neopixel_mode = 0 self.r = 20 self.g = 40 self.b = 90 self.prv_up = 0 self.prv_down = 0 self.prv_left = 0 self.prv_right = 0 self.charge_percent = 0 self.charge_mode = False self.low_battery_mode = False self.wait_mode_change = False
def timer_callback(self): if self.first_time: self.first_time = False self.start_timer.cancel() self.timer = self.create_timer(UPDATE_PERIOD, self.timer_callback) start = self.get_clock().now() diff = start.nanoseconds - self.prev_time.nanoseconds point_i = int(SERVO_FREQ * diff / (10 ** 9)) print(diff/10**9, point_i) if point_i >= len(self.prev_trajectory): point_i = len(self.prev_trajectory) - 1 print(point_i) current_point = self.prev_trajectory[point_i] new_trajectory = self.trajectory_generator.generate_trajectory(current_point) inversed_trajectory = [ self.inv_kin_calc.calc_point(point) for point in new_trajectory ] data = self.trajectory_encoder.encode_trajectory(inversed_trajectory)[1:] computation_time = self.get_clock().now() - start cmd_size = 3 + 18 * 2 point_k = int(SERVO_FREQ * computation_time.nanoseconds / (10 ** 9)) points_passed = min(point_i + point_k, len(self.prev_trajectory) - 1) - point_i print(points_passed) data = bytearray([42]) + data[points_passed * cmd_size:(points_passed + 250 * 2) * cmd_size] spi_msg = UInt8MultiArray(data=data) self.spi_bridge.publish(spi_msg) self.prev_time = start self.prev_trajectory = new_trajectory[points_passed:points_passed + 250 * 2] front_point = self.path_proxy.first_unused() triangle = self.inv_kin_calc.triangle(front_point, left_side=False) self.publish_transform('odom', 'front_point', front_point) self.publish_transform('odom', 'tri_a', triangle[0]) self.publish_transform('odom', 'tri_b', triangle[1]) self.publish_transform('odom', 'tri_c', triangle[2]) elapsed = self.get_clock().now() - start # self.get_logger().debug(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s') print(f'Elapsed {elapsed.nanoseconds / 10**9:.3f}s')
def callback(self, data): axes = data.axes sL = int(axes[5] * 255.) sR = int(axes[1] * 255.) dL = 0 dR = 1 if sL < 0: dL = 1 sL = -sL if sR < 0: dR = 0 sR = -sR pub_msg = UInt8MultiArray() pub_msg.data = [sL, sR, dL, dR] self.pub.publish(pub_msg)