def search(self, signal):
direction = random.randint(0, 1)
# slowly rotate for fixed degree and wait for few msec, repeat for x times
sig = ""
for i in range(0, 10):
if signal == 'light':
sig = self.lightSignal
else:
sig = self.irSignal
if self.leftTouchSignal == 1 or self.rightTouchSignal == 1:
self.avoid()
if sig == 1: # no detection
action = Int16MultiArray()
action.data = [3, 24 * direction - 12]
self.pub.publish(action)
time.sleep(0.1)
action = Int16MultiArray()
action.data = [0, 0]
self.pub.publish(action)
time.sleep(0.23)
else:
break
#if no detection after rotating for 10 times, walk forward a little
action = Int16MultiArray()
action.data = [8, 0]
self.pub.publish(action)
time.sleep(0.7)
def avoid(self):
# move backward a little
action = Int16MultiArray()
action.data = [-10, 0]
self.pub.publish(action)
time.sleep(0.15)
# rotate either clockwise or counterclockwise, randomly
direction = random.randint(0, 1)
action = Int16MultiArray()
action.data = [0, 20 * direction - 10]
self.pub.publish(action)
time.sleep(0.15)
def publish_instructions(self,
power1=0,
power2=0,
angle1=0,
angle2=0,
rotation1=0,
rotation2=0):
"""publishing movement parameters for hexapod"""
hex1_array = Int16MultiArray(
data=[self.power1, self.angle1, self.rotation1])
hex2_array = Int16MultiArray(
data=[self.power2, self.angle2, self.rotation2])
self.pub1.publish(hex1_array)
self.pub2.publish(hex1_array)
time.sleep(0.2)
def main():
# parse arguments
inputURI = rospy.get_param('~inputURI', '/dev/video0')
outputURI = rospy.get_param('~outputURI', '')
network = rospy.get_param('~network', 'facenet-120')
overlay = rospy.get_param('~overlay', 'box,labels,conf')
threshold = rospy.get_param('~threshold', 0.5)
# prepare publisher
box_publisher = rospy.Publisher("bounding_box",
Int16MultiArray,
queue_size=10)
# load the object detection network
net = jetson.inference.detectNet(network, threshold)
# create video sources & outputs
input = jetson.utils.videoSource(inputURI)
output = jetson.utils.videoOutput(outputURI)
# process frames until the user exits
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# capture the next image
img = input.Capture()
# detect objects in the image (with overlay)
detections = net.Detect(img, overlay=overlay)
# print the detections
# rospy.loginfo("detected {:d} objects in image".format(len(detections)))
if (len(detections) == 0):
continue
target_detection = detections[0]
rospy.loginfo(target_detection)
# render the image
output.Render(img)
# update the title bar
output.SetStatus("{:s} | Network {:.0f} FPS".format(
network, net.GetNetworkFPS()))
# Publish bounding box
rospy.loginfo("Publishing detection box")
box = Int16MultiArray()
box.data.append(target_detection.ClassID)
box.data.append(target_detection.Left)
box.data.append(target_detection.Top)
box.data.append(target_detection.Right)
box.data.append(target_detection.Bottom)
box_publisher.publish(box)
rate.sleep()
def __init__(self):
print("init")
rospy.init_node('detector', anonymous=True)
self.bridge = CvBridge()
self.pose_pub = rospy.Publisher("detector/pose",
Float32MultiArray,
queue_size=1)
self.pspace_pub = rospy.Publisher("detector/p_space",
Int16MultiArray,
queue_size=1)
self.pspace_id_pub = rospy.Publisher("p_space_id", Int16, queue_size=1)
self.distance = rospy.Publisher("car_dis", Int16, queue_size=1)
self.parking_distance = rospy.Publisher("goal_dis",
Int16,
queue_size=1)
self.image_sub = rospy.Subscriber("/ipm0", Image, self.imageCB)
self.pspace_info = Int16MultiArray()
self.pspace_info.data = []
self.pose_info = Float32MultiArray()
self.pose_info.data = []
self.ego_x = 0.0
self.ego_y = 0.0
self.ego_heading = 0.0
self.Det = Detector()
self.img_w, self.img_h = 1300, 1200
self.center_rescale = np.float32(
[self.img_w / 512., self.img_h / 512.])
self.box_rescale = np.tile(self.center_rescale, (4, 1))
self.prev_center = None
self.pprev_center = None
self.prev_heading = None
print("init end")
def _movement_controller_state_callback(self, req):
'''
Callback function for the movement controller enable/disable service. If true,
the movement controller will make corrections to meet the desired speed and
steering direction with PID controller. Else if disabled, the controller will
simply send PWM values of [0, 0, 0, 0] to the motor driver.
Parameters:
req: The request message (bool) to enable/disable the motor. Type std_srvs/SetBool.
Returns:
response: A response with a string & bool notifiying if the request was valid. Type std_srvs/SetBoolResponse
'''
self.movement_controller_enabled = req.data
response = SetBoolResponse()
response.success = True
if (self.movement_controller_enabled):
response.message = "Movement Controller Enabled"
else:
response.message = "Movement Controller Disabled"
pwm_msg = Int16MultiArray()
pwm_msg.data = [0, 0, 0, 0]
self.pwm_pub.publish(pwm_msg)
return response
def publishData(leftPower, rightPower):
global stampId
stampId += 1
#print stampId, " Published from steering at: ", int(round(time.time() * 1000))
data = Int16MultiArray()
data.data = [leftPower, rightPower]
pub.publish(data)
def set_servos(servo_positions, publisher):
'''
Set servo positions (0.0-180.0/closed-open). ([a, b], pub)
'''
servo_msg = Int16MultiArray()
servo_msg.data = [int(servo_positions[0]), int(servo_positions[1])] # [Thumb, Fingers]
publisher.publish(servo_msg)
def talker():
flag = 1
data_list = Int16MultiArray()
pub = rospy.Publisher('contl_data', Int16MultiArray, queue_size=2)
rospy.init_node('talker', anonymous=True)
while not rospy.is_shutdown():
if (flag == 1):
data_list.data = [25, -60, -60]
flag = 0
pub.publish(data_list)
time.sleep(2)
elif (flag == 2):
data_list.data = [-15, 0, 0]
flag = 0
pub.publish(data_list)
time.sleep(1.0)
elif (flag == 3):
data_list.data = [20, 0, 0]
flag = 0
pub.publish(data_list)
time.sleep(1)
else:
data_list.data = [0, 0, 0]
pub.publish(data_list)
time.sleep(1.0)
def __init__(self, id, NofUWBs, pos):
self.ID = id
self.NofUWBs = NofUWBs
self.pos = pos
self.pub_list = Int16MultiArray()
self.pub_list.data = []
print("Initializing UWB " + str(self.ID) + " with position: " +
str(pos))
rospy.init_node('UWB' + str(self.ID), anonymous=True)
self.UWB_pub = rospy.Publisher("UWBdata",
Int16MultiArray,
queue_size=10)
self.UWB_sub_list = []
for i in range(int(self.NofUWBs)):
if i != self.ID:
self.UWB_sub_list.append(
rospy.Subscriber("UWBdata", Int16MultiArray,
self.callback))
self.pub_list.data.append(self.ID)
for i in range(3):
self.pub_list.data.append(pos[i])
rospy.Timer(rospy.Duration(1. / 0.5), self.timer_callback)
def callback(msg, prevMarkers):
detectedMarkers = msg.markers
# The current time in seconds
now = rospy.get_time()
for detectedMarker in detectedMarkers:
measuredTime = detectedMarker.header.stamp.secs
markerID = detectedMarker.id
prevMarkers[markerID] = measuredTime
detected_markers = list()
for marker in prevMarkers.keys():
if abs(prevMarkers[marker] - now) > 5:
del prevMarkers[marker]
else:
detected_markers.append(marker)
array = MultiArrayDimension()
array.label = 'numMarkers'
array.size = len(detected_markers)
array.size = len(detected_markers)
layout = MultiArrayLayout()
layout.dim = [
array,
]
layout.data_offset = 0
msg = Int16MultiArray()
msg.layout = layout
msg.data = detected_markers
pub.publish(msg)
def talker():
rospy.init_node('control_motor', anonymous=False)
pub = rospy.Publisher('py_control', Int16MultiArray, queue_size=10)
rate = rospy.Rate(10)
b = [0, 0, 0, 0, 0]
while not rospy.is_shutdown():
msg = Int16MultiArray()
i = raw_input()
if (i == "w"):
b = [1, 0, 0, 0, 1]
elif (i == "s"):
b = [0, 1, 0, 0, 1]
elif (i == "a"):
b = [0, 0, 1, 0, 1]
elif (i == "d"):
b = [0, 0, 0, 1, 1]
elif (i == "p"):
if b[4] <= 3:
b[4] += 1
elif (i == "o"):
if b[4] >= 2:
b[4] -= 1
else:
b = [0, 0, 0, 0, 0]
msg.data = b
pub.publish(msg)
rate.sleep()
def execute(self, userdata):
rospy.loginfo('Executing state AtTable')
rospy.Subscriber("serve_status", String, self.callback)
global attable_count
global reached
global ready_to_go
global cur_path
cur_state = robot_state()
cur_state.state = "AtTable"
if len(cur_path) >1:
cur_state.next_goal = cur_path[0]
else:
cur_state.next_goal = 0
pub.publish(cur_state)
attable_count = 0
if ready_to_go == False:
time.sleep(0.1)
return 'wait'
attable_count = 1
ready_to_go = False
print(cur_path[0])
if cur_path[0]==11:
return 'finished'
else:
nav_data = [cur_path[0],cur_path[1]]
nav = Int16MultiArray(data = nav_data)
nav_pub.publish(nav)
nav_pub.publish(nav)
nav_pub.publish(nav)
nav_pub.publish(nav)
cur_path.pop(0)
return 'Done'
def ultrasonic_sensor():
distance_pub = rospy.Publisher('isObstacle', Int16MultiArray, queue_size=1)
flag_pub = rospy.Publisher('isFlagSet', Int16, queue_size=1)
rospy.init_node('ultrasonic_sensor',
anonymous=True) # Initializing the node
sensor_1 = Distance(2, 3)
sensor_2 = Distance(4, 17)
sensor_3 = Distance(27, 22)
sensor_4 = Distance(10, 9)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
reading_1 = sensor_1.distance_from_obj()
reading_2 = sensor_2.distance_from_obj()
reading_3 = sensor_3.distance_from_obj()
reading_4 = sensor_4.distance_from_obj()
threshold_flag = get_threshold_flag(reading_1, reading_2, reading_3,
reading_4)
sensor_reading_array = Int16MultiArray()
sensor_reading_array.data = [
reading_1, reading_2, reading_3, reading_4
]
distance_pub.publish(sensor_reading_array)
flag_pub.publish(threshold_flag)
print("sensor:", sensor_reading_array.data)
print("flags:", threshold_flag)
rate.sleep()
def publishData(power, objectPosition, objectDimension):
global stampId
stampId += 1
#print stampId, " Published from power at: ", int(round(time.time() * 1000))
data = Int16MultiArray()
data.data = [objectPosition, objectDimension, power]
pub.publish(data)
def callback1(self, data):
# Recieve the image
try:
self.cv_image1 = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
cv2.imshow("", self.cv_image1)
cv2.waitKey(1)
# Uncomment if you want to save the image
#cv2.imwrite('image_copy_1.png', self.cv_image1)
#im1=cv2.imshow('window1', self.cv_image1)
# Publish the results
try:
self.image_pub1.publish(
self.bridge.cv2_to_imgmsg(self.cv_image1, "bgr8"))
except CvBridgeError as e:
print(e)
self.red = self.detect_red(self.cv_image1)
self.green = self.detect_green(self.cv_image1)
self.blue = self.detect_blue(self.cv_image1)
self.target = self.detect_target(self.cv_image1)
self.pub = Int16MultiArray()
self.pub.data = np.array([
self.red[0], self.red[1], self.green[0], self.green[1],
self.blue[0], self.blue[1], self.target[0], self.target[1]
])
#Publish the y and z coordinates
self.y_z_pub.publish(self.pub)
def __init__(self):
self.twist_pub = rospy.Publisher('transform', Twist, queue_size=5)
self.light_pub = rospy.Publisher('light', Bool, queue_size=5)
self.motor_values_pub = rospy.Publisher('motor_values', Int16MultiArray, queue_size=5)
self.e_stop_pub = rospy.Publisher('e_stop', Bool, queue_size=5)
self.prev_light_button_state = self.light_toggle = self.e_stop = False
self.twist_msg = Twist()
self.motor_values_msg = Int16MultiArray()
self.motor_values_msg.layout.dim = [MultiArrayDimension('data', 1, 9)]
self.light_toggle_msg = Bool()
self.light_toggle_msg = False
self.e_stop_msg = Bool()
self.e_stop_msg = False
# direction vectors for each of the motors
self.front_left_dir = [math.sqrt(2) / 2, math.sqrt(2) / 2]
self.front_right_dir = [-math.sqrt(2) / 2, math.sqrt(2) / 2]
self.back_left_dir = [math.sqrt(2) / 2, -math.sqrt(2) / 2]
self.back_right_dir = [-math.sqrt(2) / 2, -math.sqrt(2) / 2]
self.vt = [0.0, 0.0]
self.w = 0.0
self.front_left_vel = self.front_right_vel = self.back_left_vel = self.back_right_vel = 0.0
self.front_up_vel = self.back_up_vel = 0.0
self.claw_grab = self.claw_rotation = self.claw_bottom = 0.0
self.motor_values = [0, 0, 0, 0, 0, 0, 0, 0, 0]
def rec_thread(quit_event):
global chunk_lock, chunk_buffer, azimuth_global
conn, addr = s.accept()
pubFrameRaw = rospy.Publisher('sound_raw', Int16MultiArray, queue_size=1)
while not rospy.is_shutdown():
# con_start = rospy.Time.now()
# It only takes 0.00011 secs to transfer
# 32768 bits from respeaker to robots through Ethernet
chunk_buffer = conn.recv(32768, socket.MSG_WAITALL)
# con_end = rospy.Time.now()
chunk_time = rospy.Time.now()
# print("Receive")
# print("con time: ", (con_end-con_start).to_sec())
# print("==========================")
xx = np.frombuffer(chunk_buffer, 'int16')
# print("The xx: ", xx)
# print("xx len: ", xx.size)
s_raw_msg = Int16MultiArray()
s_raw_msg.data = xx
pubFrameRaw.publish(s_raw_msg)
chunk_lock = False
conn.sendall(str(int(np.floor(azimuth_global))).encode('utf-8'))
conn.close()
def watcher(data):
if data is None:
return
msg = Int16MultiArray()
msg.data = data
msg.layout = gen_layout(data.shape)
self.publisher.publish(msg)
def callback1(data):
global cur_x
global cur_y
global flag
max_pwm = 100
# read current angles
for i in range(0, num_of_joints):
cur_x[i] = data.data[2 * i]
cur_y[i] = data.data[2 * i + 1]
# only start publishing after the first desired angles publish
if flag:
max_angle = 35
# the main calculation
w_len = PID()
# if one of the angles exceeds maximum angle, zero all
for i in range(0, num_of_joints):
if (abs(cur_x[i]) > max_angle) or (abs(cur_y[i]) > max_angle):
w_len = np.zeros(num_of_joints * 3)
# if one of the pwm values exceeds a max val, reduce it to max val and all other values in ratio
for i in range(0, 9):
pwm_val = abs(w_len[i])
if pwm_val > max_pwm:
for j in range(0, 9):
w_len[j] = w_len[j] * max_pwm / pwm_val
w_len = np.around(w_len)
msg = Int16MultiArray()
msg.data = w_len
pub.publish(msg)
def run(self):
start = time.time()
image = cv2.resize(self.cv_image,
(self.input_shape[0], self.input_shape[1]))
tmp_inp = np.array(image, dtype=np.uint8)
tmp_inp = np.expand_dims(tmp_inp, 0)
preds = model.predict(tmp_inp)[0]
top_preds = np.argsort(preds)[::-1][0:predictions_to_return]
# print('\n' + '--PREDICTED SCENE CATEGORIES:')
# output the prediction
pub_num = Int16MultiArray()
prob = Float32MultiArray()
for i in range(0, 3):
# if preds[top_preds[i]] > 0.1:
print classes[top_preds[i]],
prob.data.append(
Decimal(str(preds[top_preds[i]])).quantize(
Decimal('0.000001'), rounding=ROUND_HALF_UP))
pub_num.data.append(top_preds[i])
print ''
# print 'processing time:', time.time() - start
pub_num.layout.data_offset = 5
self.pub_labels.publish(pub_num)
self.pub_prob.publish(prob)
def onButton7(event):
print "7"
pub_array = np.array([7])
msg = Int16MultiArray()
msg.data = pub_array
pub.publish(msg)
rate.sleep()
def callback2(self,data):
# Recieve the image
try:
self.cv_image2 = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Uncomment if you want to save the image
#cv2.imwrite('image_copy_2.png', self.cv_image2)
im2=cv2.imshow('window2', self.cv_image2)
cv2.waitKey(1)
# Publish the results
try:
self.image_pub2.publish(self.bridge.cv2_to_imgmsg(self.cv_image2, "bgr8"))
except CvBridgeError as e:
print(e)
red = self.detect_red(self.cv_image2)
green = self.detect_green(self.cv_image2)
blue = self.detect_blue(self.cv_image2)
target = self.detect_target(self.cv_image2)
yellow = self.detect_yellow(self.cv_image2)
self.pub = Int16MultiArray()
self.pub.data = np.array([red[0], red[1], green[0],green[1],
blue[0], blue[1],yellow[0], yellow[1],
target[0],target[1]])
# Publish the x and z coordinates
self.cam2_pub.publish(self.pub)
def talker():
pub = rospy.Publisher('get_speed_from_random',
Int16MultiArray,
queue_size=10)
rospy.init_node('get_speed_from_random', anonymous=True)
rate = rospy.Rate(0.1)
while not rospy.is_shutdown():
speed1 = random.randint(200, 250)
speed2 = random.randint(200, 250)
speed1_boolean = random.randint(0, 1)
speed2_boolean = random.randint(0, 1)
if speed1_boolean == 1:
speed1 = -speed1
if speed2_boolean == 1:
speed2 = -speed2
var_from_the_phone = Int16MultiArray()
var_from_the_phone.data = [speed1, speed2]
rospy.loginfo(var_from_the_phone)
pub.publish(var_from_the_phone)
rate.sleep()
def convert_np_vector_to_int16_multi_array(vector):
assert len(vector.shape) == 1
N = vector.shape[0]
msg = Int16MultiArray(layout=MultiArrayLayout(
dim=[MultiArrayDimension(label="", size=N, stride=1)], data_offset=0),
data=vector.astype(int).tolist())
return msg
def loop(self, args):
state_file = None
if len(args):
state_file = args[0]
# periodic reports
count = 0
# safety dropout if receiver not present
dropout_data_r = -1
dropout_count = 3
# loop
while not rospy.core.is_shutdown():
# check state_file
if not state_file is None:
if not os.path.isfile(state_file):
break
# if we've received a report
if self.buffer_total > 0:
# compute amount to send
buffer_rem = self.buffer_total - self.buffer_space
n_bytes = BUFFER_STUFF_BYTES - buffer_rem
n_bytes = max(n_bytes, 0)
n_bytes = min(n_bytes, MAX_STREAM_MSG_SIZE)
# if amount to send is non-zero
if n_bytes > 0:
msg = Int16MultiArray(
data=self.data[self.data_r:self.data_r + n_bytes])
self.pub_stream.publish(msg)
self.data_r += n_bytes
# break
if self.data_r >= len(self.data):
break
# report once per second
if count == 0:
count = 10
print "streaming:", self.data_r, "/", len(self.data), "bytes"
# check at those moments if we are making progress, also
if dropout_data_r == self.data_r:
if dropout_count == 0:
print "dropping out because of no progress..."
break
print "dropping out in", str(dropout_count) + "..."
dropout_count -= 1
else:
dropout_data_r = self.data_r
# count tenths
count -= 1
time.sleep(0.1)
def execute(self, action):
speed_wheel = Int16MultiArray()
rospy.loginfo("%s", action.direction)
# rospy.loginfo("Server get action %s", action.angle)
if action.direction == "backward":
speed_wheel.data = [-100, -100]
if action.direction == "forward":
speed_wheel.data = [100, 100]
elif action.direction == "stop":
speed_wheel.data = [0, 0]
elif action.direction == "backward":
speed_wheel.data = [-100, -100]
elif action.direction == "rotate":
if action.side == "right":
speed_wheel.data = [100, -100]
elif action.side == "left":
speed_wheel.data = [-100, 100]
speed_wheel = self.modificator_direction(speed_wheel, action)
# rospy.loginfo(speed_wheel)
self.change_directionToArduino(speed_wheel, action)
self.result = setDirectionRobotResult(done=True)
self.server.set_succeeded(self.result)
def microphone_listener_node():
global flag
rospy.init_node('microphone_listener_node')
pub = rospy.Publisher('/audio/mic_samples',
Int16MultiArray,
queue_size=800)
rospy.Subscriber('/audio/detection_flag', Bool, flag_callback)
#Block samples / second
rate = rospy.Rate(int(RATE))
#Start audio stream from microphone
p = pyaudio.PyAudio()
stream = p.open(format=p.get_format_from_width(WIDTH),
channels=CHANNELS,
rate=RATE,
input=True,
output=False)
sample = Int16MultiArray()
while not rospy.is_shutdown():
if flag:
#Samples microphone input
input_string = stream.read(BLOCKSIZE, exception_on_overflow=False)
input_value = struct.unpack('h' * BLOCKSIZE, input_string)
sample.data = input_value
pub.publish(sample)
else:
rate.sleep()
#Closes input stream
stream.stop_stream()
stream.close()
p.terminate()
def publish_interpolation_pos():
msg = Int16MultiArray()
try:
msg.data = np.array(next(control.interpolation), dtype=np.int16)
except StopIteration:
msg.data = [0]
control.reset()
interpolation_pub.publish(msg)
def talker():
data_list = Int16MultiArray()
pub = rospy.Publisher('Testmessage', Int16MultiArray, queue_size=1)
rospy.init_node('talker', anonymous=True)
while not rospy.is_shutdown():
data_list.data = [3, 10]
pub.publish(data_list)
time.sleep(5)
