def publish_imu(self):
msg = String()
val = "temp: {} euler: {} grav: {}".format(self.sensor.temperature,
self.sensor.euler,
self.sensor.gravity)
msg.data = "" + val
self.imu_publisher.publish(msg)
def test_add_filter():
filter = AddFilter()
parser = argparse.ArgumentParser('add')
filter.add_arguments(parser)
args = parser.parse_args(
['-t', '/data', '--type', 'example_interfaces/msg/String', '-v', 'test/data.yaml',
'--align-to', '/align'])
filter.set_args(None, args)
topic_metadata = TopicMetadata(
'/align', 'example_interfaces/msg/String', 'cdr')
topics = filter.filter_topic(topic_metadata)
assert(len(topics) == 2)
assert(topics[0].name == '/align')
assert(topics[0].type == 'example_interfaces/msg/String')
assert(topics[1].name == '/data')
assert(topics[1].type == 'example_interfaces/msg/String')
msg = String()
msg.data = 'align'
msgs = filter.filter_msg(('/align', serialize_message(msg), 1))
assert(len(msgs) == 2)
(topic0, data0, t0) = msgs[0]
(topic1, data1, t1) = msgs[1]
assert(topic0 == '/align')
assert(topic1 == '/data')
assert(t0 == t1)
assert(deserialize_message(data0, String).data == 'align')
assert(deserialize_message(data1, String).data == 'out')
def publish_news(self):
result = instance.read()
msg = String()
msg.data = "Last valid input:" + str(
datetime.datetime.now()) + " Temperature: " + str(
result.temperature)
self.publisher_.publish(msg)
def test_cut_filter():
filter = CutFilter()
parser = argparse.ArgumentParser('cut')
filter.add_arguments(parser)
args = parser.parse_args(['--duration', '0.5'])
filter.set_args([read_metadata('test/test.bag')], args)
string_msg = String()
string_msg.data = 'in'
# timestamp before bag start
msg = ('/data', serialize_message(string_msg), 0)
assert(filter.filter_msg(msg) == FilterResult.DROP_MESSAGE)
# exact start timestamp
msg = ('/data', serialize_message(string_msg), 1000)
assert(filter.filter_msg(msg) == msg)
# timestamp within the bag and cut duration
msg = ('/data', serialize_message(string_msg), 500 * 1000 * 1000 - 1000)
assert(filter.filter_msg(msg) == msg)
# timestamp after the requested duration, but before the last message in the test bag
msg = ('/data', serialize_message(string_msg), 1000 * 1000 * 1000 + 1000)
assert(filter.filter_msg(msg) == FilterResult.STOP_CURRENT_BAG)
def timer_call(self):
msg=String()
if (self.curv > 1):
msg.data="The robot is turning with a curvature" + str(self.curv)
self.obj_pub.publish(msg)
else:
msg.data="The path is straight"
self.obj_pub.publish(msg)
def timer_call(self):
msg=String()
if(self.flag == True):
msg.data="Hi"
self.flag=False
self.obj_pub.publish(msg)
elif(self.flag == False):
msg.data="Hello"
self.flag=True
self.obj_pub.publish(msg)
def timer_call(self):
msg = String()
if (self.dif_yaw < 0):
msg.data = "The robot is turning to the right with a curv" + str(
self.curv)
self.obj_pub.publish(msg)
else:
msg.data = "The robot is turning to the left with curv" + str(
self.curv)
self.obj_pub.publish(msg)
def create_test_bag(path):
writer = SequentialWriter()
storage_options, converter_options = get_rosbag_options(path)
writer.open(storage_options, converter_options)
topic = TopicMetadata('/data', 'example_interfaces/msg/String', 'cdr')
writer.create_topic(topic)
msg = String()
msg.data = 'test_start'
writer.write('/data', serialize_message(msg), 1000)
msg.data = 'test_end'
writer.write('/data', serialize_message(msg), CONVERSION_CONSTANT + 2000)
def setUp(self):
self.tmp_dir = tempfile.TemporaryDirectory()
bag_file_name = "{}{}{}".format(self.tmp_dir.name, os.sep, BAGNAME)
with baggie.BagWriter(bag_file_name) as bag:
for i in range(N_MSGS):
int_msg = Int32()
int_msg.data = i
str_msg = String()
str_msg.data = "The count is: %s" % i
bag.write(TOPIC_INT, int_msg)
bag.write(TOPIC_STR, str_msg)
time.sleep(1. / N_MSGS)
def timer_callBack(self):
#Increase the counter with every callback
self.counter = self.counter + 1
#Printing messages with counter value every call
self.get_logger().info("Msg number #{}".format(self.counter))
#Creating a local object from class String
msg = String()
#Overloading data over msg object to be published over "New_topic" topic
msg.data = "Pusblish msg number #{}".format(self.counter)
#Publish msg data over selected topic
self.varPublisher.publish(msg)
def test_rename_filter():
filter = RenameFilter()
parser = argparse.ArgumentParser('rename')
filter.add_arguments(parser)
args = parser.parse_args(['-t', '/data', '--name', '/renamed'])
filter.set_args(None, args)
topic_metadata = TopicMetadata(
'/data', 'example_interfaces/msg/String', 'cdr')
assert(filter.filter_topic(topic_metadata).name == '/renamed')
msg = String()
msg.data = 'test'
# timestamp within the bag and cut duration
bag_msg = ('/data', serialize_message(msg), 1)
(topic, _, _) = filter.filter_msg(bag_msg)
assert(topic == '/renamed')
def callback(self, msg):
#pos=self.get_logger().info(msg.data)
pos = int(msg.data)
directions = String()
if pos == 0:
directions.data = "no gate detected"
else:
if (pos - 1) // 3 == 0:
str1 = "UP"
elif (pos - 1) // 3 == 1:
str1 = ""
elif (pos - 1) // 3 == 2:
str1 = "down"
if (pos - 1) % 3 == 0:
str2 = " left"
elif (pos - 1) % 3 == 1:
str2 = " forward"
elif (pos - 1) % 3 == 2:
str2 = " right"
directions.data = str1 + str2
self.publisher_.publish(directions)
def setUp(self):
self.tmp_dir = tempfile.TemporaryDirectory()
bag_file_name = "{}{}{}".format(self.tmp_dir.name, os.sep, BAGNAME)
bag = baggie.Baggie(bag_file_name, mode="w", compress=False)
bag_comp_file_name = "{}{}{}".format(self.tmp_dir.name, os.sep,
BAGNAME_COMPRESSED)
bag_comp = baggie.Baggie(bag_comp_file_name, mode="w", compress=True)
for i in range(N_MSGS):
int_msg = Int32()
int_msg.data = i
str_msg = String()
str_msg.data = "The count is: %s" % i
bag.write(TOPIC_INT, int_msg)
bag.write(TOPIC_STR, str_msg)
bag_comp.write(TOPIC_INT, int_msg)
bag_comp.write(TOPIC_STR, str_msg)
time.sleep(1. / N_MSGS)
def test_replace_filter():
filter = ReplaceFilter()
parser = argparse.ArgumentParser('replace')
filter.add_arguments(parser)
args = parser.parse_args(['-t', '/data', '-v', 'test/data.yaml'])
filter.set_args(None, args)
string_msg = String()
string_msg.data = 'in'
msg = ('/data', serialize_message(string_msg), 0)
with pytest.raises(RuntimeError):
# if topic hasn't been found, filter_msg fails with error
filter.filter_msg(msg)
topic_metadata = TopicMetadata(
'/data', 'example_interfaces/msg/String', 'cdr')
assert(filter.filter_topic(topic_metadata) == topic_metadata)
(topic, result_data, t) = filter.filter_msg(msg)
assert(topic == '/data')
result_msg = deserialize_message(result_data, String)
assert(result_msg.data == 'out')
assert(t == 0)
def create_day_time_bag(path):
writer = SequentialWriter()
storage_options, converter_options = get_rosbag_options(path)
writer.open(storage_options, converter_options)
topic = TopicMetadata('/data', 'example_interfaces/msg/String', 'cdr')
writer.create_topic(topic)
HOUR_TO_NS = 60 * 60 * CONVERSION_CONSTANT
msg = String()
msg.data = 'msg0'
writer.write('/data', serialize_message(msg), 13 * HOUR_TO_NS - 1000)
msg.data = 'msg1'
writer.write('/data', serialize_message(msg), 13 * HOUR_TO_NS)
msg.data = 'msg2'
writer.write('/data', serialize_message(msg), 14 * HOUR_TO_NS)
msg.data = 'msg2'
writer.write('/data', serialize_message(msg), 14 * HOUR_TO_NS + 1000)
def publish_news(self):
msg = String()
msg.data = "Hello, this is " + str(self.robot_name)
self.publisher_.publish(msg)
def sub_callBack(self, msg):
state = String()
poses_length = len(msg.poses)
curvature = 0
direction = 0
curve_point1_x = 0
curve_point1_y = 0
curve_point2_x = int(poses_length / 10)
curve_point2_y = int(poses_length / 10)
curve_point3_x = int(poses_length / 5)
curve_point3_y = int(poses_length / 5)
if poses_length > 10:
p1_x = msg.poses[0].pose.position.x
p1_y = msg.poses[0].pose.position.y
v = msg.poses[0].pose.orientation
r, p, yaw_1 = self.euler_from_quaternion(
msg.poses[0].pose.orientation)
p2_x = msg.poses[5].pose.position.x
p2_y = msg.poses[5].pose.position.y
r, p, yaw_2 = self.euler_from_quaternion(
msg.poses[5].pose.orientation)
p3_x = msg.poses[10].pose.position.x
p3_y = msg.poses[10].pose.position.y
r, p, yaw_3 = self.euler_from_quaternion(
msg.poses[10].pose.orientation)
direction = (yaw_1 - yaw_3)
curvature = self.menger_curvature(p1_x, p1_y, p2_x, p2_y, p3_x,
p3_y)
if curvature > 1:
state.data = (
" The robot is turning with a curvature:{} where {} is the curavature of the path."
.format(curvature, curvature))
self.string_pub.publish(state)
if direction > 0:
self.get_logger().info(
" The robot is turning RIGHT with a curvature:{} where {} is the curavature of the path."
.format(curvature, curvature))
else:
self.get_logger().info(
" The robot is turning LEFT with a curvature:{} where {} is the curavature of the path."
.format(curvature, curvature))
else:
state.data = ("The path is straight :{}".format(curvature))
self.get_logger().info(
"The path is straight :{}".format(curvature))
self.string_pub.publish(state)
self.get_logger().info("The path is :{}".format(direction))
def process_img(self):
cap = cv2.VideoCapture('/home/islam/Videos/right_log11.avi')
fps = 5
cnt = 0
# cv2.namedWindow('HSV')
# cv2.resizeWindow('HSV',640,480)
# cv2.createTrackbar('th1','HSV',80,180,empty)
# cv2.createTrackbar('th2','HSV',150,180,empty)
# cv2.createTrackbar('hue min','HSV',0,180,empty)
# cv2.createTrackbar('hue max','HSV',84,180,empty)
# cv2.createTrackbar('Min sat','HSV',0,255,empty)
# cv2.createTrackbar('Max sat','HSV',143,255,empty)
# cv2.createTrackbar('Min val','HSV',0,255,empty)
# cv2.createTrackbar('Max val','HSV',255,255,empty)
if cap.isOpened() == False:
print(
"Error opening the video file. Please double check your file path for typos. Or move the movie file to the same location as this script/notebook"
)
while cap.isOpened():
success, frame = cap.read()
if success == True:
framehsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# th1=cv2.getTrackbarPos('th1','HSV')
# th2=cv2.getTrackbarPos('th2','HSV')
# x=cv2.getTrackbarPos('hue min','HSV')
# h_max=cv2.getTrackbarPos('hue max','HSV')
# s_min=cv2.getTrackbarPos('Min sat','HSV')
# s_max=cv2.getTrackbarPos('Max sat','HSV')
# v_min=cv2.getTrackbarPos('Min val','HSV')
# v_max=cv2.getTrackbarPos('Max val','HSV')
# if x%2!=1 and x!=100:
# cv2.setTrackbarPos("hue min","HSV",x+1)
# x+=1
# elif x==100:
# x-=1
#lower=np.array([h_min,s_min,v_min])
#upper=np.array([h_max,s_max,v_max])
mask_orange1 = cv2.inRange(framehsv, np.array([0, 0, 0]),
np.array([90, 136, 148]))
result_mask = cv2.bitwise_and(frame, frame, mask=mask_orange1)
result = np.array(result_mask)
result = getcontours(1, result, [80, 150])
contours, hierarchy = cv2.findContours(result, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
Rects = []
areas = []
for c in contours:
peri = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, 0.02 * peri, True)
if cv2.contourArea(c) > 1000 and len(approx) > 2 and len(
approx) < 7:
(x, y, w, h) = cv2.boundingRect(c)
(win_w, win_h, j) = frame.shape
flag = 0
if y + h > 150 and y < .8 * win_h:
Rects.append((x, y, w, h))
cv2.rectangle(frame, (x, y), (x + w, y + h),
(255, 0, 255), 2)
Rects.sort()
#print(Rects)
if len(Rects) == 0:
pos = 0
elif len(Rects) == 1:
(x, y, w, h) = Rects[0]
pos = 0
else:
(x, y, w, h) = Rects[0]
(x2, y2, w2, h2) = Rects[len(Rects) - 1]
p1 = (x, y)
p2 = (x2 + w2, y2 + h2)
area = (x2 - x + w2) * (y2 + h2 - y)
if area > 8000:
if len(Rects) == 2 or len(Rects) > 3:
if y < y2:
cv2.rectangle(frame, (x, y), (x2 + w2, y2 + h2),
(255, 255, 0), 2)
#elf.publisher_.publish(pos)
else:
p1 = (x, y2)
p2 = (x2 + w2, y + h)
cv2.rectangle(frame, (x, y2), (x2 + w2, y + h),
(255, 255, 0), 2)
pos = getPos(frame.copy(), p1, p2)
elif len(Rects) == 3:
(x1, y1, w1, h1) = Rects[0]
(x2, y2, w2, h2) = Rects[1]
(x3, y3, w3, h3) = Rects[2]
if x2 + int(w2 / 2) - x3 - int(w3 / 2) > x1 + int(
w1 / 2) - x2 - int(w2 / 2):
if y1 > y3:
p1 = (x1, y1)
p2 = (x3 + w3, y3 + h3)
else:
p1 = (x, y3)
p2 = (x3 + w3, y1 + h1)
p3, p4 = (x2 + w2, y2), (x3 + w3, y3 + h3)
cv2.rectangle(frame, (x2 + w2, y2),
(x3 + w3, y3 + h3), (0, 255, 255), 2)
cv2.putText(frame, 'Bonus', (x2 + w2, y2),
cv2.FONT_HERSHEY_COMPLEX, .7,
(0, 255, 255), 2)
else:
if y1 > y3:
p1 = (x1, y1)
p2 = (x3 + w3, y3 + h3)
else:
p1 = (x, y3)
p2 = (x3 + w3, y1 + h1)
p3, p4 = (x2 + w2, y2), (x1, y1 + h1)
cv2.rectangle(frame, (x2 + w2, y2), (x1, y1 + h1),
(0, 255, 255), 2)
cv2.putText(frame, 'Bonus', (x2 + w2 + 20, y2),
cv2.FONT_HERSHEY_COMPLEX, .7,
(0, 255, 255), 2)
pos = getPos(frame.copy(), p3, p4)
cv2.rectangle(frame, p1, p2, (255, 255, 0), 2)
msg = String()
msg.data = str(pos)
self.publisher_.publish(msg)
frame = np.array(frame)
#imgs=stackImages(.8,[frame,result_mask,result])
cv2.imshow('frame', frame)
cv2.resizeWindow('frame', 1000, 1000)
time.sleep(1 / fps)
key = cv2.waitKey(1)
if key == ord('q'):
break
if key == ord('p'):
cv2.waitKey(-1)
else:
break
cap.release()
cv2.destroyAllWindows()
def publish_news(self):
msg = String() # create an instance
msg.data = "Hi, this is " + str(self.robot_name_) + " from the robot_news_station."
self.publisher_.publish(msg)
def publish_news(self):
msg = String()
msg.data = "Hello, this is " + str(
self.robot_name_) + "at the robot news station."
self.publisher_.publish(msg)
def timer_callback(self):
self.counter_ += 1
data = String()
data.data = "Hello " + str(self.counter_)
self.publisher.publish(data)
self.get_logger().info("Hello " + str(self.counter_))
def publish_news(self):
msg = String()
msg.data = "Hi, this is " + \
str(self.robot_name_) + " from the Robot News Station"
self.publisher_.publish(msg)
def publish_news(self):
msg = String()
msg.data = "Hi, this is " + self.robot_name_ + "!"
self.publisher_.publish(msg)
def publish_news(self):
msg = String()
msg.data = f"publishing: {self.count}"
self.publisher.publish(msg)
self.count += 1