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)
