def camera():
rospy.init_node('camera')
image_topic = rospy.get_param('~image', '/camera/image')
image_pub = rospy.Publisher(image_topic, Image, queue_size=5)
bridge = CvBridge()
width = rospy.get_param('width', 640)
height = rospy.get_param('height', 480)
camera = Camera(pixel_clock=50, exposure_time=8)
#camera.configure(width=width, height=height)
while not rospy.is_shutdown():
try:
frame = camera.capture()
stamp = rospy.Time.now()
if len(frame.shape) == 2:
image_msg = bridge.cv2_to_imgmsg(frame, encoding='mono8')
else:
image_msg = bridge.cv2_to_imgmsg(frame, encoding='bgr8')
image_msg.header.stamp = stamp
image_pub.publish(image_msg)
except CvBridgeError, e:
print e
class image_publisher:
def __init__(self):
self.camera = PiCamera()
self.camera.resolution = (1024, 768) #(1920, 1080)
self.camera.ISO = 100
self.camera.sa = 100
self.camera.awb = "flash"
self.camera.co = 100
self.rawCapture = PiRGBArray(self.camera)
time.sleep(0.1)
self.image_pub = rospy.Publisher("image_topic",Image)
self.small_image_pub = rospy.Publisher("small_image_topic",Image)
self.bridge = CvBridge()
def publishImage(self):
for frame in self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True):
try:
cv_image = frame.array
except CvBridgeError as e:
print(e)
#cv_image = (cv_image * 0.5.astype(umpy.uint8)
self.rawCapture.truncate(0)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
small = cv2.resize(cv_image, (0,0), fx=0.5, fy=0.5)
self.small_image_pub.publish(self.bridge.cv2_to_imgmsg(small, "bgr8"))
except CvBridgeError as e:
print("problem")
print(e)
def test_numpy_types(self):
import cv2
import numpy as np
bridge_ = CvBridge()
self.assertRaises(TypeError, lambda: bridge_.cv2_to_imgmsg(1, "rgba8"))
if hasattr(cv2, 'cv'):
self.assertRaises(TypeError, lambda: bridge_.cv2_to_imgmsg(cv2.cv(), "rgba8"))
def imagePublisher():
front_pub = rospy.Publisher('center_cam', Image, queue_size=1)
left_pub = rospy.Publisher('left_cam', Image, queue_size =1)
right_pub = rospy.Publisher('right_cam', Image, queue_size = 1)
rospy.init_node('camera_node', anonymous=True)
#rate=rospy.Rate(30)#10hz
bridge = CvBridge()
while not rospy.is_shutdown():
_, front_img = cam_front.read()
_, left_img = cam_left.read()
_, right_img = cam_right.read()
cv2.imshow('image', front_img) # for debugging purposes
cv2.imshow('left', left_img)
cv2.imshow('right', right_img)
front_img = bridge.cv2_to_imgmsg(front_img, "bgr8")
left_img = bridge.cv2_to_imgmsg(left_img, "bgr8") #change to grayscale?
# right_img = bridge.cv2_to_imgmsg(left_img, "bgr8")
rospy.loginfo("images sent")
# for debugging purposes, remove if cv2.imshow('imgae', img) is deleted
k = cv2.waitKey(1) & 0xFF
if k ==27:
break
front_pub.publish(front_img)
left_pub.publish(left_img)
# right_pub.publish(right_img)
cv2.destroyAllWindows()
cam_front.release()
cam_left.release()
class ProcessDepth:
def __init__(self):
# Allows conversion between numpy arrays and ROS sensor_msgs/Image
self.bridge = CvBridge()
# Allow our topics to be dynamic.
self.input_camera_topic = rospy.get_param('~input_camera_topic', '/camera/depth/image_rect')
self.output_camera_topic = rospy.get_param('~output_camera_topic', '/processed')
# WE are going to publish a debug image as it comes in.
self.pub = rospy.Publisher(self.output_camera_topic, Image,queue_size=10)
rospy.Subscriber(self.input_camera_topic, Image, self._process_depth_img)
# run the node
self._run()
# Keep the node alive
def _run(self):
rospy.spin()
def _process_depth_img(self,input):
# convert our image to CV2 numpy format from ROS format
latest_image = self.bridge.imgmsg_to_cv2(input)
if( latest_image is not None ):
try:
# convert the image to SimpleCV
# The input image is single channel float and we want rgb uint8
# it is also full of nasty nans. We get the min and max and scale
# the image from [0,flt_max] to [0,255]
dmin = np.nanmin(latest_image)
dmax = np.nanmax(latest_image)
latest_image = latest_image - dmin
sf = 255.0/(dmax-dmin)
latest_image = sf*latest_image
# Convert to uint8
temp = latest_image.astype(np.uint8)
# move to SimpleCV RGB
img = scv.Image(temp, cv2image=True, colorSpace=scv.ColorSpace.RGB)
# get values less than 128
lt = img.threshold(128).invert()
# get values greater than 64
gt = img.threshold(64)
# do the logical and of the two depths
range = lt*gt
# apply the mask to the input image
blobs = img.findBlobsFromMask(mask=range)
# draw the results.
if( blobs ):
blobs.draw(color=scv.Color.RED,width=-1)
img = img.applyLayers()
# convert SimpleCV to CV2 Numpy Format
cv2img = img.getNumpyCv2()
# Convert Cv2 numpy to ROS format
img_msg = self.bridge.cv2_to_imgmsg(cv2img, "bgr8")
# publish the topic.
self.pub.publish(self.bridge.cv2_to_imgmsg(cv2img, "bgr8"))
except CvBridgeError, e:
rospy.logwarn("PROCESSING EXCEPTION {0}".format(e))
class VirtualMirror:
def __init__(self, target_img="cone.png"):
self.node_name = "Cone Detector"
self.thread_lock = threading.Lock()
self.sub_image = rospy.Subscriber("~image_raw", CompressedImage, self.cbImage, queue_size=1)
self.pub_image_mirror = rospy.Publisher("~virtual_mirror_img", Image, queue_size=1)
self.pub_image_regular = rospy.Publisher("~virtual_regular_img", Image, queue_size=1)
self.pub_image_average = rospy.Publisher("~virtual_regular_img_avg", Image, queue_size=1)
self.bridge = CvBridge()
self.image_prev = None
# Subscribers
rospy.loginfo("[%s] Initialized." %(self.node_name))
def cbImage(self,image_msg):
# Start a daemon thread to process the image
thread = threading.Thread(target=self.processImage,args=(image_msg,))
thread.setDaemon(True)
thread.start()
# Returns rightaway
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
# Return immediately if the thread is locked
return
#self.sub_image.unregister()
np_arr = np.fromstring(image_msg.data, np.uint8)
image_cv = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
img = image_cv.copy()
img_avg = cv2.flip(image_cv, 1)
(H,W,_) = image_cv.shape
"""
H -= 1
W -= 1
for u in range(H):
for v in range(W):
img[u][v] = image_cv[u, W-v]
if self.image_prev !=None:
curr = list(image_cv[u][v])
prev = list(self.image_prev[u][v])
avg = [ (curr[i] + prev[i])/2 for i in range(3)]
img_avg[u][v] = avg
"""
try:
self.pub_image_mirror.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
self.pub_image_regular.publish(self.bridge.cv2_to_imgmsg(image_cv, "bgr8"))
if self.image_prev != None:
self.pub_image_average.publish(self.bridge.cv2_to_imgmsg(img_avg, "bgr8"))
except CvBridgeError as e:
print(e)
self.image_prev = image_cv
self.thread_lock.release()
class PersonInfoNode:
def __init__(self):
#self._people_pub = rospy.Publisher('track_people', TrackPeople, queue_size=10)
self._image_pub = rospy.Publisher('~/people_track_img', Image, queue_size=10)
self._new_body = False
self._new_image = False
self._body_lock = Lock()
self._img_lock = Lock()
self.bridge = CvBridge()
self.bodies = None
self.img = None
def body_handler(self, body_array):
with self._body_lock:
self.bodies = body_array.bodies
self._new_body = True
def image_handler(self, data):
with self._img_lock:
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logwarn(e)
self._new_image = True
def pub_msgs(self):
with self._body_lock:
with self._img_lock:
if not self._new_body or not self._new_image:
return
self._new_body = False
self._new_image = False
track_people = TrackPeople()
show_img = array(self.img, copy=True)
rows, cols, _ = self.img.shape
for body in self.bodies:
person = TrackPerson()
person.trackingId = body.trackingId
person.pos = average_position(body)
person.header = body.header
rospy.loginfo("Bounding box")
from_x = max(min(body.fromX, body.toX), 0)
to_x = min(max(body.fromX, body.toX), cols)
from_y = max(min(body.fromY, body.toY), 0)
to_y = min(max(body.fromY, body.toY), rows)
print(from_x, from_y)
print(to_x, to_y)
person.image = self.bridge.cv2_to_imgmsg(
self.img[from_y:to_y, from_x:to_x], 'bgr8')
track_people.people.append(person)
cv2.rectangle(show_img,
(body.fromX, body.fromY),
(body.toX, body.toY),
(255, 0, 0), 3)
self._image_pub.publish(
self.bridge.cv2_to_imgmsg(show_img, 'bgr8'))
class ImageProcessingExample:
def __init__(self):
# Allows conversion between numpy arrays and ROS sensor_msgs/Image
self.bridge = CvBridge()
# Allow our topics to be dynamic.
self.input_camera_topic = rospy.get_param('~input_camera_topic', '/camera/image_rect_color')
self.output_camera_topic = rospy.get_param('~output_camera_topic', '/camera/color_stuff')
# WE are going to publish a debug image as it comes in.
self.pub = rospy.Publisher(self.output_camera_topic, Image,queue_size=10)
# We also publish the position of the thing we found.
self.point_pub = rospy.Publisher("/position",Point,queue_size=10)
# we get our input from a topic, it is of type image, call _show_colored_stuff.
rospy.Subscriber(self.input_camera_topic, Image, self._show_colored_stuff)
# run the node
self._run()
# Keep the node alive
def _run(self):
rospy.spin()
def _show_colored_stuff(self,input):
# convert our image to CV2 numpy format from ROS format
latest_image = self.bridge.imgmsg_to_cv2(input)
if( latest_image is not None ):
try:
# convert the image to SimpleCV
img = scv.Image(latest_image, cv2image=True, colorSpace=scv.ColorSpace.RGB)
# create a writeable copy
src = img.copy()
# Do our image processing get hue, threshold, morphology, connected components.
mask = src.hueDistance(180).invert().threshold(240).erode(2).dilate(3)
blobs = src.findBlobsFromMask(mask=mask)
# if we find something
if( blobs ):
# draw its convex hull with alpha
blobs[-1].drawHull(width=-1,color=scv.Color.YELLOW,alpha=128)
# and publish a position update
pt = Point(x=blobs[-1].x,y=blobs[-1].y,z=blobs[-1].area())
self.point_pub.publish(pt)
src = src.applyLayers()
# convert SimpleCV to CV2 Numpy Format
cv2img = src.getNumpyCv2()
# Convert Cv2 numpy to ROS format
img_msg = self.bridge.cv2_to_imgmsg(cv2img, "bgr8")
# publish the topic.
self.pub.publish(self.bridge.cv2_to_imgmsg(cv2img, "bgr8"))
except CvBridgeError, e:
print e
def convertData(self, bagfile):
seq = 0
bridge = CvBridge()
while(True):
# Capture frame-by-frame
ret, cvImage = self.capture.read()
try:
imageMsg = bridge.cv2_to_imgmsg(cvImage, "bgr8") # TODO: format spec as cmd option?
except CvBridgeError, e:
print e
# creating ros message
seq = seq + 1
imageMsg.header.seq = seq
# TODO: temporary hack, time sync/source is needed
imageMsg.header.stamp = rospy.Time.from_sec(time.time())
# write message to bag file
bagfile.write(self.topic, imageMsg, imageMsg.header.stamp)
# this is not so important for conversion
if self.showFrames == True:
cv2.imshow('frame', cvImage)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def processFrame(image):
global pub1, pub2
conVerter = CvBridge()
#Convert from a ROS image to a CV image
try:
image = conVerter.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print (e)
result = bT.computeSpheres(image)
msgBallCoords = ballcoords()
msgModImage = Image()
if result == None:
return
msgBallCoords.color= "RED"
msgBallCoords.x = result[0]
msgBallCoords.y = result[1]
msgBallCoords.radius = result[2]
# convert from a CV image to a ROS image
resImage = result[3]
#resImage = conVerter.cv2_to_imgmsg(resImage , "bgr8")
msgModImage = conVerter.cv2_to_imgmsg(resImage , "bgr8")
rospy.loginfo(msgBallCoords)
pub1.publish(msgBallCoords)
pub2.publish(msgModImage)
def spin(self):
time.sleep(1.0)
started = time.time()
counter = 0
cvim = numpy.zeros((480, 640, 1), numpy.uint8)
ball_xv = 10
ball_yv = 10
ball_x = 100
ball_y = 100
cvb = CvBridge()
while not rospy.core.is_shutdown():
cvim.fill(0)
cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1)
ball_x += ball_xv
ball_y += ball_yv
if ball_x in [10, 630]:
ball_xv = -ball_xv
if ball_y in [10, 470]:
ball_yv = -ball_yv
self.pub.publish(cvb.cv2_to_imgmsg(cvim))
time.sleep(0.03)
def tachyon():
rospy.init_node('tachyon')
image_topic = rospy.get_param('~image', '/tachyon/image')
image_pub = rospy.Publisher(image_topic, Image, queue_size=5)
bridge = CvBridge()
mode = rospy.get_param('~mode', 'mono8')
config_file = rospy.get_param('~config', 'tachyon.yml')
path = rospkg.RosPack().get_path('mashes_tachyon')
config_filename = os.path.join(path, 'config', config_file)
tachyon = Tachyon(config=config_filename)
tachyon.connect()
tachyon.calibrate(24)
while not rospy.is_shutdown():
try:
frame, header = tachyon.read_frame()
frame = tachyon.process_frame(frame)
if mode == 'rgb8':
frame = LUT_IRON[frame]
else:
frame = np.uint8(frame >> 2)
image_msg = bridge.cv2_to_imgmsg(frame, encoding=mode)
image_msg.header.stamp = rospy.Time.now()
image_pub.publish(image_msg)
except CvBridgeError, e:
print e
def sendImg():
global FPS
global DEV
global last_fpsGlobal
rospy.init_node('camera_publisher_node', anonymous=True)
image_pub = rospy.Publisher("/camera_publisher/output" + str(DEV) + "_" + str(FPS), Image, queue_size=10)
bridge = CvBridge()
cap = cv2.VideoCapture(DEV)
cap.set(cv2.cv.CV_CAP_PROP_FPS, FPS)
threading.Timer(1, printFPS).start ()
while not rospy.is_shutdown():
start = datetime.now()
# Capture frame-by-frame
ret, frame = cap.read()
fpsGlobal = 1000000 / (datetime.now() - start).microseconds
last_fpsGlobal = (fpsGlobal * 0.1) + (last_fpsGlobal * 0.9)
if GUI:
#gray = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
cv2.imshow("output" + str(DEV) + "_" + str(FPS), frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
try:
image_pub.publish(bridge.cv2_to_imgmsg(frame, "bgr8"))
except:
pass
def send_camera_data(rgb_image, thresh_image):
#global cv_depth_thresh
bridge = CvBridge()
cd = CameraData()
cd.rgb_image = rgb_image
cd.thresh_image = bridge.cv2_to_imgmsg(thresh_image, "passthrough")
return cd
class DetectionPublisher(object):
"""
Publish ROS detection messages
"""
def __init__(self):
self.DetPub = rospy.Publisher('Detection', Detection, queue_size=10)
self._bridge = CvBridge()
def publish(self, boxes, scores, classes, num, category_index, masks=None, fps=0):
# init detection message
msg = Detection()
for i in range(boxes.shape[0]):
if scores[i] > 0.5:
if classes[i] in category_index.keys():
class_name = category_index[classes[i]]['name']
else:
class_name = 'N/A'
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
box = RegionOfInterest()
box.x_offset = xmin + (xmax-xmin)/2.0
box.y_offset = ymin + (ymax-ymin)/2.0
box.height = ymax - ymin
box.width = xmax - xmin
# fill detection message with objects
obj = Object()
obj.box = box
obj.class_name = class_name
obj.score = int(100*scores[i])
if masks is not None:
obj.mask = self._bridge.cv2_to_imgmsg(masks[i], encoding="passthrough")
msg.objects.append(obj)
msg.fps = fps
# publish detection message
self.DetPub.publish(msg)
class VirtualMirror(object):
def __init__(self):
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.flip_direction = self.setupParam("~flip_direction","horz") # default horz
self.pub_img = rospy.Publisher("~image/flipped",Image,queue_size=1)
self.last_stamp = rospy.Time.now()
self.sub_img = rospy.Subscriber("~img_in",CompressedImage, self.flipImage)
self.param_timer = rospy.Timer(rospy.Duration.from_sec(1.0),self.cbParamTimer)
def cbParamTimer(self,event):
self.flip_direction = rospy.get_param("~flip_direction", "horz")
def setupParam(self,param_name,default_value):
value = rospy.get_param(param_name,default_value)
rospy.set_param(param_name,value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
return value
def flipImage(self,msg):
if self.flip_direction == "vert":
int_direction = 0
else:
int_direction = 1 # Default horz
np_arr = np.fromstring(msg.data, np.uint8)
cv_image = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
img_msg = cv2.flip(cv_image,int_direction)
img_msg = self.bridge.cv2_to_imgmsg(img_msg, "bgr8")
self.pub_img.publish(img_msg)
class Node():
def __init__(self):
self.image_pub = rospy.Publisher('/image_raw', Image, queue_size=1)
self.bridge = CvBridge()
self.sensor_sub = rospy.Subscriber('/sensors', PendulumPose, self.pose_callback)
self.cmd_sub = rospy.Subscriber('/cmd', Cmd, self.cmd_callback)
self.rate = rospy.Rate(rospy.get_param('pendulum/gui_update_frequency'))
self.cmd = Cmd()
self.pose = PendulumPose()
parameters = rospy.get_param('pendulum')
self.visualizer = Visualizer(track_length = parameters['track_length'])
def cmd_callback(self, cmd):
self.cmd = cmd
def pose_callback(self, pose):
self.pose = pose
def update(self):
try:
image = self.visualizer.draw(self.pose.x, self.pose.theta*pi/180,
self.pose.xDot, self.pose.thetaDot*pi/180, self.cmd.cmd)
ros_image = self.bridge.cv2_to_imgmsg(image, "rgb8")
self.cmd = Cmd()
self.image_pub.publish(ros_image)
except CvBridgeError as e:
print(e)
class SegmentationPublisher(object):
"""
Publish ROS detection messages
"""
def __init__(self):
self.SegPub = rospy.Publisher('Segmentation', Segmentation, queue_size=10)
self._bridge = CvBridge()
def publish(self, boxes, classes, labels, seg_map, fps=0):
# init detection message
msg = Segmentation()
boxes = []
for i in range(boxes.shape[0]):
class_name = label[i]
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
box = RegionOfInterest()
box.x_offset = xmin + (xmax-xmin)/2.0
box.y_offset = ymin + (ymax-ymin)/2.0
box.height = ymax - ymin
box.width = xmax - xmin
# fill segmentation message
msg.boxes.append(box)
msg.class_names.append(class_name)
msg.seg_map = self._bridge.cv2_to_imgmsg(seg_map, encoding="passthrough")
msg.fps = fps
# publish detection message
self.SegPub.publish(msg)
class Mirror(object):
""" """
def __init__(self):
""" """
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.pub_mirror = rospy.Publisher( "image/mirror", Image, queue_size=1)
self.sub_compressed_img = rospy.Subscriber( "camera_node/image/compressed" , CompressedImage, self.callback, queue_size=1)
def callback(self,msg):
""" """
# Load message
#cv_img = self.bridge.imgmsg_to_cv2( msg.data )
np_arr = np.fromstring(msg.data, np.uint8)
cv_img = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
# Mirror
cv_img_mirror = cv_img.copy()
cv_img_mirror = cv2.flip(cv_img,1)
# Publish Message
img_msg = self.bridge.cv2_to_imgmsg( cv_img_mirror , "bgr8")
img_msg.header.stamp = msg.header.stamp
img_msg.header.frame_id = msg.header.frame_id
self.pub_mirror.publish(img_msg)
class image_converter:
def __init__(self):
rospy.init_node('image_converter', anonymous=True)
self.image_pub = rospy.Publisher("analyzed_image", Image, queue_size=10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("image_raw", Image, self.callback)
self.stabilizer = VideoStabilizer([[409, 250], [300, 762], [1123, 848], [1600, 238]])
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
#cv_image = self.analyze_image(cv_image)
#self.showImage(cv_image)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def analyze_image(self, image):
image = self.stabilizer.stabilize_frame(image)
return image
def showImage(self, image):
cv2.imshow("Image window", image)
cv2.waitKey(3)
class Echo:
def __init__(self):
self.node_name = "BlobDetector"
self.thread_lock = threading.Lock()
self.sub_image = rospy.Subscriber("/camera/rgb/image_rect_color",\
Image, self.cbImage, queue_size=1)
self.pub_image = rospy.Publisher("/blob_image",\
Image, queue_size=1)
self.pub_data = rospy.Publisher("/blob_detections", BlobDetections, queue_size = 1)
self.bridge = CvBridge()
rospy.loginfo("[%s] Initialized." %(self.node_name))
def cbImage(self,image_msg):
thread = threading.Thread(target=self.processImage,args=(image_msg,))
thread.setDaemon(True)
thread.start()
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
return
image_cv = self.bridge.imgmsg_to_cv2(image_msg)
msg, img = process(image_cv)
try:
self.pub_image.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
self.pub_data.publish(msg)
except CvBridgeError as e:
print(e)
self.thread_lock.release()
def test_target_image(self):
"""Tests posting telemetry data through client."""
# Set up test data.
url = "http://interop"
client_args = (url, "testuser", "testpass", 1.0)
target_id = 1
width = 40
height = 30
nparr = np.random.randint(0, 256, (width, height, 3)).astype(np.uint8)
bridge = CvBridge()
ros_img = bridge.cv2_to_imgmsg(nparr)
img = TargetImageSerializer.from_msg(ros_img)
with InteroperabilityMockServer(url) as server:
# Setup mock server.
server.set_root_response()
server.set_login_response()
server.set_post_target_image_response(target_id)
server.set_get_target_image_response(target_id, img, "image/png")
server.set_delete_target_image_response(target_id)
# Connect client.
client = InteroperabilityClient(*client_args)
client.wait_for_server()
client.login()
client.post_target_image(target_id, ros_img)
client.get_target_image(target_id)
client.delete_target_image(target_id)
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2",Image)
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("image_topic",Image,self.callback)
def callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows,cols,channels) = cv_image.shape
if cols > 60 and rows > 60 :
cv2.circle(cv_image, (50,50), 10, 255)
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
class circle_detect:
def __init__(self):
self.image_pub = rospy.Publisher("circle_detection", Image, queue_size = 10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/rgb_image", Image, self.callback)
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
x0 = 320
y0 = 240
idx = None
minC = 800
numC = None
img = cv2.medianBlur(cv_image,5)
imgg = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
cimg = cv2.cvtColor(imgg, cv2.COLOR_GRAY2BGR)
circles = cv2.HoughCircles(imgg,cv2.cv.CV_HOUGH_GRADIENT, 1.2, 100, param1 = 200, param2 = 30, minRadius = 5, maxRadius = 20)
if circles is None:
cv2.imshow("Image Windows", cv_image)
rospy.loginfo('No circles detected')
#circles = np.uint16(np.around(circles)) # .astype("int") # convert the (x,y) coordinates and radius of the circles to integers
#numC = len(circles[0,:])
else:
print circles.shape
# rospy.loginfo('circles found, number of circles {}'.format(numC))
# for i in circles:
# dist[i] = distance(x0,y0, i[0],i[1])
# if dist[i] < minC:
# minC = dist[i]
# idx = i
#for i in circles[0,:]:
# print idx
# cv2.circle(cv_image, (idx[0],idx[1]),idx[2], (0,255,0),1) # draw the outer cirlce
# cv2.circle(cv_image, (idx[0],idx[1]), 2, (0,0,255), 3) # draw the center of the circle
for i in circles[0,:]:
cv2.circle(cv_image, (i[0],i[1]),i[2], (0,255,0),1) # draw the outer cirlce
cv2.circle(cv_image, (i[0],i[1]), 2, (0,0,255), 3) # draw the center of the circle
cv2.imshow("Image Windows", cv_image)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def run():
global right_image
global left_image
# Init node
rospy.init_node("stereo_vision");
# Subscribe for two "eyes"
rospy.Subscriber("/stereo/right/image_raw", Image, right_image_callback)
rospy.Subscriber("/stereo/left/image_raw", Image, left_image_callback)
stereo_publisher = rospy.Publisher("/vision/stereo", Image, queue_size=1000)
# Action
print "Start processing..."
bridge = CvBridge()
worker = StereoVision()
# rate = rospy.Rate(10)
while not rospy.is_shutdown():
if right_image != None and left_image != None:
cv_right_image = bridge.imgmsg_to_cv2(right_image, desired_encoding="bgr8")
cv_left_image = bridge.imgmsg_to_cv2(left_image, desired_encoding="bgr8")
cv_stereo_image = worker.create_stereo_image( cv_right_image, cv_left_image )
stereo_image = bridge.cv2_to_imgmsg(cv_stereo_image, encoding="bgr8")
stereo_publisher.publish( stereo_image )
class afisare:
def __init__(self):
self.bridge = CvBridge()
self.cap = cv2.VideoCapture(0)
self.cap2 = rospy.Subscriber("/optris/image_color", Image, self.callback)
self.pub = rospy.Publisher("topic1", Image, queue_size = 5)
def callback(self, data):
try:
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
gray = cv2.cvtColor(self.cv_image,cv2.COLOR_BGR2GRAY)
except CvBridgeError as e:
print (e)
try:
self.pub.publish(self.bridge.cv2_to_imgmsg(gray, '8UC1'))
except CvBridgeError as e:
print (e)
cv2.imshow("Fereastra1", gray)
cv2.waitKey(3)
def gen_blog_entry(self, roi_id, obj_id, objs):
print 'Region: ' + self.get_roi_name(roi_id)
time = dt.fromtimestamp(int(rospy.get_time()))
body = '### CHANGE DETECTION REPORT\n\n'
body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n'
#body += '- **Object ID:** ' + str(obj_id) + '\n\n'
body += '- **Time:** ' + str(time) + '\n\n'
# Create some blog entries
msg_store = MessageStoreProxy(collection=self.blog_collection)
robblog_path = roslib.packages.get_pkg_dir('soma_utils')
for idx,obj in enumerate(objs):
bridge = CvBridge()
im = bridge.imgmsg_to_cv2(obj.image_mask, desired_encoding="bgr8")
imgray = cv2.cvtColor(im,cv2.COLOR_BGR2GRAY)
ret,thresh = cv2.threshold(imgray,1,1,1) #cv2.threshold(imgray,127,255,0)
contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(obj.image_full, desired_encoding="bgr8")
cv2.drawContours(cv_image,contours,-1,(0,0,255),2)
full_scene_contour = bridge.cv2_to_imgmsg(cv_image)
img_id = msg_store.insert(full_scene_contour)
body += '<font color="red">Detected change (' + str(idx+1) + '/'+ str(len(objs)) + '):</font>\n\n)\n\n' % img_id
e = RobblogEntry(title=str(time) + " Change Detection - " + self.roi_name[roi_id], body= body )
msg_store.insert(e)
def test_encode_decode_cv2(self):
import cv2
import numpy as np
fmts = [cv2.CV_8U, cv2.CV_8S, cv2.CV_16U, cv2.CV_16S, cv2.CV_32S, cv2.CV_32F, cv2.CV_64F]
cvb_en = CvBridge()
cvb_de = CvBridge()
for w in range(100, 800, 100):
for h in range(100, 800, 100):
for f in fmts:
for channels in ([], 1, 2, 3, 4, 5):
if channels == []:
original = np.uint8(np.random.randint(0, 255, size=(h, w)))
else:
original = np.uint8(np.random.randint(0, 255, size=(h, w, channels)))
rosmsg = cvb_en.cv2_to_imgmsg(original)
newimg = cvb_de.imgmsg_to_cv2(rosmsg)
self.assert_(original.dtype == newimg.dtype)
if channels == 1:
# in that case, a gray image has a shape of size 2
self.assert_(original.shape[:2] == newimg.shape[:2])
else:
self.assert_(original.shape == newimg.shape)
self.assert_(len(original.tostring()) == len(newimg.tostring()))
def send_test_messages(self, filename):
self.msg_received = False
# Publish the camera info TODO make this a field in the annotations file to dictate the source calibration file
msg_info = CameraInfo()
msg_info.height = 480
msg_info.width = 640
msg_info.distortion_model = "plumb_bob"
msg_info.D = [-0.28048157543793056, 0.05674481026365553, -0.000988764087143394, -0.00026869128565781613, 0.0]
msg_info.K = [315.128501, 0.0, 323.069638, 0.0, 320.096636, 218.012581, 0.0, 0.0, 1.0]
msg_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg_info.P = [217.48876953125, 0.0, 321.3154072384932, 0.0, 0.0, 250.71084594726562, 202.30416165274983, 0.0,
0.0, 0.0, 1.0, 0.0]
msg_info.roi.do_rectify = False
# Publish the test image
img = cv2.imread(filename)
cvb = CvBridge()
msg_raw = cvb.cv2_to_imgmsg(img, encoding="bgr8")
self.pub_info.publish(msg_info)
self.pub_raw.publish(msg_raw)
# Wait for the message to be received
timeout = rospy.Time.now() + rospy.Duration(10) # Wait at most 5 seconds for the node to reply
while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout:
rospy.sleep(0.1)
self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.")
class ImageAvNode(object):
def __init__(self):
self.node_name = "Image Av"
self.sub_image = rospy.Subscriber("/ernie/camera_node/image/compressed", CompressedImage,self.flipImageNP)
self.pub_image = rospy.Publisher("~mirror_image/compressed", CompressedImage, queue_size=1)
self.bridge = CvBridge()
def flipImageNP(self,msg):
np_array = np.fromstring(msg.data, np.uint8)
image_np = cv2.imdecode(np_array, cv2.CV_LOAD_IMAGE_COLOR)
#flip_arr = np.fliplr(np_arr)
imageflip_np=cv2.flip(image_np,1)
flip_im = CompressedImage()
flip_im.data = np.array(cv2.imencode('.jpg', imageflip_np)[1]).tostring()
#flip_im.data = flip_arr.tostring()
flip_im.header.stamp = msg.header.stamp
self.pub_image.publish(flip_im)
def flipImage(self,msg):
# Decode from compressed image
# with OpenCV
cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
#image_cv = cv2.imdecode(np.fromstring(msg.data, np.uint8), cv2.CV_LOAD_IMAGE_COLOR)
hei_original = image_cv.shape[0]
wid_original = image_cv.shape[1]
#reverseimage = image_cv[:,:,-1]
reverseimg=cv2.flip(cv_image,1)
image_msg_out = self.bridge.cv2_to_imgmsg(reverseimage, "bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
class BaselineOrangeFinder:
def __init__(self, image_topic='/d400/color/image_raw', depth_topic='/d400/aligned_depth_to_color/image_raw', gcop_topic="/gcop_odom"):
print(image_topic, depth_topic)
self.image_sub = message_filters.Subscriber(image_topic, Image)
self.depth_sub = message_filters.Subscriber(depth_topic, Image)
self.odom_node = message_filters.Subscriber(gcop_topic, Odometry)
self.__params()
self.__bridge = CvBridge()
self.__segmodel = SegmentationNet()
self.__loadModel()
torch.no_grad()
self.__loadMeanImages()
self.__pointcloud_publisher = rospy.Publisher("/pointcloud_topic", PointCloud, queue_size=2)
self.__pointcloud_publisher_extra = rospy.Publisher("/pointcloud_topic_extra", PointCloud, queue_size=2)
self.__pointcloud_publisher_extra_norange = rospy.Publisher("/pointcloud_topic_extra_norange", PointCloud, queue_size=2)
self.__goalpoint_publisher = rospy.Publisher("/ros_tracker", PoseArray, queue_size=100)
self.__normal_vec_publisher = rospy.Publisher("/normal_tracker", PoseArray, queue_size=100)
self.__seg_image_publisher = rospy.Publisher("/seg_image", Image, queue_size=5)
rot = Rotations()
self.world2orange = None #np.eye(3)#np.matmul(rot.rotz(0), np.matmul(rot.rotx(np.pi/2),np.matmul(rot.rotz(np.pi/2), rot.roty(np.pi/2))))
#self.world2orange = np.matmul(rot.rotz(np.pi/2), rot.roty(np.pi/2))
self.listener = tf.TransformListener()
self.br = tf.TransformBroadcaster()
self.world2orange_pos = None
self.t = 0
self.alpha = 0.5
self.min_alpha = 0.05
self.max_steps = 50
self.stamp_now = None
print("Setup complete")
def __loadModel(self):
if os.path.isfile(self.__segload):
print("Loading Model: ", self.__segload)
if not self.__gpu is None:
checkpoint = torch.load(self.__segload,map_location=torch.device('cuda'))
else:
checkpoint = torch.load(self.__segload)
self.__segmodel.load_state_dict(checkpoint)
self.__segmodel.eval()
print("Model Loaded.")
else:
print("No checkpoint found at: ", self.__segload)
exit(0)
if not self.__gpu is None:
self.__device = torch.device('cuda:'+str(self.__gpu))
self.__segmodel = self.__segmodel.to(self.__device)
else:
self.__device = torch.device('cpu')
self.__segmodel = self.__segmodel.to(self.__device)
self.__segmodel.eval()
def __loadMeanImages(self):
if not (os.path.exists(self.__mean_image_loc)):
print('mean image file not found', self.__mean_image_loc)
exit(0)
else:
print('mean image file found')
self.__mean_image = np.load(self.__mean_image_loc)
def __params(self, gpu=True, relative=True):
self.__num_images = 1
self.__segload = "/home/gabe/repos/aerial_autonomy_ws/src/orange_picking/useful_models/seg/model_seg145.pth.tar"
self.__mean_image_loc = "/home/gabe/repos/aerial_autonomy_ws/src/orange_picking/useful_models/seg/mean_image.npy"
if torch.cuda.is_available() and gpu:
self.__gpu = 0
else:
self.__gpu = None
self.__h = 480
self.__w = 640
def __rosmsg2np(self,data):
try:
image_arr = self.__bridge.imgmsg_to_cv2(data,"rgb8")
except CvBridgeError as e:
print(e)
return image_arr
def __meanSubtract(self, cv_image):
cv_image = cv2.resize(cv_image,(self.__w,self.__h))
cv2.imwrite("test_pre.png", cv_image)
cv_image = cv_image/255.0
#print(cv_image.shape)
if self.__mean_image is None:
mean_subtracted = (cv_image)
print("ISSUEE: NO MEAN!!! ")
else:
mean_subtracted = (cv_image-self.__mean_image)
return mean_subtracted
def __process4model(self, image_arr):
image_arr = self.__meanSubtract(image_arr)
image_arr = image_arr.transpose(2,0,1)
image_arr = image_arr.reshape((1,3,self.__h,self.__w))
image_tensor = torch.tensor(image_arr)
image_tensor = image_tensor.to(self.__device,dtype=torch.float)
return image_tensor
def __segmentationInference(self, image_tensor):
seglogits = self.__segmodel(image_tensor)
seglogits = seglogits.view(-1,2,self.__segmodel.h,self.__segmodel.w)
segimages = (torch.max(seglogits, 1).indices).to('cpu') #.type(torch.FloatTensor).to(device)
seg_np = np.array(segimages[0,:,:])
pub_np = (255 * seg_np.copy()).astype(np.uint8).reshape((self.__h, self.__w))
image_message = self.__bridge.cv2_to_imgmsg(pub_np, encoding="passthrough")
self.__seg_image_publisher.publish(image_message)
cv2.imwrite('test.png', pub_np)
return seg_np
def __depthnpFromImage(self, msg):
#Stolen from https://github.com/eric-wieser/ros_numpy/blob/master/src/ros_numpy/image.py
if not msg.encoding in name_to_dtypes:
raise TypeError('Unrecognized encoding {}'.format(msg.encoding))
dtype_class, channels = name_to_dtypes[msg.encoding]
dtype = np.dtype(dtype_class)
dtype = dtype.newbyteorder('>' if msg.is_bigendian else '<')
shape = (msg.height, msg.width, channels)
data = np.fromstring(msg.data, dtype=dtype).reshape(shape)
data.strides = (
msg.step,
dtype.itemsize * channels,
dtype.itemsize
)
if channels == 1:
data = data[...,0]
return data.astype(np.float64)
# dtype_class = np.uint16
# channels = 1
# dtype = np.dtype(dtype_class)
# dtype = dtype.newbyteorder('>' if image.is_bigendian else '<')
# shape = (image.height, image.width, channels)
# data = np.fromstring(image.data, dtype=dtype).reshape(shape)
# data.strides = (
# image.step,
# dtype.itemsize * channels,
# dtype.itemsize
# )
# data = data[:,:,0].astype(np.float64)
# cm_from_pixel = 0.095
# return data #*cm_from_pixel
def __getCentroid(self, seg_image):
centroid = ndimage.measurements.center_of_mass(seg_np)
if np.isnan(centroid[0]) or np.isnan(centroid[1]):
return None
#if (c[0] < 100) or (c[1] < 100):
# PIL.Image.fromarray((seg_np*255).astype('uint8')).show()
return centroid
def __convert_depth_frame_to_pointcloud(self, depth_image, camera_intrinsics, area=None):
#stolen from https://github.com/IntelRealSense/librealsense/blob/master/wrappers/python/examples/box_dimensioner_multicam/helper_functions.py#L151
"""
Convert the depthmap to a 3D point cloud
Parameters:
-----------
depth_frame : rs.frame()
The depth_frame containing the depth map
camera_intrinsics : The intrinsic values of the imager in whose coordinate system the depth_frame is computed
Return:
----------
x : array
The x values of the pointcloud in meters
y : array
The y values of the pointcloud in meters
z : array
The z values of the pointcloud in meters
"""
if area is None:
[height, width] = depth_image.shape
nx = np.linspace(0, width-1, width)
ny = np.linspace(0, height-1, height)
u, v = np.meshgrid(nx, ny)
else:
u = area[:, 1]
v = area[:, 0]
x = (u.flatten() - camera_intrinsics.ppx)/camera_intrinsics.fx
y = (v.flatten() - camera_intrinsics.ppy)/camera_intrinsics.fy
if area is None:
z = depth_image.flatten() / 1000
else:
z = depth_image[area[:,0], area[:,1]].flatten() / 1000
x = np.multiply(x,z)
y = np.multiply(y,z)
x = x[np.nonzero(z)]
y = y[np.nonzero(z)]
z = z[np.nonzero(z)]
return x, y, z
def __publishPointCloud(self, x, y, z, publ, step=100):
pointcloud = PointCloud()
header = std_msgs.msg.Header()
header.stamp = self.stamp_now #rospy.Time.now()
header.frame_id = 'camera_depth_optical_frame'
#header.frame_id = 'camera_link'
pointcloud.header = header
for i in range(0, len(x), step):
pointcloud.points.append(Point32(x[i], y[i], z[i]))
publ.publish(pointcloud)
def __rotate_frame(self, x, y, z):
x = x.reshape((x.shape[0], 1))
y = y.reshape((y.shape[0], 1))
z = z.reshape((z.shape[0], 1))
points = np.concatenate((x, y, z), axis=1)
#print(x.shape, points.shape)
r = Rotations()
points = (np.matmul(r.rotz(np.pi/2), np.matmul(r.rotx(np.pi/2), points.T))).T
x = points[:,0]
y = points[:,1]
z = points[:,2]
return x, y, z
def __orange_orientation(self, trans, rot, mean_pos, orientation=None):
if self.alpha != self.min_alpha:
delta = (self.alpha - self.min_alpha)/self.max_steps
self.alpha -= delta
print(self.alpha)
#decaying alpha in future
rot = R.from_quat(np.array(rot))
#fixed_transform = R.from_quat(np.array([-0.500, 0.500, 0.500, 0.500])).as_dcm()
temp_world2orange_pos = np.matmul(rot.as_dcm(), mean_pos) + np.array(trans)
if self.world2orange_pos is None:
self.world2orange_pos = temp_world2orange_pos.copy()
else:
self.world2orange_pos = self.world2orange_pos + self.alpha*(temp_world2orange_pos-self.world2orange_pos)
if orientation is not None:
rot_obj = Rotations()
orientation = R.from_dcm(np.matmul(np.matmul(rot.as_dcm(),R.from_quat(orientation).as_dcm()), rot_obj.rotx(np.pi/2))).as_quat()
if self.world2orange is None:
self.world2orange = R.from_quat(orientation).as_dcm()
else:
new_rot = orientation
old_rot = R.from_dcm(self.world2orange).as_quat()
key_times = [0, 1]
key_rots = np.array([old_rot, new_rot])
key_rots = R.from_quat(key_rots)
slerp = Slerp(key_times, key_rots)
times = [self.alpha]
mean_rot = slerp(times)
mean_rot = mean_rot[0]
self.world2orange = mean_rot.as_dcm()
self.br.sendTransform((self.world2orange_pos[0], self.world2orange_pos[1], self.world2orange_pos[2]),
R.from_dcm(self.world2orange).as_quat(),
self.stamp_now, #rospy.Time.now(),
"orange",
"world")
Rot = R.from_dcm(np.matmul(np.linalg.inv(rot.as_dcm()), self.world2orange)).as_quat()
Trans = np.matmul(np.linalg.inv(rot.as_dcm()), (self.world2orange_pos - np.array(trans)))
return Rot, Trans
def _publishGoalPoints(self, x, y, z, trans=None, rot=None, orientation=None, odom_data=None, num_points=1):
if trans is None or rot is None:
try:
# (trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame_filtered', rospy.Time(0))
(trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
if np.any(np.isnan(trans)) or np.any(np.isnan(rot)) or np.any(np.isnan(x)) or np.any(np.isnan(y)) or np.any(np.isnan(z)):
print("NANANNANANANANANNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNANANANANANANANANNANNANANA")
return
mean_x = np.mean(x)
mean_y = np.mean(y)
mean_z = np.mean(z)
# if odom_data is not None:
# rot = np.array([odom_data.pose.pose.orientation.x, odom_data.pose.pose.orientation.y, odom_data.pose.pose.orientation.z, odom_data.pose.pose.orientation.w])
orientation, mean_pos = self.__orange_orientation(trans, rot, np.array([mean_x, mean_y, mean_z]), orientation)
mean_x, mean_y, mean_z = mean_pos[0], mean_pos[1], mean_pos[2]
header = std_msgs.msg.Header()
header.stamp = self.stamp_now #rospy.Time.now()
header.frame_id = 'camera_depth_optical_frame_filtered'
header.frame_id = 'camera_depth_optical_frame'
#header.frame_id = 'camera_link'
goalarray_msg = PoseArray()
goalarray_msg.header = header
for i in range(num_points):
goal = Pose()
goal.position.x = mean_x
goal.position.y = mean_y
goal.position.z = mean_z
goal.orientation.x = orientation[0]
goal.orientation.y = orientation[1]
goal.orientation.z = orientation[2]
goal.orientation.w = orientation[3]
goalarray_msg.poses.append(goal)
# print("Publishing")
self.__goalpoint_publisher.publish(goalarray_msg)
def __skew(self, vec):
return np.array([[0, -vec[2], vec[1]],
[vec[2], 0, -vec[0]],
[-vec[1], vec[0], 0]])
def __find_rot(self, vector):
vector = np.array(vector)
vector = vector/np.linalg.norm(vector)
vector2 = np.array([1, 0, 0])
v = np.cross(vector, vector2)
s = np.linalg.norm(v)
c = np.dot(vector, vector2)
rot = np.eye(3) + self.__skew(v) + (np.matmul(self.__skew(v), self.__skew(v))*(1-c)/(s*s))
return R.from_dcm(rot).as_quat()
def debugPlanePlotter(self, pts, plane, orientation, position):
x = np.linspace(-2.,2.,20)
y = np.linspace(-2.,2.,20)
X,Y = np.meshgrid(x,y)
Z=(plane[0]*X + plane[1]*Y + plane[3])/-plane[2]
fig = plt.figure()
ax = fig.gca(projection='3d')
#surf = ax.plot_surface(X, Y, Z)
ax.scatter3D(X, Y, Z, c='b')
ax.scatter3D(pts[:,0], pts[:,1], pts[:,2], c='r')
# ax.set_aspect('equal')
ax.set_zlabel("x")
ax.set_xlabel("-y")
ax.set_ylabel("-z")
pos = np.array(position) + np.array(plane[:3])
print("Pos: ", pos)
ax.scatter3D(pos[0], pos[1], pos[2], c='g')
ax.scatter3D(pos[0], position[1], pos[2], c='k')
ax.scatter3D(position[0], position[1], position[2], c='y')
R_m = R.from_quat(orientation).as_dcm()
plot_basis(ax, R_m, np.array(position), alpha=1)
ax.set_xlim(-2. + position[0], 2.+ position[0])
ax.set_ylim(-2.+ position[1], 2.+ position[1])
ax.set_zlim(-2.+ position[2], 2.+ position[2])
ax.set_aspect('equal', 'box')
plt.show()
def __find_plane(self, x, y, z, Trans, Rot, mean_pt, debug=False):
#mean_pt = mean_pt/np.linalg.norm(mean_pt)
pts = np.array([x, y, z]).T
print(pts.shape)
t_size = np.min((500, pts.shape[0]))
t_pts = np.random.choice(np.arange(pts.shape[0]), size=t_size, replace=False)
pts = pts[t_pts]
print(pts.shape)
all_points = open3d.utility.Vector3dVector(pts)
pc = geometry.PointCloud(all_points)
plane, success_pts = pc.segment_plane(0.05, int(pts.shape[0]*0.7), 1000)
plane = np.array(plane)
# print("Plane:", pts.shape, len(success_pts))
if debug:
print("Plane: ", plane)
print("Mean pt: ", mean_pt)
plane_debug = plane.copy()
d = np.dot(mean_pt, plane[:3])
if d > 0:
plane = -plane
plane_ = plane[:3] #np.matmul(R.from_quat(Rot).as_dcm(), plane[:3].T)
plane_[1] = 0
plane_ /= np.linalg.norm(plane_)
y = np.array([0, -1, 0])
z = np.cross(plane_, y)
rot = np.array([plane_, y, z]).T
# print(rot, np.linalg.det(rot), R.from_dcm(rot).as_euler('ZYX', degrees=True))
orientation = R.from_dcm(rot).as_quat()
if debug:
self.debugPlanePlotter(pts, plane, orientation, position=mean_pt)
normal_vec = Pose()
normal_vec.position.x = plane_[0]
normal_vec.position.y = plane_[1]
# normal_vec.position.z = plane[2]
#orientation = [0, 0, 0, 1]#self.__find_rot(plane[0:3])
#normal_vec.orientation.x = orientation[0]
#normal_vec.orientation.y= orientation[1]
#normal_vec.orientation.z= orientation[2]
#normal_vec.orientation.w = orientation[3]
vecs = PoseArray()
vecs.poses.append(normal_vec)
header = std_msgs.msg.Header()
header.stamp = self.stamp_now #rospy.Time.now()
header.frame_id = "orange"
vecs.header = header
self.__normal_vec_publisher.publish(vecs)
# print(plane)
return orientation
def publishData(self, depth_image, camera_intrinsics, area, publ, Trans, Rot, mean_pt = None, norm_tracker=False, tracker=False):
if area.shape[0] > 30:
# print(area.shape)
x, y, z = self.__convert_depth_frame_to_pointcloud(depth_image, camera_intrinsics, area)
#print(x.shape, y.shape, z.shape)
if np.any(np.isnan(x)) or np.any(np.isnan(y)) or np.any(np.isnan(z)):
return
# print(np.mean(x), np.mean(y), np.mean(z))
# print(np.min(x),np.max(x),np.min(y),np.max(y), np.min(z),np.max(z))
# print("\n\n\n")
filter = True
if filter:
pts = np.array([x, y, z]).T
pts = self.reject_outliers3d(pts)
x, y, z = pts[:, 0], pts[:, 1], pts[:, 2]
total_points = 1000
step = int(x.shape[0]/total_points)
step = np.max((1, step))
#z -= 0.02
self.__publishPointCloud(x, y, z, publ, step=step)
if x.shape[0] < 10:
return None
orientation = None
if norm_tracker:
if mean_pt is None:
print("ISSUUEE")
exit(0)
#mean_pt = np.array([np.mean(x), np.mean(y), np.mean(z)])
orientation = self.__find_plane(x, y, z, Trans, Rot, mean_pt)
if tracker:
if orientation is None or mean_pt is None:
print("This should not happen!")
exit(0)
self._publishGoalPoints(mean_pt[0], mean_pt[1], mean_pt[2], orientation=orientation, trans=Trans, rot=Rot, odom_data=None)
else:
pass
#self._publishGoalPoints(x, y, z, orientation=orientation, odom_data=None)
return np.array([np.mean(x), np.mean(y), np.mean(z)])
else:
print("Failed in Publish Data check")
def reject_outliers(self, data, m=1.5):
return data[np.multiply(np.abs(data[:, 0] - np.mean(data[:, 0])) < m * np.std(data[:, 0]), np.abs(data[:,1] - np.mean(data[:,1])) < m * np.std(data[:,1]))]
def reject_outliers3d(self, data, m=3):
return data[np.multiply(np.multiply(np.abs(data[:, 0] - np.mean(data[:, 0])) < m * np.std(data[:, 0]), np.abs(data[:,1] - np.mean(data[:,1])) < m * np.std(data[:,1])), np.abs(data[:,2] - np.mean(data[:,2])) < m * np.std(data[:,2]))]
def reject_outliers3dMedian(self, data, m=3):
return data[np.multiply(np.multiply(np.abs(data[:, 0] - np.median(data[:, 0])) < m * np.std(data[:, 0]), np.abs(data[:,1] - np.median(data[:,1])) < m * np.std(data[:,1])), np.abs(data[:,2] - np.median(data[:,2])) < m * np.std(data[:,2]))]
def callback(self, image_data, depth_image_data):
print("Reaching callbazck")
self.t += 1
t1 = time.time()
self.stamp_now = image_data.header.stamp
image = self.__rosmsg2np(image_data)
depth_image = self.__depthnpFromImage(depth_image_data)
image_tensor = self.__process4model(image)
seg_np = self.__segmentationInference(image_tensor)
#centroid = self.__getCentroid(seg_np)
camera_intrinsics = CameraIntrinsics()
area = np.argwhere(seg_np == 1)
pre_area = area.copy()
# print("Orignal shape: ", area.shape)
if area.shape[0] < 30:
print("Early return??", area.shape)
return
area = self.reject_outliers(area)
# print("No outlier shape: ", area.shape)
mean_x, mean_y = np.mean(area, axis=0)
min_ = np.min(area, axis=0)
max_ = np.max(area, axis=0)
min_x, min_y = min_[0], min_[1]
max_x, max_y = max_[0], max_[1]
size_x = np.abs(max_x - min_x)
size_y = np.abs(max_y - min_y)
size_ = np.max((size_x, size_y))
# mult_x, mult_y = 1.5, 1.5
#extra_min_x, extra_max_x = np.max((0, min_x-int(mult_x*size_x))), np.min((seg_np.shape[0], max_x+int(mult_x*size_)))
#extra_min_y, extra_max_y = np.max((0, min_y-int(mult_y*size_y))), np.min((seg_np.shape[1], max_y+int(mult_y*size_)))
mult_x, mult_y = 2., 2.
extra_min_x, extra_max_x = np.max((0, mean_x-int(mult_x*size_x))), np.min((seg_np.shape[0], mean_x+int(mult_x*size_)))
extra_min_y, extra_max_y = np.max((0, mean_y-int(mult_y*size_y))), np.min((seg_np.shape[1], mean_y+int(mult_y*size_)))
nx = np.linspace(extra_min_x, extra_max_x-1, extra_max_x - extra_min_x, dtype=np.int32)
ny = np.linspace(extra_min_y, extra_max_y-1, extra_max_y - extra_min_y, dtype=np.int32)
extra_area = np.transpose([np.tile(nx, len(ny)), np.repeat(ny, len(nx))])
trans, rot = None, None
try:
# (trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame_filtered', rospy.Time(0))
(trans,rot) = self.listener.lookupTransform('world', 'camera_depth_optical_frame', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
if trans is None or rot is None:
print("tf None")
return
if area.shape[0] > 30:
# print("In orange: ")
mean_pt = self.publishData(depth_image, camera_intrinsics, area, self.__pointcloud_publisher, Trans=trans, Rot=rot, tracker=False)
if mean_pt is None:
return
# print("In extra orange ")
self.publishData(depth_image, camera_intrinsics, extra_area, self.__pointcloud_publisher_extra, Trans=trans, Rot=rot, mean_pt=mean_pt, norm_tracker=True, tracker=True)
# print(area.shape)
# x, y, z = self.__convert_depth_frame_to_pointcloud(depth_image, camera_intrinsics, area)
# #print(x.shape, y.shape, z.shape)
# if np.any(np.isnan(x)) or np.any(np.isnan(y)) or np.any(np.isnan(z)):
# return
# print(np.mean(x), np.mean(y), np.mean(z))
# # print(np.min(x),np.max(x),np.min(y),np.max(y), np.min(z),np.max(z))
# # print("\n\n\n")
# self.__publishPointCloud(x, y, z, step=20)
# #self.__find_plane(x, y, z)
# self._publishGoalPoints(x, y, z, odom_data=None)
print(time.time()-t1)
else:
print("Left pre-area check", area.shape, pre_area.shape)
class ArucoLocalization():
def __init__(self):
# Load params from launch file
param = rospy.search_param("subscribed_image_topic")
sub_img_topic = rospy.get_param(param)
param = rospy.search_param("aruco_image_topic")
aruco_image_topic = rospy.get_param(param)
param = rospy.search_param("qualcomm_pose_topic")
qualcomm_pose_topic = rospy.get_param(param)
param = rospy.search_param("cam_calib_path")
cam_calibration_path = rospy.get_param(param)
param = rospy.search_param("marker_map_path")
marker_map_path = rospy.get_param(param)
param = rospy.search_param("aruco_dictionary")
aruco_dict = rospy.get_param(param)
if aruco_dict == "4x4" or aruco_dict == "4X4" or str(
aruco_dict) == "4":
aruco_dictionary = cv2.aruco.DICT_4X4_250
elif aruco_dict == "5x5" or aruco_dict == "5X5" or str(
aruco_dict) == "5":
aruco_dictionary = cv2.aruco.DICT_5X5_250
elif aruco_dict == "6x6" or aruco_dict == "6X6" or str(
aruco_dict) == "6":
aruco_dictionary = cv2.aruco.DICT_6X6_250
else:
print("Invalid (or not included) aruco marker dictionary")
print(aruco_dictionary)
# Initialize CV bridge
self.bridge = CvBridge()
# Initialize publisher
self.pose_pub = rospy.Publisher(qualcomm_pose_topic,
PoseStamped,
queue_size=10)
self.im_pub = rospy.Publisher(aruco_image_topic, Image, queue_size=1)
# Initialize aruco details
self.cam_mtx, self.distort_mtx = self.loadCoefficients(
cam_calibration_path)
self.marker_set, self.marker_dict = self.load_markers(marker_map_path)
self.dictionary = cv2.aruco.Dictionary_get(aruco_dictionary)
self.parameters = cv2.aruco.DetectorParameters_create()
# Initialize transform from camera to qualcomm
tmat = translation_matrix((0.1, 0, 0.2))
qmat = quaternion_matrix(quaternion_from_euler(-1.57, 0, -1.57))
self.tf_cam2baselink = inverse_matrix(np.dot(tmat, qmat))
# Initialize subscriber to image stream
rospy.Subscriber(sub_img_topic, Image,
self.img_callback) #, queue_size = 1)
rospy.sleep(0.5) # Delay briefly to allow subscribers to find messages
def img_callback(self, msg):
try:
image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="mono8")
image = cv2.resize(
image,
(640, 480)) # Temporary until camera is calibrated properly
ids, corners = self.detectMarkersInMap(image)
pose, image = self.updatePoseFromMarkers(ids, corners, image)
self.im_pub.publish(self.bridge.cv2_to_imgmsg(
image, "passthrough"))
if pose:
qualcomm_pose = PoseStamped()
qualcomm_pose.pose = pose
qualcomm_pose.header.frame_id = "map"
qualcomm_pose.header.stamp = rospy.Time.now()
self.pose_pub.publish(qualcomm_pose)
except CvBridgeError as e:
print(e)
def updatePoseFromMarkers(self, ids, corners, image):
if ids:
xs, ys, yaws = [], [], []
for i, idx in enumerate(ids):
rvec, tvec = cv2.aruco.estimatePoseSingleMarkers(
corners[i], self.marker_dict[str(idx[0])].len,
self.cam_mtx, self.distort_mtx)
rvec = rvec[0][0]
tvec = tvec[0][0]
tmat = translation_matrix((tvec[0], tvec[1], tvec[2]))
qmat = np.zeros((4, 4))
qmat[:3, :3], _ = cv2.Rodrigues(rvec)
qmat[3, 3] = 1.
tf_cam2mark = np.dot(tmat, qmat)
tf_mark2cam = inverse_matrix(tf_cam2mark)
marker = self.marker_dict[str(idx[0])]
tf_map2cam = np.dot(marker.tf_mat, tf_cam2mark)
# trans = translation_from_matrix(tf_map2cam)
rot = euler_from_quaternion(quaternion_from_matrix(tf_map2cam))
tf_map2baselink = np.dot(tf_map2cam, self.tf_cam2baselink)
baselink_pose = Pose()
baselink_pose.position = Vector3(
*translation_from_matrix(tf_map2baselink))
baselink_pose.orientation = Quaternion(
*quaternion_from_matrix(tf_map2baselink))
print("detected marker", marker.id)
print("cam pose", baselink_pose)
# Draw axis on marker and publish image
image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
image = cv2.aruco.drawAxis(image, self.cam_mtx,
self.distort_mtx, rvec, tvec, 0.1)
"""
NOTE
Still need to take average of poses when multiple markers are detected
"""
return baselink_pose, image
else:
return None, image
def detectMarkersInMap(self, image):
"""
Detects all markers in camera view
and returns lists of ids and corners
of those that are included in the marker_map.yaml
"""
try:
corners, ids, _ = cv2.aruco.detectMarkers(
image, self.dictionary, parameters=self.parameters)
map_ids = []
map_corners = []
for i, idx in enumerate(ids):
if idx[0] in self.marker_set:
map_ids.append(idx)
map_corners.append(corners[i])
return map_ids, map_corners
except:
print("No aruco markers detected")
return None, None
@staticmethod
def load_markers(path):
"""
Loads a yaml file containing marker definitions
Returns a list of marker ids and a dictionary
of corresponding Marker() objects
"""
marker_dict = {}
marker_list = []
with open(path) as f:
raw_markers = yaml.load(f)
for marker in raw_markers:
id_ = raw_markers[marker][0]
len_ = raw_markers[marker][1]
x = raw_markers[marker][2][0]
y = raw_markers[marker][2][1]
z = raw_markers[marker][2][2]
rot_x = raw_markers[marker][3][0]
rot_y = raw_markers[marker][3][1]
rot_z = raw_markers[marker][3][2]
marker_list.append(id_)
marker_dict[str(id_)] = Marker(id_, len_, x, y, z, rot_x,
rot_y, rot_z)
return set(marker_list), marker_dict
@staticmethod
def loadCoefficients(path):
"""
Loads a yaml file containing camera calibration values
generated by calibration/camera_calibration.py
"""
with open(path) as f:
calibration_data = yaml.load(f)
#Camera matrix and distortion
cam_mtx = calibration_data.get("camera_matrix")
distort_mtx = calibration_data.get("dist_coeff")
cam_mtx = np.asarray(cam_mtx)
distort_mtx = np.asarray(distort_mtx)
return cam_mtx, distort_mtx
class LineDetectorNode(object):
def __init__(self):
self.node_name = "LineDetectorNode"
# Thread lock
self.thread_lock = threading.Lock()
# Constructor of line detector
self.bridge = CvBridge()
self.active = True
self.stats = Stats()
# Only be verbose every 10 cycles
self.intermittent_interval = 100
self.intermittent_counter = 0
# color correction
self.ai = AntiInstagram()
# these will be added if it becomes verbose
self.pub_edge = None
self.pub_colorSegment = None
self.detector = None
self.verbose = None
self.updateParams(None)
# Publishers
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines",
Image,
queue_size=1)
# Subscribers
self.sub_image = rospy.Subscriber("~image",
CompressedImage,
self.cbImage,
queue_size=1)
self.sub_transform = rospy.Subscriber("~transform",
AntiInstagramTransform,
self.cbTransform,
queue_size=1)
self.sub_switch = rospy.Subscriber("~switch",
BoolStamped,
self.cbSwitch,
queue_size=1)
rospy.loginfo("[%s] Initialized (verbose = %s)." %
(self.node_name, self.verbose))
rospy.Timer(rospy.Duration.from_sec(2.0), self.updateParams)
def updateParams(self, _event):
old_verbose = self.verbose
self.verbose = rospy.get_param('~verbose', True)
# self.loginfo('verbose = %r' % self.verbose)
if self.verbose != old_verbose:
self.loginfo('Verbose is now %r' % self.verbose)
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
if self.detector is None:
c = rospy.get_param('~detector')
assert isinstance(c, list) and len(c) == 2, c
# if str(self.detector_config) != str(c):
self.loginfo('new detector config: %s' % str(c))
self.detector = instantiate(c[0], c[1])
# self.detector_config = c
if self.verbose and self.pub_edge is None:
self.pub_edge = rospy.Publisher("~edge", Image, queue_size=1)
self.pub_colorSegment = rospy.Publisher("~colorSegment",
Image,
queue_size=1)
def cbSwitch(self, switch_msg):
self.active = switch_msg.data
def cbImage(self, image_msg):
self.stats.received()
if not self.active:
return
# Start a daemon thread to process the image
thread = threading.Thread(target=self.processImage, args=(image_msg, ))
thread.setDaemon(True)
thread.start()
# Returns rightaway
def cbTransform(self, transform_msg):
self.ai.shift = transform_msg.s[0:3]
self.ai.scale = transform_msg.s[3:6]
self.loginfo("AntiInstagram transform received")
def loginfo(self, s):
rospy.loginfo('[%s] %s' % (self.node_name, s))
def intermittent_log_now(self):
return self.intermittent_counter % self.intermittent_interval == 1
def intermittent_log(self, s):
if not self.intermittent_log_now():
return
self.loginfo('%3d:%s' % (self.intermittent_counter, s))
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
self.stats.skipped()
# Return immediately if the thread is locked
return
try:
self.processImage_(image_msg)
finally:
# Release the thread lock
self.thread_lock.release()
def processImage_(self, image_msg):
self.stats.processed()
if self.intermittent_log_now():
self.intermittent_log(self.stats.info())
self.stats.reset()
tk = TimeKeeper(image_msg)
self.intermittent_counter += 1
# Decode from compressed image with OpenCV
try:
image_cv = image_cv_from_jpg(image_msg.data)
except ValueError as e:
self.loginfo('Could not decode image: %s' % e)
return
tk.completed('decoded')
# Resize and crop image
hei_original, wid_original = image_cv.shape[0:2]
if self.image_size[0] != hei_original or self.image_size[
1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv,
(self.image_size[1], self.image_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:, :, :]
tk.completed('resized')
# apply color correction: AntiInstagram
image_cv_corr = self.ai.applyTransform(image_cv)
image_cv_corr = cv2.convertScaleAbs(image_cv_corr)
tk.completed('corrected')
# Set the image to be detected
self.detector.setImage(image_cv_corr)
# Detect lines and normals
white = self.detector.detectLines('white')
yellow = self.detector.detectLines('yellow')
red = self.detector.detectLines('red')
tk.completed('detected')
# SegmentList constructor
segmentList = SegmentList()
segmentList.header.stamp = image_msg.header.stamp
# Convert to normalized pixel coordinates, and add segments to segmentList
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array(
(1. / self.image_size[1], 1. / self.image_size[0],
1. / self.image_size[1], 1. / self.image_size[0]))
if len(white.lines) > 0:
lines_normalized_white = ((white.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_white, white.normals,
Segment.WHITE))
if len(yellow.lines) > 0:
lines_normalized_yellow = ((yellow.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_yellow, yellow.normals,
Segment.YELLOW))
if len(red.lines) > 0:
lines_normalized_red = ((red.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_red, red.normals,
Segment.RED))
self.intermittent_log(
'# segments: white %3d yellow %3d red %3d' %
(len(white.lines), len(yellow.lines), len(red.lines)))
tk.completed('prepared')
# Publish segmentList
self.pub_lines.publish(segmentList)
tk.completed('--pub_lines--')
# VISUALIZATION only below
if self.verbose:
# Draw lines and normals
image_with_lines = np.copy(image_cv_corr)
drawLines(image_with_lines, white.lines, (0, 0, 0))
drawLines(image_with_lines, yellow.lines, (255, 0, 0))
drawLines(image_with_lines, red.lines, (0, 255, 0))
tk.completed('drawn')
# Publish the frame with lines
image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
tk.completed('pub_image')
# if self.verbose:
colorSegment = color_segment(white.area, red.area, yellow.area)
edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector.edges,
"mono8")
colorSegment_msg_out = self.bridge.cv2_to_imgmsg(
colorSegment, "bgr8")
self.pub_edge.publish(edge_msg_out)
self.pub_colorSegment.publish(colorSegment_msg_out)
tk.completed('pub_edge/pub_segment')
self.intermittent_log(tk.getall())
def onShutdown(self):
self.loginfo("Shutdown.")
def toSegmentMsg(self, lines, normals, color):
segmentMsgList = []
for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)):
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segmentMsgList.append(segment)
return segmentMsgList
class DetectLine:
def __init__(self):
# 创建cv_bridge，声明图像的发布者和订阅者
self.image_pub = rospy.Publisher("cv_bridge_image",
Image,
queue_size=1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image,
self.callback)
self.rgb_image = None
self.tile_pub = rospy.Publisher("/tile_uv", tileuv, queue_size=10)
def callback(self, data):
try:
pass
# video_capture=self.bridge.imgmsg_to_cv2(data,"bgr8")
# self.rgb_image = video_capture.copy()
# self.rgb_image = cv2.imread('/data/ros/ur_ws_yue/src/tile_robot/Impedance_control/1.jpg')
except CvBridgeError as e:
print e
def segment_by_angle_kmeans(self, lines, k=2, **kwargs):
"""Groups lines based on angle with k-means.
Uses k-means on the coordinates of the angle on the unit circle
to segment `k` angles inside `lines`.
"""
# Define criteria = (type, max_iter, epsilon)
default_criteria_type = cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER
criteria = kwargs.get('criteria', (default_criteria_type, 10, 1.0))
flags = kwargs.get('flags', cv2.KMEANS_PP_CENTERS)
attempts = kwargs.get('attempts', 10)
# returns angles in [0, pi] in radians
angles = np.array([line[0][1] for line in lines])
print "angles", angles
# multiply the angles by two and find coordinates of that angle
pts = np.array(
[[np.cos(2 * angle), np.sin(2 * angle)] for angle in angles],
dtype=np.float32)
# run kmeans on the coords
# print pts
# labels1 = np.zeros(k)
labels, centers = cv2.kmeans(pts, k, None, criteria, attempts,
flags)[1:]
print "labels", labels, centers
labels = labels.reshape(-1) # transpose to row vec
print "labels", labels
# segment lines based on their kmeans label
segmented = defaultdict(list)
segmented0 = defaultdict(list)
segmented1 = defaultdict(list)
for i, line in zip(range(len(lines)), lines):
print i, line
segmented[labels[i]].append(line)
print "before segmented", segmented
segmented = list(segmented.values())
segmented0 = list(segmented[0])
segmented1 = list(segmented[1])
print "after segmented\n", segmented
return segmented, segmented0, segmented1
def convert_hsv(self, image):
return cv2.cvtColor(image, cv2.COLOR_RGB2HSV)
def convert_hls(self, image):
return cv2.cvtColor(image, cv2.COLOR_RGB2HLS)
# image is expected be in RGB color space
def select_rgb_white_yellow(self, image):
# white color mask
lower = np.uint8([200, 200, 200])
upper = np.uint8([255, 255, 255])
white_mask = cv2.inRange(image, lower, upper)
# yellow color mask
lower = np.uint8([190, 190, 0])
upper = np.uint8([255, 255, 255])
yellow_mask = cv2.inRange(image, lower, upper)
# combine the mask
mask = cv2.bitwise_or(white_mask, yellow_mask)
masked = cv2.bitwise_and(image, image, mask=mask)
return masked
def convert_gray_scale(self, image):
return cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
def apply_smoothing(self, image, kernel_size=15):
"""
kernel_size must be postivie and odd
"""
return cv2.GaussianBlur(image, (kernel_size, kernel_size), 0)
def detect_edges(self, image, low_threshold=50, high_threshold=150):
return cv2.Canny(image, low_threshold, high_threshold)
def select_white(self, image):
converted = self.convert_hls(image)
# converted = self.convert_hsv(image)
# print "con-------",converted
# white color mask
#white
lower = np.uint8([0, 200, 0])
upper = np.uint8([255, 255, 255])
# lower = np.uint8([10, 0, 100])
# upper = np.uint8([40, 255, 255])
white_mask = cv2.inRange(converted, lower, upper)
#yellow color mask
lower = np.uint8([0, 200, 0])
upper = np.uint8([255, 255, 255])
yellow_mask = cv2.inRange(converted, lower, upper)
# combine the mask
mask = cv2.bitwise_or(white_mask, yellow_mask)
return cv2.bitwise_and(image, image, mask=mask)
def select_yellow(self, image):
converted = self.convert_hls(image)
# converted = self.convert_hsv(image)
lower = np.uint8([10, 0, 100])
upper = np.uint8([40, 255, 255])
white_mask = cv2.inRange(converted, lower, upper)
#yellow color mask
lower = np.uint8([10, 0, 100])
upper = np.uint8([40, 255, 255])
yellow_mask = cv2.inRange(converted, lower, upper)
# combine the mask
mask = cv2.bitwise_or(white_mask, yellow_mask)
return cv2.bitwise_and(image, image, mask=mask)
def select_white_yellow(self, image):
converted = self.convert_hls(image)
# converted = self.convert_hsv(image)
# print "con-------",converted
# white color mask
#white
lower = np.uint8([0, 200, 0])
upper = np.uint8([255, 255, 255])
white_mask = cv2.inRange(converted, lower, upper)
#yellow color mask
lower = np.uint8([10, 0, 100])
upper = np.uint8([40, 255, 255])
yellow_mask = cv2.inRange(converted, lower, upper)
# combine the mask
mask = cv2.bitwise_or(white_mask, yellow_mask)
return cv2.bitwise_and(image, image, mask=mask)
def filter_region(self, image, vertices):
"""
Create the mask using the vertices and apply it to the input image
"""
mask = np.zeros_like(image)
if len(mask.shape) == 2:
cv2.fillPoly(mask, vertices, 255)
else:
cv2.fillPoly(mask, vertices, (255, ) * mask.shape[2]
) # in case, the input image has a channel dimension
return cv2.bitwise_and(image, mask)
def select_region(self, image, bottom_left_cols1, bottom_left_rows1,
top_left_cols1, top_left_rows1, bottom_right_cols1,
bottom_right_rows1, top_right_cols1, top_right_rows1):
"""
It keeps the region surrounded by the `vertices` (i.e. polygon). Other area is set to 0 (black).
bottom_left_cols1=0.53
bottom_left_rows1=0.70
top_left_cols1=0.53
top_left_rows1=0.28
bottom_right_cols1=0.95
bottom_right_rows1=0.70
top_right_cols1=0.99
top_right_rows1=0.28
"""
# first, define the polygon by vertices
rows, cols = image.shape[:2]
bottom_left = [cols * bottom_left_cols1, rows * bottom_left_rows1]
top_left = [cols * top_left_cols1, rows * top_left_rows1]
bottom_right = [cols * bottom_right_cols1, rows * bottom_right_rows1]
top_right = [cols * top_right_cols1, rows * top_right_rows1]
# the vertices are an array of polygons (i.e array of arrays) and the data type must be integer
vertices = np.array([[bottom_left, top_left, top_right, bottom_right]],
dtype=np.int32)
return self.filter_region(image, vertices)
def hough_lines(self, image):
"""
`image` should be the output of a Canny transform.
Returns hough lines (not the image with lines)
"""
return cv2.HoughLinesP(image,
rho=1,
theta=np.pi / 180,
threshold=20,
minLineLength=20,
maxLineGap=300)
def Draw_triangle(self, contours, rgb, tile_id, obj_desire):
##################
DELAY = 0.02
USE_CAM = 1
IS_FOUND = 0
count = 0 #count feature tile numbers
cnt = 0
central_list = []
uvuv = uv()
tile_uv = tileuv()
##################
_width = 480.0
_height = 640.0
_margin = 0.0
corners = np.array([
[[_margin, _margin]],
[[_margin, _height + _margin]],
[[_width + _margin, _height + _margin]],
[[_width + _margin, _margin]],
])
pts_dst = np.array(corners, np.float32)
for cont in contours:
resultuv = []
"""
#1,num,2,centeral point 3,for angular point uv ,4,clockwise direction
#caculating Area for tile selected just one tile
"""
if cv2.contourArea(cont) > 5000 and cv2.contourArea(cont) < 60000:
# print "cont----------", cont
# 获取轮廓长度
arc_len = cv2.arcLength(cont, True)
# 多边形拟合
approx = cv2.approxPolyDP(cont, 0.1 * arc_len, True)
if (len(approx) == 4):
IS_FOUND = 1
M = cv2.moments(cont)
# 获取图像质心坐标
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.putText(rgb, obj_desire, (cX, cY),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
print "CX,CY", [cX, cY]
central_list.append([cX, cY])
pts_src = np.array(approx, np.float32)
# print "pts_src", pts_src
cv2.circle(rgb, (cX, cY), 5, (0, 0, 0), -1)
print approx.tolist()
angular_point = []
for i in range(len(approx.tolist())):
if i == 0:
cv2.circle(rgb, (approx.tolist()[i][0][0],
approx.tolist()[i][0][1]), 5,
(20, 60, 220), -1)
print "first point x,y,others use clockwise---", approx.tolist()[i][0][0], \
approx.tolist()[i][0][1]
angular_point.append([
approx.tolist()[i][0][0],
approx.tolist()[i][0][1]
])
cv2.putText(rgb, str(i),
(approx.tolist()[i][0][0],
approx.tolist()[i][0][1]),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 0), 2)
else:
cv2.circle(rgb, (approx.tolist()[i][0][0],
approx.tolist()[i][0][1]), 5,
(0, 255, 0), -1)
print "x,y", approx.tolist(
)[i][0][0], approx.tolist()[i][0][1]
angular_point.append([
approx.tolist()[i][0][0],
approx.tolist()[i][0][1]
])
print "chr(i)", str(i)
cv2.putText(rgb, str(i),
(approx.tolist()[i][0][0],
approx.tolist()[i][0][1]),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1,
(0, 0, 0), 2)
resultuv.append([[tile_id], [cX, cY], angular_point])
# draw trangle in image
h, status = cv2.findHomography(pts_src, pts_dst)
out = cv2.warpPerspective(rgb, h,
(int(_width + _margin * 2),
int(_height + _margin * 2)))
cv2.drawContours(rgb, [approx], -1, (0, 255, 255), 3)
print "all info for tile------", resultuv
print "Now tile id", tile_id
tile_uv.tile_id = tile_id
tile_uv.obj_desire = obj_desire
tile_uv.cen_uv.uvinfo = [cX, cY]
tile_uv.f1th_uv.uvinfo = angular_point[0]
tile_uv.s2th_uv.uvinfo = angular_point[1]
tile_uv.t3th_uv.uvinfo = angular_point[2]
tile_uv.f4th_uv.uvinfo = angular_point[3]
self.tile_pub.publish(tile_uv)
else:
pass
# count += 1
# cnt += 11
return rgb.copy()
def pub_empty_uv_info(self, tile_id, obj_desire):
uvuv = uv()
tile_uv = tileuv()
tile_uv.tile_id = tile_id
tile_uv.obj_desire = obj_desire
tile_uv.cen_uv.uvinfo = [0, 0]
tile_uv.f1th_uv.uvinfo = [0, 0]
tile_uv.s2th_uv.uvinfo = [0, 0]
tile_uv.t3th_uv.uvinfo = [0, 0]
tile_uv.f4th_uv.uvinfo = [0, 0]
self.tile_pub.publish(tile_uv)
def intersection(self, line1, line2):
"""Finds the intersection of two lines given in Hesse normal form.
Returns closest integer pixel locations.
See https://stackoverflow.com/a/383527/5087436
"""
rho1, theta1 = line1[0]
rho2, theta2 = line2[0]
A = np.array([[np.cos(theta1), np.sin(theta1)],
[np.cos(theta2), np.sin(theta2)]])
b = np.array([[rho1], [rho2]])
x0, y0 = np.linalg.solve(A, b)
x0, y0 = int(np.round(x0)), int(np.round(y0))
return [[x0, y0]]
def draw_line(self, img, segmented, color):
for ii in xrange(len(segmented)):
rho, theta = np.array(segmented[ii][0])
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
x1 = int(x0 + 1000 * (-b))
y1 = int(y0 + 1000 * (a))
x2 = int(x0 - 1000 * (-b))
y2 = int(y0 - 1000 * (a))
cv2.line(img, (x1, y1), (x2, y2), color, 6)
def segmented_intersections(self, lines):
"""Finds the intersections between groups of lines."""
intersections = []
for i, group in enumerate(lines[:-1]):
for next_group in lines[i + 1:]:
for line1 in group:
for line2 in next_group:
intersections.append(self.intersection(line1, line2))
return intersections
def process_rgb_image(self, rgb_image):
MORPH = 7
CANNY = 250
##################
rgb = rgb_image
if rgb_image is not None:
"""'
Select White Desire position
tile_id=0,fixed point
obj_desire="d"
"""
WHLS = self.select_white(rgb)
Y_gray = self.convert_gray_scale(WHLS)
# blur = cv2.medianBlur(Y_gray, 1)
Y_smooth = self.apply_smoothing(Y_gray, 3)
Y_edges = self.detect_edges(Y_smooth)
# adapt_type = cv2.ADAPTIVE_THRESH_GAUSSIAN_C
# thresh_type = cv2.THRESH_BINARY_INV
# bin_img = cv2.adaptiveThreshold(blur, 255, adapt_type, thresh_type, 7, 2)
# rho, theta, thresh = 2, np.pi / 180, 400
# lines = cv2.HoughLines(bin_img, rho, theta, thresh)
lines = self.hough_lines(Y_edges)
segmented, segmented0, segmented1 = self.segment_by_angle_kmeans(
lines)
print lines
# print np.array(segmented)
# intersections = self.segmented_intersections(segmented)
# print intersections
self.draw_line(rgb_image, segmented0, (0, 0, 255))
self.draw_line(rgb_image, segmented1, (100, 0, 255))
# for i in intersections:
# cv2.circle(rgb_image, (i[0][0], i[0][1]), 5, (255, 0, 0), -1)
"""
HLS SPACE
"""
HLSDOUBLE = self.convert_hls(rgb)
cv2.namedWindow('HLSDOUBLE_Space', cv2.WINDOW_NORMAL)
cv2.imshow('HLSDOUBLE_Space', HLSDOUBLE)
cv2.namedWindow('White_HLS_Space', cv2.WINDOW_NORMAL)
cv2.imshow('White_HLS_Space', WHLS)
#
# cv2.namedWindow( 'White_tile_edges', cv2.WINDOW_NORMAL )
# cv2.imshow( 'White_tile_edges', W_edges )
#
# cv2.namedWindow( 'Yellow_HLS_Space', cv2.WINDOW_NORMAL )
# cv2.imshow( 'Yellow_HLS_Space', YHLS )
#
# cv2.namedWindow( 'Yellow_tile_edges', cv2.WINDOW_NORMAL )
# cv2.imshow( 'Yellow_tile_edges', Y_edges )
#
cv2.namedWindow('tile_pixel_frame', cv2.WINDOW_NORMAL)
cv2.imshow('tile_pixel_frame', rgb_image)
cv2.waitKey(8)
# # 再将opencv格式额数据转换成ros image格式的数据发布
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(rgb_image, "bgr8"))
except CvBridgeError as e:
print e
# return central_list
def papers_alog(self, rgb_image):
self.process_rgb_image(rgb_image)
def receiver(filename=['left.yaml','right.yaml'],CLIENT_PORT=(50677,50678)):
global image_size
image_size=[640,320]
CLIENT_IP='10.42.0.1'
sockets=[]
sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
sockets.append(sock)
sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
sockets.append(sock)
sockets[0].bind(('', CLIENT_PORT[0]))
sockets[1].bind(('', CLIENT_PORT[1]))
pic=['','']
temp=[-1,-1]
# ROS stuf
image_pub=[]
image_pub.append(rospy.Publisher("/camera/left/image_raw",Image,queue_size=10))
image_pub.append(rospy.Publisher("/camera/right/image_raw",Image,queue_size=10))
cameraInfo_pub=[]
cameraInfo_pub.append(rospy.Publisher("/camera/left/camera_info",CameraInfo,queue_size=10))
cameraInfo_pub.append(rospy.Publisher("/camera/right/camera_info",CameraInfo,queue_size=10))
rospy.init_node('camera_streamer',anonymous=True)
bridge=CvBridge()
camera_info=[CameraInfo()]*2
parse_yaml(filename[0],camera_info[0])
parse_yaml(filename[1],camera_info[1])
################################################################################
while True:
temp=[-1,-1]
flag=[0]*2
data=['','']
addr=['','']
seq=['','']
img=['','']
res=['','']
pic=['','']
for index in xrange(2):
while True:
data[index], addr[index] = sockets[index].recvfrom(image_size[0]*image_size[1])
seq[index]=struct.unpack('I',data[index][0:4])[0]
try:
if (struct.unpack('I',data[index][4:8])[0]==9999):
#print str(index)+' broke'
break
except:
pass
if seq[index]-1 > temp[index]:
print str(index)+' error'
flag[index]=1
temp[index]=seq[index]
pic[index]=pic[index]+data[index][4:]
for index in xrange(2):
if seq[index] != temp[index]+1:
flag[index]=1
print str(index)+ " flag error $!"
try:
if flag[index]==0:
img[index] = cv2.imdecode(np.asarray(bytearray(pic[index]),dtype=np.uint8), 1)
res[index] = cv2.resize(img[index],None,fx=1, fy=1, interpolation = cv2.INTER_CUBIC)
except:
print str(index)+ " flag error %@"
flag[index]=1
try:
if flag[0]==0 and flag[1]==0:
cv2.imshow('left_frame',res[0])
cv2.imshow('right_frame',res[1])
rosTime=rospy.Time.from_sec(time.time())
camera_info[0].header.stamp=rosTime
camera_info[1].header.stamp=rosTime
cameraInfo_pub[0].publish(camera_info[0])
cameraInfo_pub[1].publish(camera_info[1])
image_pub[0].publish(bridge.cv2_to_imgmsg(img[0], "bgr8"))
image_pub[1].publish(bridge.cv2_to_imgmsg(img[1], "bgr8"))
if cv2.waitKey(1) & 0xFF == ord('q'):
break;
except:
pass
pic=['','']
class Converter(object):
def __init__(self):
'''
Publisher initializations
'''
self.out_right = rospy.Publisher('omni_right/image_raw',Image, queue_size = 10)
self.out_left = rospy.Publisher('omni_left/image_raw',Image , queue_size = 10)
self.rect_right = rospy.Publisher('omni_right/rect_image' , Image , queue_size = 10)
self.rect_left = rospy.Publisher('omni_left/rect_image' , Image , queue_size = 10)
self.left_cam_calib_info = rospy.Publisher('omni_left_info' , Omni, queue_size = 10)
self.right_cam_calib_info = rospy.Publisher('omni_right_info' , Omni, queue_size = 10)
'''
Omni msg type object
'''
self.msg_left = Omni()
self.msg_right = Omni()
self.bridge = CvBridge() ## Cvbridge object
'''
Reading the calibration results
'''
self.right_path = '../Results_OcamCalib/calib_results_right.txt'
self.left_path = '../Results_OcamCalib/calib_results_left.txt'
self.ocam_model_right = get_ocam_model(self.right_path)
self.ocam_model_left = get_ocam_model(self.left_path)
self.msg_left.len_pol = self.ocam_model_left['length_pol']
self.msg_left.pols = self.ocam_model_left['pols']
self.msg_left.len_invpol = self.ocam_model_left['length_invpol']
self.msg_left.invpols = self.ocam_model_left['inv_pols']
self.msg_left.cx = self.ocam_model_left['xc']
self.msg_left.cy = self.ocam_model_left['yc']
self.msg_left.height = self.ocam_model_left['height']
self.msg_left.width = self.ocam_model_left['width']
self.msg_right.len_pol = self.ocam_model_right['length_pol']
self.msg_right.pols = self.ocam_model_right['pols']
self.msg_right.len_invpol = self.ocam_model_right['length_invpol']
self.msg_right.invpols = self.ocam_model_right['inv_pols']
self.msg_right.cx = self.ocam_model_right['xc']
self.msg_right.cy = self.ocam_model_right['yc']
self.msg_right.height = self.ocam_model_right['height']
self.msg_right.width = self.ocam_model_right['width']
self.scalingFactor = 3.2
(self.mapx_right , self.mapy_right) = create_perspective_undistortion_LUT(self.ocam_model_right , self.scalingFactor)
(self.mapx_left , self.mapy_left) = create_perspective_undistortion_LUT(self.ocam_model_left , self.scalingFactor)
self.mapx_right = self.mapx_right.astype("float32")
self.mapy_right = self.mapy_right.astype("float32")
self.mapx_left = self.mapx_left.astype("float32")
self.mapy_left = self.mapy_left.astype("float32")
rospy.loginfo("Omni Cam Node Starting Publishing") ## logging info stating the node has started publishing
self.image_sub = rospy.Subscriber('/omni_cam/image_raw',Image,self.callback) ## subscribes to topic mentioned
def callback(self,data): ## callback function of image_sub
time = data.header.stamp ## storing timestamp of orig image, to be used for left and right image header time stamp
'''
Conversion of Image msg to cv, also rotating the image, assuming the came in upright, if not remove the rotation part
'''
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
right , left = self.separate(cv_image)
## rotation part for left image
scale = 1.0
rows , cols = left.shape[:2]
M1 = cv2.getRotationMatrix2D((cols/2, rows/2) , -90 , scale)
cos = abs(M1[0,0])
sin = abs(M1[0,1])
nw = int((rows*sin) + (cols*cos))
nh = int((rows*cos) + (cols*sin))
M1[0,2] += (nw/2) - cols/2
M1[1,2] += (nh/2) - rows/2
left = cv2.warpAffine(left , M1 , (nw , nh))
## rotation part for right image
rows , cols = right.shape[:2]
M2 = cv2.getRotationMatrix2D((cols/2 , rows/2) , 90 , scale)
cos = abs(M2[0,0])
sin = abs(M2[0,1])
nw = int((rows*sin) + (cols*cos))
nh = int((rows*cos) + (cols*sin))
M2[0,2] += (nw/2) - cols/2
M2[1,2] += (nh/2) - rows/2
right = cv2.warpAffine(right , M2 , (cols , rows))
try:
'''
Sending the Image msg
'''
omni_left = self.bridge.cv2_to_imgmsg(left,"bgr8")
omni_left.header.stamp = time
omni_left.header.frame_id = "omni_left"
omni_right = self.bridge.cv2_to_imgmsg(right,"bgr8")
omni_right.header.stamp = omni_left.header.stamp
omni_right.header.frame_id = "omni_right"
self.out_right.publish(omni_right)
self.out_left.publish(omni_left)
except CvBridgeError as e:
print e
## conversion to grayscale for remapping with mapx,mapy obtained from the undistortion method
gray_right = cv2.cvtColor(right , cv2.COLOR_BGR2GRAY)
gray_left = cv2.cvtColor(left , cv2.COLOR_BGR2GRAY)
rect_left = cv2.remap(gray_left , self.mapx_left , self.mapy_left , cv2.INTER_LINEAR)
rect_right = cv2.remap(gray_right , self.mapx_right , self.mapy_right , cv2.INTER_LINEAR)
try:
'''
conversion to BGR, it does not change the gray image to color, just adds the same value to all 3 channels,
image view node does not accept the format if not converted to BGR.
'''
color_left = cv2.cvtColor(rect_left , cv2.COLOR_GRAY2BGR)
color_right = cv2.cvtColor(rect_right , cv2.COLOR_GRAY2BGR)
rectified_left = self.bridge.cv2_to_imgmsg(color_left,"bgr8")
rectified_left.header.stamp = time
rectified_left.header.frame_id = "omni_rect_left"
rectified_right = self.bridge.cv2_to_imgmsg(left,"bgr8")
rectified_right.header.stamp = time
rectified_right.header.frame_id = "omni_rect_right"
self.rect_left.publish(rectified_left)
self.rect_right.publish(rectified_right)
except CvBridgeError as e:
print e
'''
publishing the camera info, affine parameters and co-efficients
'''
self.left_cam_calib_info.publish(self.msg_left)
self.right_cam_calib_info.publish(self.msg_right)
## method implements separation of images
def separate(self,img):
height,width = img.shape[:2]
start_row , start_col = int(0) , int(0)
end_row , end_col = int(height) , int(width * .5)
cropped_left = img[start_row : end_row , start_col : end_col]
start_row_right , start_col_right = int(0) , int(width * .5)
end_row_right , end_col_right = int(height) , int(width)
cropped_right = img[start_row_right : end_row_right , start_col_right : end_col_right]
return cropped_right,cropped_left
class line_follower:
def __init__(self):
# publish to "image_topic_2" topic using message type Image
self.image_pub = rospy.Publisher("image_topic_2", Image, queue_size=1)
self.bridge = CvBridge()
# subscribe to raw image using message type Image; do callback when there is an image
self.image_sub = rospy.Subscriber("/rrbot/camera1/image_raw", Image,
self.callback)
self.move_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
def callback(self, data):
global lastError
global lineOnRight
global lineOnLeft
global x
global z
try:
# convert ROS image message into grayscale image
# mono8 for grayscale, passthrough for no change
cv_image = self.bridge.imgmsg_to_cv2(data, 'mono8')
except CvBridgeError as e:
print(e)
(rows, cols) = cv_image.shape
centx = int(cols / 2)
centy = rows - 50
cv2.circle(cv_image, (int(cols / 2), rows - 50), 10, 255)
#print(cv_image[50,50])
# show encoded image in image window
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
#loop twice a second
#rate = rospy.Rate(2)
colour_arr = []
i = 0
while (i < 16):
colour_arr.append(cv_image[750, i * 50])
i += 1
num = 0
denom = 0
for i in range(len(colour_arr)):
num += i * 100 * colour_arr[i]
denom += colour_arr[i]
position = num / denom
print(position)
print(colour_arr)
error = position - 750
kP = 0.1
kD = 0.06
speedAdjustment = kP * error + kD * (error - lastError)
lastError = error
baseSpeed = 1.5
onWhite = 1
# if the line is lost:
for i in colour_arr:
if i < 100:
onWhite = 0
break
if onWhite == 1:
if lineOnLeft == 1:
x = 0
z = 2
print("searching, left")
elif lineOnRight == 1:
x = 0
z = -2
print("searching, right")
else:
x = baseSpeed
z = baseSpeed + speedAdjustment
lav = int((colour_arr[0] + colour_arr[1] + colour_arr[2] +
colour_arr[3] + colour_arr[4]) / 5)
rav = int((colour_arr[15] + colour_arr[14] + colour_arr[13] +
colour_arr[12] + colour_arr[11]) / 5)
if rav < 110:
print("rav")
print(rav)
lineOnRight = 1
lineOnLeft = 0
elif lav < 110:
print("lav")
print(lav)
lineOnLeft = 1
lineOnRight = 0
else:
lineOnRight = 0
lineOnLeft = 0
if (z > 8):
z = 8
if (z < -8):
z = -8
move = Twist()
move.linear.x = x
move.angular.z = z
print(z)
#print(height, width)
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(cv_image, "passthrough"))
self.move_pub.publish(move)
except CvBridgeError as e:
print(e)
class pointCloud:
def __init__(self):
self.bridge = CvBridge()
#self.image_sub = rospy.Subscriber("/rtabmap/cloud_map",PointCloud2,self.callback)
self.image_sub = rospy.Subscriber("/orb_slam2_mono/map_points",PointCloud2,self.callback)
self.pose_sub = rospy.Subscriber("/orb_slam2_mono/pose",PoseStamped,self.callback_pose)
self.bebop_state_sub= rospy.Subscriber('/bebop/state', String, self.bebop_state_callback)
self.image_pub = rospy.Publisher("/bebop/image_map",Image, queue_size=1)
self.start_pub = rospy.Publisher("/bebop/start_map",Point, queue_size=1)
self.goal_pub = rospy.Publisher("/bebop/goal_map",Point, queue_size=1)
self.fig = None
self.z=0
self.width =1080
self.height=1080
self.pose = []
self.bebop_state = 0
self.goalx = 0
self.goaly = 0
self.goalz = 0
self.startx = 0
self.starty = 0
self.startz = 0
def rotX(self, cloud, theta):
cos = np.cos(theta)
sin = np.sin(theta)
MatrixX = np.array([[1 ,0 ,0 , 0],
[0 ,cos,-sin, 0],
[0 ,-sin, cos,0],
[0 ,0 ,0 ,1]])
return MatrixX.dot(cloud.T).T
def rotY(self, cloud, theta):
cos = np.cos(theta)
sin = np.sin(theta)
MatrixY = np.array([[cos ,0 ,sin ,0],
[0 ,1 ,0 ,0],
[-sin,0 ,cos ,0],
[0 ,0 ,0 ,1]])
return MatrixY.dot(cloud.T).T
def rotZ(self, cloud, theta):
cos = np.cos(theta)
sin = np.sin(theta)
MatrixZ = np.array([[cos ,-sin,0 ,0],
[sin ,cos ,0 ,0],
[0 ,0 ,1 ,0],
[0 ,0 ,0 ,1]])
return MatrixZ.dot(cloud.T).T
def tran(self, cloud, vec):
MatrixT = np.array([[1 ,0 ,0 ,vec[0]],
[0 ,1 ,0 ,vec[1]],
[0 ,0 ,1 ,vec[2]],
[0 ,0 ,0 ,1 ]])
return MatrixT.dot(cloud.T).T
def pinhole(self, cloud):
d=1
width = self.width
height = self.height
MatrixP = np.array([[width ,0 ,width/2 ,0 ],
[0 ,width ,height/2 ,0 ],
[0 ,0 ,-1/d ,0 ]])
return MatrixP.dot(cloud.T).T
def ortho(self, cloud):
d=1
width = self.width
height = self.height
MatrixP = np.array([[1 ,0 ,width/2 ,0 ],
[0 ,1 ,height/2 ,0 ],
[0 ,0 ,0 ,1]])
return MatrixP.dot(cloud.T).T
def eucDist(self, point):
return np.sqrt(point[0]**2 + point[1]**2 + point[2]**2)
def send_image_map(self, image):
img = image.astype(np.uint8)
image_message = self.bridge.cv2_to_imgmsg(img, "mono8")
try:
self.image_pub.publish(image_message)
except rospy.ROSInterruptException as ros_e :
print(ros_e)
#self.rate.sleep()
def send_start(self, p):
#self.rate.sleep()
point = Point()
point.x = p[0]
point.y = p[1]
point.z = 0
try:
self.start_pub.publish(point)
except rospy.ROSInterruptException as ros_e :
print(ros_e)
def send_goal(self, p):
#self.rate.sleep()
point = Point()
point.x = p[0]
point.y = p[1]
point.z = 0
try:
self.goal_pub.publish(point)
except rospy.ROSInterruptException as ros_e :
print(ros_e)
def bebop_state_callback(self, data):
self.bebop_state = int(data.data)
#print(self.goalx,self.goaly)
def callback_pose(self,data):
self.pose = data.pose
#print(self.bebop_state)
if self.bebop_state == 4:
self.goalx = 1
self.goaly = self.pose.position.y
self.goalz = self.pose.position.z
#print(data.pose)
if self.bebop_state == 5 and self.goalx == 0 and self.goaly == 0:
self.goalx = 1
self.goaly = self.pose.position.y
self.goalz = self.pose.position.z
#print(data.pose)
self.startx = 1
self.starty = self.pose.position.y
self.startz = self.pose.position.z
def callback(self,data):
if self.goalx == 0:
return
cloud_points = list(pc2.read_points(data, skip_nans=True, field_names = ("x", "y", "z" )))
cloud_points = np.array(cloud_points)
cloud_points = np.insert(cloud_points, 3, 1, axis = 1)
save('data_points_ddr.npy', cloud_points)
#cloud_points = load('data_points_ddr.npy')
goal = np.array([[self.goalx ,self.goaly ,self.goalz,1]])
start = np.array([[self.startx,self.starty,self.startz,1]])
width = self.width
height = self.height
self.z = -1.5
theta = np.pi * (0.5)
cloud_points = self.rotY(cloud_points, theta)
goal = self.rotY(goal, theta)
start = self.rotY(start, theta)
theta = np.pi * (0.5)
cloud_points = self.rotZ(cloud_points, theta)
goal = self.rotZ(goal, theta)
start = self.rotZ(start, theta)
meanx = np.mean(cloud_points.T[0])
meany = np.mean(cloud_points.T[1])
meanz = np.mean(cloud_points.T[2])
#print(cloud_points[0])
#print(meanz)
vec =[-meanx,-meany,self.z] #
#vec =[0,0.0,self.z] #
cloud_points = self.tran(cloud_points, vec)
goal = self.tran(goal, vec)
start = self.tran(start, vec)
cloud_points = self.pinhole(cloud_points)
goal = self.pinhole(goal)
start = self.pinhole(start)
#cloud_points = self.ortho(cloud_points)
cloud_points = cloud_points.T
goal = goal.T
start = start.T
#print(goal)
indextoremove = []
# for i in range(len(cloud_points[2])):
# print(i)
# if cloud_points[2][i] < -5:
# indextoremove.append(i)
# if cloud_points[2][i] > 5:
# indextoremove.append(i)
#cloud_points = np.delete(cloud_points,indextoremove,1)
xy = np.array([np.around(cloud_points[0]/cloud_points[2] + width ),
np.around(cloud_points[1]/cloud_points[2] + height),
cloud_points[2] ])
goal_image = np.array([np.around(goal[0]/goal[2] + width ),
np.around(goal[1]/goal[2] + height),
goal[2] ])
start_image = np.array([np.around(start[0]/start[2] + width ),
np.around(start[1]/start[2] + height),
start[2] ])
#print(goal_image.T)
#print(int(start_image[0]))
#xy = cloud_points
indextoremove =[]
# for i in range(len(xy[2])):
# #print(i)
# if xy[2][i] < 4:
# indextoremove.append(i)
# if xy[2][i] > 5:
# indextoremove.append(i)
#xy = np.delete(xy,indextoremove,1)
uv = np.ones((height, width, 1))*100
xy = xy.T
for point in xy:
if point[0] < width and point[0] >= 0 and point[1] < height and point[1] > 0:
v = int(point[0])
u = int(point[1])
if uv[u,v,0] > point[2]:
uv[u,v,0] = point[2]
# print("points draw")
#print(np.mean(xy.T[2]))
mean = np.mean(xy.T[2])
#print(mean - (mean * .8))
#print(mean + (mean * .05))
#print("")
uv[:,:,0][uv[:,:,0] >= mean + (mean * .05)] = 0
uv[:,:,0][uv[:,:,0] < mean - (mean * .9) ] = 0
uv[:,:,0][uv[:,:,0] != 0 ] = 255
uv = np.flip(uv,0)
start_image[0] = int(start_image[0])
start_image[1] = int(1080 - start_image[1])
goal_image[0] = int(goal_image[0])
goal_image[1] = int(1080 - goal_image[1])
# uv[int(start_image[0])][int(start_image[1])] = 2000
# uv[int(goal_image[0] )][int(goal_image[1] )] = 1000
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(10,10))
uv = cv2.dilate(uv,kernel,iterations = 2)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(5,5))
uv = cv2.erode(uv,kernel,iterations = 2)
# print(uv[int(start_image[0])][int(start_image[1])])
# print(start_image[0], start_image[1])
self.send_goal(goal_image)
self.send_start(start_image)
self.send_image_map(uv)
# plt.imshow(np.reshape(uv,(height,width)),origin='lower')
# plt.pause(.001)
# plt.cla()
#print(np.array(data.data).shape)
pass
def processSegments(self, segment_list_msg):
if not self.active:
return
t_start = rospy.get_time()
if self.use_propagation:
self.propagateBelief()
self.t_last_update = rospy.get_time()
# initialize measurement likelihood
measurement_likelihood = np.zeros(self.d.shape)
for segment in segment_list_msg.segments:
if segment.color != segment.WHITE and segment.color != segment.YELLOW:
continue
if segment.points[0].x < 0 or segment.points[1].x < 0:
continue
d_i, phi_i, l_i = self.generateVote(segment)
if d_i > self.d_max or d_i < self.d_min or phi_i < self.phi_min or phi_i > self.phi_max:
continue
if self.use_max_segment_dist and (l_i > self.max_segment_dist):
continue
i = floor((d_i - self.d_min) / self.delta_d)
j = floor((phi_i - self.phi_min) / self.delta_phi)
if self.use_distance_weighting:
dist_weight = self.dwa * l_i**3 + self.dwb * l_i**2 + self.dwc * l_i + self.zero_val
if dist_weight < 0:
continue
measurement_likelihood[
i, j] = measurement_likelihood[i, j] + dist_weight
else:
measurement_likelihood[
i, j] = measurement_likelihood[i, j] + 1 / (l_i)
if np.linalg.norm(measurement_likelihood) == 0:
return
measurement_likelihood = measurement_likelihood / np.sum(
measurement_likelihood)
if self.use_propagation:
self.updateBelief(measurement_likelihood)
else:
self.beliefRV = measurement_likelihood
# TODO entropy test:
#print self.beliefRV.argmax()
maxids = np.unravel_index(self.beliefRV.argmax(), self.beliefRV.shape)
# rospy.loginfo('maxids: %s' % maxids)
self.lanePose.header.stamp = segment_list_msg.header.stamp
self.lanePose.d = self.d_min + maxids[0] * self.delta_d
self.lanePose.phi = self.phi_min + maxids[1] * self.delta_phi
self.lanePose.status = self.lanePose.NORMAL
# publish the belief image
bridge = CvBridge()
belief_img = bridge.cv2_to_imgmsg(
(255 * self.beliefRV).astype('uint8'), "mono8")
belief_img.header.stamp = segment_list_msg.header.stamp
max_val = self.beliefRV.max()
self.lanePose.in_lane = max_val > self.min_max and len(
segment_list_msg.segments) > self.min_segs and np.linalg.norm(
measurement_likelihood) != 0
self.pub_lane_pose.publish(self.lanePose)
self.pub_belief_img.publish(belief_img)
# print "time to process segments:"
# print rospy.get_time() - t_start
# Publish in_lane according to the ent
in_lane_msg = BoolStamped()
in_lane_msg.header.stamp = segment_list_msg.header.stamp
in_lane_msg.data = self.lanePose.in_lane
# ent = entropy(self.beliefRV)
# if (ent < self.max_entropy):
# in_lane_msg.data = True
# else:
# in_lane_msg.data = False
self.pub_in_lane.publish(in_lane_msg)
class DetectLane():
def __init__(self):
self.sub_image_original = rospy.Subscriber('/camera/image',
Image,
self.cbFindLane,
queue_size=1)
self.pub_image_detect = rospy.Publisher('/detect/lane',
Image,
queue_size=1)
self.pub_center_white_lane = rospy.Publisher('/control/white_lane',
Float64,
queue_size=1)
self.pub_center_yellow_lane = rospy.Publisher('/control/yellow_lane',
Float64,
queue_size=1)
self.cvBridge = CvBridge()
self.counter = 1
self.hue_white_l = 0
self.hue_white_h = 179
self.saturation_white_l = 0
self.saturation_white_h = 30
self.lightness_white_l = 221
self.lightness_white_h = 255
self.hue_yellow_l = 26
self.hue_yellow_h = 34
self.saturation_yellow_l = 43
self.saturation_yellow_h = 255
self.lightness_yellow_l = 46
self.lightness_yellow_h = 255
def cbFindLane(self, image_msg):
# Change the frame rate by yourself. Now, it is set to 1/3 (10fps).
# Unappropriate value of frame rate may cause huge delay on entire recognition process.
# This is up to your computer's operating power.
if self.counter % 3 != 0:
self.counter += 1
return
else:
self.counter = 1
cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8")
# find White and Yellow Lanes
self.maskLane(cv_image)
# yellow_fraction, cv_yellow_lane = self.maskYellowLane(cv_image)
def maskLane(self, image):
# convert image to hsv
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
# param of irange hsv(white & yellow)
Hue_white_h = self.hue_white_l
Hue_white_l = self.hue_white_h
Saturation_white_h = self.saturation_white_l
Saturation_white_l = self.saturation_white_h
Lightness_white_h = self.lightness_white_h
Lightness_white_l = self.lightness_white_l
Hue_yellow_h = self.hue_yellow_l
Hue_yellow_l = self.hue_yellow_h
Saturation_yellow_h = self.saturation_yellow_l
Saturation_yellow_l = self.saturation_yellow_h
Lightness_yellow_h = self.lightness_yellow_h
Lightness_yellow_l = self.lightness_yellow_l
# define range of white color in HSV
lower_white = np.array(
[Hue_white_h, Saturation_white_h, Lightness_white_l])
upper_white = np.array(
[Hue_white_l, Saturation_white_l, Lightness_white_h])
lower_yellow = np.array(
[Hue_yellow_h, Saturation_yellow_h, Lightness_yellow_l])
upper_yellow = np.array(
[Hue_yellow_l, Saturation_yellow_l, Lightness_yellow_h])
# Threshold the HSV image to get only white colors
mask_white = cv2.inRange(hsv, lower_white, upper_white)
mask_yellow = cv2.inRange(hsv, lower_yellow, upper_yellow)
kernel = np.ones((5, 5))
erosion_white = cv2.erode(mask_white, kernel)
erosion_yellow = cv2.erode(mask_yellow, kernel)
Gaussian_white = cv2.GaussianBlur(erosion_white, (5, 5), 0)
Gaussian_yellow = cv2.GaussianBlur(erosion_yellow, (5, 5), 0)
# findContours of image
contours_white, hierarchy_white = cv2.findContours(
Gaussian_white, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
contours_yellow, hierarchy_yellow = cv2.findContours(
Gaussian_yellow, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
# print contours
# print(contours_white)
# print(contours_yellow)
# draw the contours in origin image
cv2.drawContours(image, contours_white, -1, (139, 104, 0), 3)
cv2.drawContours(image, contours_yellow, -1, (139, 104, 0), 3)
# center of white and yellow lane
# white_center = Center()
# yellow_center = Center()
# try:
white_center = self.calculate_average(contours_white[0])
print("white: ", white_x, white_y)
is_detect_white = 1
# except:
# is_detect_white = 0
# print(contours_white[0])
# print("The Camera Can`t Catch The White Lane.")
# try:
yellow_center = self.calculate_average(contours_yellow[0])
print("yellow: ", yellow_x, yellow_y)
is_detect_yellow = 1
# except:
# is_detect_yellow = 0
# print(contours_yellow[0])
# print("The Camera Can`t Catch The Yellow Lane.")
# print(contours_white[0])
# yellow_x, yellow_y = self.calculate_average(contours_yellow[0])
# Publish Image
self.pub_image_detect.publish(
self.cvBridge.cv2_to_imgmsg(image, 'bgr8'))
# Publish Center
self.pub_center_white_lane.publish(white_center)
self.pub_center_yellow_lane.publish(yellow_center)
def calculate_average(self, input):
sum_x = 0
for i in input:
sum_x += i[0][0]
return sum_x / len(input)
def main(self):
rospy.spin()
cap = cv2.VideoCapture(0)
rospy.init_node('lepton_driver', anonymous=True)
image_pub = rospy.Publisher("/flir_lepton/image", Image, queue_size=1)
while (True and not rospy.is_shutdown()):
time.sleep(0.1)
# Capture frame-by-frame
ret, frame = cap.read()
frame = frame[:, :, 0]
frame.astype('int')
gray = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
print(gray.shape)
image_message = bridge.cv2_to_imgmsg(frame, encoding="passthrough")
image_pub.publish(image_message)
# Our operations on the frame come here
# Display the resulting frame
# cv2.imshow('frame',frame)
# if cv2.waitKey(1) & 0xFF == ord('q'):
# break
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
class SensorStreaming(sensor_streaming_pb2_grpc.SensorStreamingServicer):
def __init__(self, camera_pubs, lidar_pub, radar_pub, clock_pub):
print("creating")
self.bridge = CvBridge()
self.camera_pubs = camera_pubs
self.lidar_pub = lidar_pub
self.radar_pub = radar_pub
self.clock_pub = clock_pub
def StreamCameraSensor(self, request, context):
"""
Takes in a gRPC SensorStreamingRequest containing
all the data needed to create and publish a sensor_msgs/Image
ROS message.
"""
img_string = request.data
cv_image = np.fromstring(img_string, np.uint8)
# NOTE, the height is specifiec as a parameter before the width
cv_image = cv_image.reshape(request.height, request.width, 3)
cv_image = cv2.flip(cv_image, 0)
bgr_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
msg = Image()
header = std_msgs.msg.Header()
try:
# RGB
# msg = self.bridge.cv2_to_imgmsg(cv_image, 'rgb8')
# BGR
msg = self.bridge.cv2_to_imgmsg(bgr_image, 'bgr8')
header.stamp = rospy.Time.from_sec(request.timeStamp)
msg.header = header
except CvBridgeError as e:
print(e)
camera_pubs[request.frame_id].publish(msg)
return sensor_streaming_pb2.CameraStreamingResponse(success=True)
def StreamLidarSensor(self, request, context):
"""
Takes in a gRPC LidarStreamingRequest containing
all the data needed to create and publish a PointCloud2
ROS message.
"""
pointcloud_msg = PointCloud2()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.from_sec(request.timeInSeconds)
header.frame_id = "velodyne"
pointcloud_msg.header = header
pointcloud_msg.height = request.height
pointcloud_msg.width = request.width
fields = request.fields
# Set PointCloud[] fields in pointcloud_msg
for i in range(len(fields)):
pointcloud_msg.fields.append(PointField())
pointcloud_msg.fields[i].name = fields[i].name
pointcloud_msg.fields[i].offset = fields[i].offset
pointcloud_msg.fields[i].datatype = fields[i].datatype
pointcloud_msg.fields[i].count = fields[i].count
pointcloud_msg.is_bigendian = request.isBigEndian
pointcloud_msg.point_step = request.point_step
pointcloud_msg.row_step = request.row_step
pointcloud_msg.data = request.data
pointcloud_msg.is_dense = request.is_dense
self.lidar_pub.publish(pointcloud_msg)
# TODO: This does not belong in this RPC implementation, should be
# moved to own or something like that.
sim_clock = Clock()
sim_clock.clock = rospy.Time.from_sec(request.timeInSeconds)
self.clock_pub.publish(sim_clock)
return sensor_streaming_pb2.LidarStreamingResponse(success=True)
def StreamRadarSensor(self, request, context):
"""
Takes in a gRPC RadarStreamingRequest containing
all the data needed to create and publish a RadarSpoke
ROS message.
"""
number_of_spokes = request.numSpokes
for i in range(number_of_spokes):
radar_spoke_msg = RadarSpoke()
# Header
header = std_msgs.msg.Header()
header.frame_id = "milliampere_radar"
header.stamp = rospy.Time.from_sec(request.timeInSeconds[i])
radar_spoke_msg.azimuth = request.azimuth[i]
radar_spoke_msg.intensity = request.radarSpokes[i * request.numSamples : i * request.numSamples + request.numSamples]
radar_spoke_msg.range_start = request.rangeStart
radar_spoke_msg.range_increment = request.rangeIncrement
radar_spoke_msg.min_intensity = request.minIntensity
radar_spoke_msg.max_intensity = request.maxIntensity
radar_spoke_msg.num_samples = request.numSamples
self.radar_pub.publish(radar_spoke_msg)
return sensor_streaming_pb2.RadarStreamingResponse(success=True)
print("No more frames")
break
_, leftFrame = left.retrieve()
leftFrame = cv2.resize(leftFrame, None,fx=0.5, fy=0.66, interpolation = cv2.INTER_AREA)
# if not (right.grab()):
# print("No more frames")
# break
# _, rightFrame = right.retrieve()
# rightFrame = cv2.resize(rightFrame, None,fx=0.5, fy=0.66, interpolation = cv2.INTER_AREA)
print("Got Image ", frameId)
if frameId % 5 == -1:
cv2.imshow('left', leftFrame)
# cv2.imshow('right', rightFrame)
left_image_pub.publish(BRIDGE.cv2_to_imgmsg(leftFrame, "bgr8"))
# right_image_pub.publish(BRIDGE.cv2_to_imgmsg(rightFrame, "bgr8"))
if cv2.waitKey(15) & 0xFF == ord('q'):
break
frameId += 1
left.release()
# right.release()
cv2.destroyAllWindows()
class Bridge(object):
def __init__(self, conf, server):
rospy.init_node('styx_server')
self.server = server
self.vel = 0.
self.yaw = None
self.angular_vel = 0.
self.bridge = CvBridge()
self.callbacks = {
'/vehicle/steering_cmd': self.callback_steering,
'/vehicle/throttle_cmd': self.callback_throttle,
'/vehicle/brake_cmd': self.callback_brake,
}
self.subscribers = [
rospy.Subscriber(e.topic, TYPE[e.type], self.callbacks[e.topic])
for e in conf.subscribers
]
self.publishers = {
e.name: rospy.Publisher(e.topic, TYPE[e.type], queue_size=1)
for e in conf.publishers
}
def create_light(self, x, y, z, yaw, state):
light = TrafficLight()
light.header = Header()
light.header.stamp = rospy.Time.now()
light.header.frame_id = '/world'
light.pose = self.create_pose(x, y, z, yaw)
light.state = state
return light
def create_pose(self, x, y, z, yaw=0.):
pose = PoseStamped()
pose.header = Header()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = '/world'
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
q = tf.transformations.quaternion_from_euler(0., 0.,
math.pi * yaw / 180.)
pose.pose.orientation = Quaternion(*q)
return pose
def create_float(self, val):
fl = Float()
fl.data = val
return fl
def create_twist(self, velocity, angular):
tw = TwistStamped()
tw.twist.linear.x = velocity
tw.twist.angular.z = angular
return tw
def create_steer(self, val):
st = SteeringReport()
st.steering_wheel_angle_cmd = val * math.pi / 180.
st.enabled = True
st.speed = self.vel
return st
def calc_angular(self, yaw):
angular_vel = 0.
if self.yaw is not None:
angular_vel = (yaw - self.yaw) / (rospy.get_time() -
self.prev_time)
self.yaw = yaw
self.prev_time = rospy.get_time()
return angular_vel
def create_point_cloud_message(self, pts):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/world'
cloud_message = pcl2.create_cloud_xyz32(header, pts)
return cloud_message
def broadcast_transform(self, name, position, orientation):
br = tf.TransformBroadcaster()
br.sendTransform(position, orientation, rospy.Time.now(), name,
"world")
def publish_odometry(self, data):
pose = self.create_pose(data['x'], data['y'], data['z'], data['yaw'])
position = (data['x'], data['y'], data['z'])
orientation = tf.transformations.quaternion_from_euler(
0, 0, math.pi * data['yaw'] / 180.)
self.broadcast_transform("base_link", position, orientation)
self.publishers['current_pose'].publish(pose)
self.vel = data['velocity'] * 0.44704
self.angular = self.calc_angular(data['yaw'] * math.pi / 180.)
self.publishers['current_velocity'].publish(
self.create_twist(self.vel, self.angular))
def publish_controls(self, data):
steering, throttle, brake = data['steering_angle'], data[
'throttle'], data['brake']
self.publishers['steering_report'].publish(self.create_steer(steering))
self.publishers['throttle_report'].publish(self.create_float(throttle))
self.publishers['brake_report'].publish(self.create_float(brake))
def publish_obstacles(self, data):
for obs in data['obstacles']:
pose = self.create_pose(obs[0], obs[1], obs[2])
self.publishers['obstacle'].publish(pose)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/world'
cloud = pcl2.create_cloud_xyz32(header, data['obstacles'])
self.publishers['obstacle_points'].publish(cloud)
def publish_lidar(self, data):
self.publishers['lidar'].publish(
self.create_point_cloud_message(
zip(data['lidar_x'], data['lidar_y'], data['lidar_z'])))
def publish_traffic(self, data):
x, y, z = data['light_pos_x'], data['light_pos_y'], data[
'light_pos_z'],
yaw = [
math.atan2(dy, dx)
for dx, dy in zip(data['light_pos_dx'], data['light_pos_dy'])
]
status = data['light_state']
lights = TrafficLightArray()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/world'
lights.lights = [
self.create_light(*e) for e in zip(x, y, z, yaw, status)
]
self.publishers['trafficlights'].publish(lights)
def publish_dbw_status(self, data):
self.publishers['dbw_status'].publish(Bool(data))
def publish_camera(self, data):
imgString = data["image"]
image = PIL_Image.open(BytesIO(base64.b64decode(imgString)))
image_array = np.asarray(image)
image_message = self.bridge.cv2_to_imgmsg(image_array, encoding="rgb8")
self.publishers['image'].publish(image_message)
def callback_steering(self, data):
self.server(
'steer',
data={'steering_angle': str(data.steering_wheel_angle_cmd)})
def callback_throttle(self, data):
#rospy.loginfo("Pedal command: " + str(data.pedal_cmd))
self.server('throttle', data={'throttle': str(data.pedal_cmd)})
def callback_brake(self, data):
#rospy.loginfo("Break command: " + str(data.pedal_cmd))
self.server('brake', data={'brake': str(data.pedal_cmd)})
class image_processor:
"""
This class takes image messages from the USB Camera and converts them to a cv2 format
subsequently it converts the image to grayscale and performs a perspective and hanning transform.
Finally, it outputs a delta value indicating the offset of the vehicle from the center of the lane
"""
def __init__(self):
global display_image, publish_image, calibrate_transform
global calibration_pts
#Create ROS Interfaces
self.offset_lane_pub = rospy.Publisher("lane_offset",
Float32,
queue_size=10)
self.cvfail_pub = rospy.Publisher("cv_abort", Int32, queue_size=10)
self.image_pub = rospy.Publisher("cv_image", Image, queue_size=10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/barc_cam/image_raw", Image,
self.callback_image)
#Get Launch File Parameters and configure node
calibrate_transform = rospy.get_param(
"/image_processing/calibrate_transform")
display_image = rospy.get_param("/image_processing/display_image")
publish_image = rospy.get_param("/image_processing/publish_image")
global image_calibrated
# Projection Transform Parameters
self.x_offset = 100
self.x_width = 400
self.y_height = 400
if calibrate_transform:
image_calibrated = False
cv2.namedWindow("Calibrate Image Transform")
cv2.setMouseCallback("Calibrate Image Transform",
print_point_callback)
else:
image_calibrated = True
calibration_pts = np.float32( ([rospy.get_param("/image_processing/upperLeftX"), rospy.get_param("/image_processing/upperLeftY")], \
[rospy.get_param("/image_processing/upperRightX"), rospy.get_param("/image_processing/upperRightY")], \
[rospy.get_param("/image_processing/lowerRightX"), rospy.get_param("/image_processing/lowerRightY")], \
[rospy.get_param("/image_processing/lowerLeftX"), rospy.get_param("/image_processing/lowerLeftY")]))
# Apply Projection Transform
# ref points [TOPLEFT, TOPRIGHT, BOTTOMRIGHT, BOTTOMLEFT]
ref_pts = np.float32([[self.x_offset,0], \
[self.x_width + self.x_offset,0], \
[self.x_width + self.x_offset, self.y_height], \
[self.x_offset, self.y_height]])
self.projection_dim = (self.x_width + self.x_offset * 2,
self.y_height)
self.projection_transform = cv2.getPerspectiveTransform(
calibration_pts, ref_pts)
self.minimumContourSize = 10
self.image_threshold = 20
self.y_offset_pixels_cg = 70
self.num_nonvalid_img = 0
self.num_failed_img_before_abort = 30
self.half_vehicle_width_pixels = (260 // 16) * 6
def callback_image(self, data):
"""
Callback for incoming image message
"""
# Global Variables
global display_image, publish_image, image_calibrated
# Convert ROS Image Message to CV2 image
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
# Convert Color Image to Grayscale
gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
if image_calibrated:
kernel_size = 7
#blur_gray = cv2.GaussianBlur(gray_image, (kernel_size, kernel_size), 0)
#cv2.imshow("blurred Image", blur_gray)
# Perform Line Detection
#edges = cv2.Canny(gray_image,1,15)
#in MPC lab, 13 15 100
edges = cv2.Canny(gray_image, 15, 300)
#cv2.imshow("Normal Image", cv_image)
# imshape = edges0.shape
# vertices = np.array([[(100,imshape[0]), (440, 325), (550, 325), (imshape[1],imshape[0])]], dtype=np.int32)
# edges = region_of_interest(edges0, vertices)
#cv2.imshow("After Canny", edges)
# rho = 2
# theta = np.pi/360
# threshold = 20#210
# min_line_length = 250
# max_line = 200
#
# lines = cv2.HoughLinesP(edges, rho, theta , threshold, np.array([]), minLineLength = min_line_length , maxLineGap = max_line )
line_img = np.zeros(cv_image.shape, dtype=np.uint8)
offset = draw_lines(line_img, edges)
pub = rospy.Publisher('chatter', String, queue_size=10)
rate = rospy.Rate(100) # 10hz
rospy.loginfo(str(offset))
pub.publish(str(offset))
rate.sleep()
alpha = .6
beta = 1.
gamma = 0.
LinesDrawn2 = cv2.addWeighted(cv_image, alpha, line_img, beta,
gamma)
cv2.rectangle(LinesDrawn2, (400, 100), (640, 0), (255, 255, 255),
thickness=-1,
lineType=8,
shift=0)
fontFace = cv2.FONT_HERSHEY_SCRIPT_SIMPLEX
cv2.putText(LinesDrawn2, "information box", (465, 20), fontFace,
.5, (0, 0, 0))
cv2.putText(LinesDrawn2, "steering angle: " + str(offset) + " deg",
(440, 50), fontFace, .5, (0, 0, 0))
#cv2.putText(LinesDrawn2, "developed by Parsa/MPC Lab", (410, 80), fontFace, .5,(0,0,0))
# Find Contours
#contours, heirarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
#contours = [contour for contour in contours if (cv2.contourArea(contour) < self.minimumContourSize)]
#cv2.drawContours(edges, contours, -1, color = 0)
#cv2.imshow("After Canny", edges)
#
# Remove small contours (noise) in edge detected image
#(rows2,cols2,channels2) = LinesDrawn.shape
#M = cv2.getRotationMatrix2D((cols2/2,rows2/2),90,1)
#LinesDrawn2 = cv2.warpAffine(LinesDrawn,M,(cols2,rows2))
#vertices = vertices.reshape((-1,1,2))
#cv2.polylines(LinesDrawn2, [vertices], True, (0, 255, 255))
#
#
#
#
# #
#
# # Apply Projection Transform
# edges = cv2.warpPerspective(edges, self.projection_transform, self.projection_dim)
#
# # Apply Thresholding
# #ret, edges = cv2.threshold(edges, self.image_threshold, 255, cv2.THRESH_BINARY)
#
# # Convert to RGB to visualize lane detection points
# if display_image or publish_image:
# backtorgb = cv2.cvtColor(edges,cv2.COLOR_GRAY2RGB)
#
# ## Lane Detection
# height, width = edges.shape
#
# # We assume that our vehicle is placed in the center of the lane initially
# offset = 0
#
# y_base = 200 #20 # this represents 3" in front of the car
#
#
# #for i in range(height // y_increment):
# index_y = height - y_base
# index_x = (width)//2
# if index_x >= width:
# index_x = width - 1
# if index_x < 0:
# index_x = 0
# x_left, x_right = find_offset_in_lane(edges, index_x, index_y, width)
#
# midpoint = (x_right + x_left)//2
# offset = midpoint - width//2
# # ~~~~ FILTERING ~~~~
# # perform median filter to remove extremities
if display_image or publish_image:
# cv2.circle(backtorgb, (x_right, index_y), 3, (0,255,255), -1)
# cv2.circle(backtorgb, (x_left, index_y), 3, (0,255,255), -1)
# cv2.circle(backtorgb, (midpoint, index_y), 3, (0,255,0),-1)
# cv2.circle(backtorgb, (width//2, index_y-5), 3, (0,0,255),-1)
#
# fontFace = cv2.FONT_HERSHEY_SCRIPT_SIMPLEX
# cv2.putText(LinesDrawn2, str(offset), (180, 255), fontFace, .5,(0,0,255))
# if offset > 0:
# cv2.arrowedLine(LinesDrawn2, (220, 250), (270, 250), (255,0,0), 2,8, 0,0.5)
# elif offset < 0:
# cv2.arrowedLine(LinesDrawn2, (170, 250), (120, 250), (255,0,0), 2,8, 0,0.5)
# else:
# cv2.arrowedLine(LinesDrawn2, (180, 230), (180, 180), (255,0,0), 2,8, 0,0.5)
#
#
#
# ## Make Steering Calculations
# angle_adjacent = 20; #experimentally determined
## Publish vehicle steering directions
self.offset_lane_pub.publish(Float32(offset))
if display_image:
#cv2.imshow("Final", backtorgb)
cv2.imshow("Advanced Lane Detection", LinesDrawn2)
if publish_image:
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(LinesDrawn2, "bgr8"))
except CvBridgeError as e:
print(e)
else:
if display_image:
cv2.imshow("Calibrate Image Transform", gray_image)
if publish_image:
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(gray_image, "bgr8"))
except CvBridgeError as e:
print(e)
if display_image:
cv2.waitKey(3)
class viewer:
def __init__(self, classifier):
self.package_directory = subprocess.check_output(
["rospack", "find", "drone_cam"]).decode().strip('\n')
self.filename = self.package_directory + '/src/office_drone_b.mp4'
self.cap = cv2.VideoCapture(self.filename)
self.frame_count = self.cap.get(cv2.CAP_PROP_FRAME_COUNT)
self.frames = 0
self.bridge = CvBridge()
self.frame_copy = []
self.topic = '/drone_b/camera/image'
# self.pub = rospy.Publisher(self.topic, drone_feed, queue_size=10)
self.pubimg = rospy.Publisher(self.topic + '_Image',
Image,
queue_size=10)
self.rate = rospy.Rate(30)
# self.img = drone_feed()
self.imgmsg = Image()
self.classifiers = {
'haarcascades':
cv2.CascadeClassifier(
'/home/lv/openCV/opencv/data/haarcascades/haarcascade_frontalface_alt.xml'
),
'lbpcascades':
cv2.CascadeClassifier(
'/home/lv/openCV/opencv/data/lbpcascades/lbpcascade_frontalface.xml'
)
}
self.classifier = self.classifiers[classifier]
def detectFaces(self, frame):
self.frame_copy = frame.copy()
grayscale = cv2.cvtColor(self.frame_copy, cv2.COLOR_BGR2GRAY)
grayscale = cv2.equalizeHist(grayscale)
faces = self.classifier.detectMultiScale(grayscale,
scaleFactor=1.2,
minNeighbors=5)
for (x, y, w, h) in faces:
cv2.rectangle(self.frame_copy, (x, y), (x + w, y + h), (0, 255, 0),
2)
def pubcam(self):
while not rospy.is_shutdown():
try:
self.frames += 1
if self.frames == self.frame_count:
self.cap = cv2.VideoCapture(self.filename)
self.frames = 0
ret, frame = self.cap.read()
self.detectFaces(frame)
self.imgmsg = self.bridge.cv2_to_imgmsg(
self.frame_copy, "bgr8")
# self.img.name = self.topic
# self.pub.publish(self.img)
# self.imgmsg = self.img.image
self.pubimg.publish(self.imgmsg)
self.rate.sleep()
except CvBridgeError as e:
rospy.loginfo(e)
class DetectBucket(object):
x0, y0, z0 = 0, 0, 0
roll0, pitch0, yaw0 = 0, 0, 0
odom_received = False
#make birdeye heatmap with size 50, 25, ppm=2, init_pos=0.7, 25
birdeye_heatmap = np.zeros((50, 100), dtype=np.uint8)
init_pos = 0.7, 25
ppm = 2
heatmaps = np.zeros((2, 50, 100), dtype=np.uint8)
def __init__(self, nodename, drive=None):
rospy.init_node(nodename, anonymous=False)
self.bridge = CvBridge()
self.init_markers()
#sub to front cam
# rospy.Subscriber("/logi_c920/usb_cam_node/image_raw", Image, self.front_img_callback, queue_size = 1)
rospy.Subscriber("/front/image_rect_color",
Image,
self.front_img_callback,
queue_size=1)
#sub to downward cam
# rospy.Subscriber("/logi_c310/usb_cam_node/image_raw", Image, self.down_img_callback, queue_size = 1)
rospy.Subscriber("/down/image_rect_color",
Image,
self.down_img_callback,
queue_size=1)
#sub to odom
rospy.Subscriber('/visual_odom',
Odometry,
self.odom_callback,
queue_size=1)
#publish detection imgs
self.img_pub = rospy.Publisher('/bucket_img', Image, queue_size=1)
self.down_img_pub = rospy.Publisher('/down_bucket_img',
Image,
queue_size=1)
self.birdeye_heatmap_pub = rospy.Publisher('/birdeye_bucket',
Image,
queue_size=1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rate.sleep()
def down_img_callback(self, msg):
font = cv2.FONT_HERSHEY_SIMPLEX
color = (0, 0, 255)
fov_w, fov_h = 48 * math.pi / 180, 36 * math.pi / 180
px_W, px_H = 640, 480
img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
img = self.img_correction(img)
h, w = img.shape[:2]
output = np.zeros_like(img)
blur = cv2.GaussianBlur(img, (7, 7), 0)
#red, then blue
boundaries = [
([50, 50, 100], [155, 155, 255], [0, 0, 255]),
([150, 50, 0], [255, 150, 100], [255, 0, 0])
# ([25, 146, 190], [62, 174, 250]),
# ([103, 86, 65], [145, 133, 128])
]
combined_mask = np.zeros((h, w), dtype=np.uint8)
i = 0
for (lower, upper, color) in boundaries:
# create NumPy arrays from the boundaries
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
# find the colors within the specified boundaries and apply
# the mask
mask = cv2.inRange(img, lower, upper)
# minLineLength=300
# lines = cv2.HoughLinesP(image=mask,rho=1,theta=np.pi/180,\
# threshold=50,lines=np.array([]), minLineLength=minLineLength, maxLineGap=12)
# if lines is not None:
# for line in lines:
# #find two major gradients
# x1, y1, x2, y2=line[0][0], line[0][1], line[0][2], line[0][3]
# cv2.line(mask, (x1, y1), (x2, y2), 0, 5, cv2.LINE_AA)
kernel = np.ones((5, 5), np.uint8)
contour_mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
# opening=mask
# opening = cv2.erode(mask, None, iterations=4)
opening = cv2.dilate(mask, None, iterations=2)
# Hough circle function parameters
(_, contours, _) = cv2.findContours(contour_mask,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
contour_mask = cv2.cvtColor(contour_mask, cv2.COLOR_GRAY2BGR)
areas = []
r = []
for contour in contours:
rect = cv2.boundingRect(contour)
area = rect[2] * rect[3]
ar = float(rect[3]) / rect[2]
# print(ar)
px_count = np.sum(opening[rect[1]:rect[1] + rect[3],
rect[0]:rect[0] + rect[2]]) / 255
# print(px_count/area)
if ar < 1.5 and ar > 0.6 and area > 10000 and px_count / area > 0.5:
cv2.rectangle(img, (rect[0], rect[1]),
(rect[2] + rect[0], rect[3] + rect[1]),
color, 2)
del_px = w / 2 - (rect[0] + rect[2] / 2)
del_py = h / 2 - (rect[1] + rect[3] / 2)
thetax = fov_w * del_px / (w / 2)
thetay = fov_h * del_py / (h / 2)
del_x = self.z0 * math.tan(thetax)
del_y = self.z0 * math.tan(thetay)
x = self.x0 - del_y
y = self.y0 - del_x
ind_x = int(self.birdeye_heatmap.shape[0] -
(self.init_pos[0] + x) * self.ppm)
ind_y = int((self.init_pos[1] - y) * self.ppm)
ind_x = np.clip(ind_x, 0, self.birdeye_heatmap.shape[1])
ind_y = np.clip(ind_y, 0, self.birdeye_heatmap.shape[0])
self.heatmaps[i, ind_x, ind_y] += 1
combined_mask = cv2.bitwise_or(combined_mask, mask)
i += 1
combined_mask = cv2.cvtColor(combined_mask, cv2.COLOR_GRAY2BGR)
self.down_img_pub.publish(
self.bridge.cv2_to_imgmsg(np.hstack([img, combined_mask]), "bgr8"))
def front_img_callback(self, msg):
font = cv2.FONT_HERSHEY_SIMPLEX
color = (0, 0, 255)
start_time = time.time()
img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
img = self.img_correction(img)
h, w = img.shape[:2]
output = np.zeros_like(img)
blur = cv2.GaussianBlur(img, (7, 7), 0)
#red, then blue
boundaries = [
([0, 0, 0], [125, 105, 255], [0, 0, 255]),
([0, 31, 4], [255, 128, 50], [255, 0, 0])
# ([25, 146, 190], [62, 174, 250]),
# ([103, 86, 65], [145, 133, 128])
]
combined_mask = np.zeros((h, w), dtype=np.uint8)
i = 0
for (lower, upper, color) in boundaries:
# create NumPy arrays from the boundaries
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
# find the colors within the specified boundaries and apply
# the mask
mask = cv2.inRange(img, lower, upper)
kernel = np.ones((5, 5), np.uint8)
# opening = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
# opening=mask
opening = cv2.erode(mask, None, iterations=4)
# opening = cv2.dilate(mask, None, iterations=2)
(_, contours, _) = cv2.findContours(opening, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
for contour in contours:
#if too low not valid
if self.z0 < 0.7:
break
rect = cv2.boundingRect(contour)
area = int(rect[3] * rect[2])
ar = float(rect[3]) / rect[2]
# print(ar)
px_count = np.sum(opening[rect[1]:rect[1] + rect[3],
rect[0]:rect[0] + rect[2]]) / 255
# print(px_count/area)
if ar < 1 and ar > 0.5 and area > 1000 and area < 120000 and px_count / area > 0.5:
# print(px_count/area)
cv2.rectangle(img, (rect[0], rect[1]),
(rect[2] + rect[0], rect[3] + rect[1]),
color, 2)
depth = self.predict_depth(max(rect[2], rect[3]))
x, y = self.compute_xy(rect[0] + rect[2] / 2,
rect[1] + rect[3] / 2, depth, img)
print(depth, x, y)
text = "x, y: " + str(round(x, 2)) + "m " + str(round(
y, 2)) + "m"
cv2.putText(img, text,
(int(rect[0]) + 10, int(rect[1]) - 20), font,
0.5, color, 1, cv2.LINE_AA)
#update heatmap
ind_x = int(self.birdeye_heatmap.shape[0] -
(self.init_pos[0] + x) * self.ppm)
ind_y = int((self.init_pos[1] - y) * self.ppm)
ind_x = np.clip(ind_x, 0, self.birdeye_heatmap.shape[1])
ind_y = np.clip(ind_y, 0, self.birdeye_heatmap.shape[0])
# self.heatmaps[i, ind_x, ind_y]+=1
#threshold->cluster heatmap to get members
combined_mask = cv2.bitwise_or(combined_mask, mask)
i += 1
self.birdeye_heatmap = np.sum(self.heatmaps, axis=0).astype(np.uint8)
max_val = 10 #np.amax(self.birdeye_heatmap)
self.birdeye_heatmap = np.clip(self.birdeye_heatmap, 0, max_val)
if max_val > 0:
birdeye_heatmap_img = cv2.applyColorMap(
self.birdeye_heatmap * int(255 / max_val), cv2.COLORMAP_JET)
else:
birdeye_heatmap_img = cv2.applyColorMap(self.birdeye_heatmap,
cv2.COLORMAP_JET)
self.birdeye_heatmap_pub.publish(
self.bridge.cv2_to_imgmsg(birdeye_heatmap_img, "bgr8"))
combined_mask = cv2.cvtColor(combined_mask, cv2.COLOR_GRAY2BGR)
self.img_pub.publish(
self.bridge.cv2_to_imgmsg(np.hstack([img, combined_mask]), "bgr8"))
def compute_xy(self, px, py, depth, img):
#compute real position of gate in x,y,z
#first x and y refers to side and height
fov_w, fov_h = 62 * math.pi / 180, 46 * math.pi / 180
px_W, px_H = 640, 480
pd = (px_W / 2) / math.tan(fov_w / 2)
del_px = (-px + img.shape[1] / 2.0)
del_py = (-py + img.shape[0] / 2.0)
del_x = depth * del_px / pd
del_y = depth * del_py / pd
del_real_x = del_x * math.cos(self.roll0) - del_y * math.sin(
self.roll0)
del_real_y = del_x * math.sin(self.roll0) + del_y * math.cos(
self.roll0) + depth * math.tan(self.pitch0)
x = self.x0 + depth * math.cos(self.yaw0) - del_real_x * math.sin(
self.yaw0)
y = self.y0 + depth * math.sin(self.yaw0) + del_real_x * math.cos(
self.yaw0)
z = self.z0 + del_real_y
return x, y
def predict_depth(self, l):
fov_w, fov_h = 62 * math.pi / 180, 46 * math.pi / 180
px_W, px_H = 640, 480
#the longest edge is independent of perspective
#real length of pole in metres
real_l = 0.55
ppm = l / real_l
H = px_H / ppm
depth = H / (2 * math.tan(fov_h / 2))
# print(depth)
return depth
def img_correction(self, img):
clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(6, 6))
res = np.zeros_like(img)
for i in range(3):
res[:, :, i] = clahe.apply(img[:, :, i])
return res
def odom_callback(self, msg):
self.x0 = msg.pose.pose.position.x
self.y0 = msg.pose.pose.position.y
self.z0 = msg.pose.pose.position.z
# print(self.z0)
self.roll0, self.pitch0, self.yaw0 = euler_from_quaternion(
(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w))
self.odom_received = True
def printMarker(self, gate_pos):
#markerList store points wrt 2D world coordinate
self.markers.points = []
p = Point()
self.markers.points.append(Point(0, 0, 0))
p.x = gate_pos[0]
p.y = gate_pos[1]
p.z = 0.75
q_angle = quaternion_from_euler(0, 0, 0)
q = Quaternion(*q_angle)
self.markers.pose = Pose(p, q)
self.marker_pub.publish(self.markers)
def init_markers(self):
# Set up our waypoint markers
marker_scale = 0.2
marker_lifetime = 0 # 0 is forever
marker_ns = 'frontiers'
marker_id = 0
marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
# Define a marker publisher.
self.marker_pub = rospy.Publisher('gate_markers', Marker, queue_size=5)
# Initialize the marker points list.
self.markers = Marker()
self.markers.ns = marker_ns
self.markers.id = marker_id
# self.markers.type = Marker.ARROW
self.markers.type = Marker.SPHERE_LIST
self.markers.action = Marker.ADD
self.markers.lifetime = rospy.Duration(marker_lifetime)
self.markers.scale.x = marker_scale
self.markers.scale.y = marker_scale
self.markers.scale.z = marker_scale
self.markers.color.r = marker_color['r']
self.markers.color.g = marker_color['g']
self.markers.color.b = marker_color['b']
self.markers.color.a = marker_color['a']
self.markers.header.frame_id = 'map'
self.markers.header.stamp = rospy.Time.now()
self.markers.points = list()
def callback(data):
list_obj = listobj()
list_obj.single_obj_info = []
brocili_list = broclist()
brocili_list.broc_uv_list = []
temp_list_broc = []
global temp_single_obj
#print(data.objects_vector[0].id)
# print(data.objects_vector[0].classname)
# print(data.objects_vector[0].score)
#print("---"*20)
single_obj = sglobj()
#print("single_obj",single_obj)
single_obj.element_data_temp = []
single_obj.classname = []
single_obj.score = []
get_msg = data.objects_vector[0]
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(data.rgb_img, "bgr8")
cv_depth_image = bridge.imgmsg_to_cv2(data.depth_img, "passthrough")
rgb_img_to_pos = cv_image.copy()
depth_img_to_pos = cv_depth_image.copy()
plate_score = [0, 0]
pan_score = [0]
vegetablebowl_score = [0]
broccoli_score = [0, 0, 0]
souppothandle_score = [0]
panhandle_score = [0]
beef_score = [0]
nethandle_score = [0]
seasoningbottle_score = [0, 0]
seasoningbowl_score = [0]
for i in range(len(get_msg.id)):
if (get_msg.classname[i] == "pan"):
pan_score.append(get_msg.score[i])
if (get_msg.classname[i] == "beef"):
beef_score.append(get_msg.score[i])
if (get_msg.classname[i] == "plate"):
plate_score.append(get_msg.score[i])
if (get_msg.classname[i] == "vegetablebowl"):
vegetablebowl_score.append(get_msg.score[i])
if (get_msg.classname[i] == "broccoli"):
broccoli_score.append(get_msg.score[i])
if (get_msg.classname[i] == "souppothandle"):
souppothandle_score.append(get_msg.score[i])
if (get_msg.classname[i] == "panhandle"):
panhandle_score.append(get_msg.score[i])
if (get_msg.classname[i] == "seasoningbowl"):
beef_score.append(get_msg.score[i])
if (get_msg.classname[i] == "nethandle"):
nethandle_score.append(get_msg.score[i])
if (get_msg.classname[i] == "seasoningbottle"):
seasoningbottle_score.append(get_msg.score[i])
plate_score = sorted(plate_score, reverse=True)
pan_score = sorted(pan_score, reverse=True)
vegetablebowl_score = sorted(vegetablebowl_score, reverse=True)
broccoli_score = sorted(broccoli_score, reverse=True)
souppothandle_score = sorted(souppothandle_score, reverse=True)
panhandle_score = sorted(panhandle_score, reverse=True)
beef_score = sorted(beef_score, reverse=True)
nethandle_score = sorted(nethandle_score, reverse=True)
seasoningbottle_score = sorted(seasoningbottle_score, reverse=True)
seasoningbowl_score = sorted(seasoningbowl_score, reverse=True)
det_bar_pos = detectionbar.model_detection(cv_image)
bro_list = broccolidetection.broccoli_detection(cv_image)
global temp_list_broc
if len(bro_list) > 0:
brocili_list.broc_uv_list = []
brocili_list.broc_uv_list = bro_list
temp_list_broc = bro_list
if len(bro_list) <= 0:
brocili_list.broc_uv_list = temp_list_broc
#print("wo cao :",temp_list_broc)
#print("xi lan hua: ",brocili_list.broc_uv_list)
'''
print("plate score : ",plate_score)
print("pan score : ",pan_score)
print("vegetablebowl score : ",vegetablebowl_score)
print("broccoli score : ",broccoli_score)
print("souppothandle score : ",souppothandle_score)
print("panhandle score : ",panhandle_score)
print("beef score : ",beef_score)
print("nethandle score : ",nethandle_score)
print("seasoningbottle score : ",seasoningbottle_score)
print("seasoningbowl score : ",seasoningbowl_score)
'''
single_obj.element_data_temp = []
single_obj.classname = []
single_obj.score = []
for i in range(len(get_msg.id)):
element_data = element_info()
element_data.id_info = []
mask_area = get_msg.roi[i].height * get_msg.roi[i].width
if mask_area > 0 and get_msg.score[i] > 0.19:
mask_image = bridge.imgmsg_to_cv2(get_msg.masks[i], "passthrough")
if get_msg.classname[i] == "plate":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > plate1[0] and center_x < plate1[
1] and center_y > plate1[2] and center_y < plate1[3]:
temp_list = []
temp_list = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list != []:
#element_data.id_info = []
element_data.id_info = temp_list
single_obj.element_data_temp.append(element_data)
#print("aaa:",single_obj.element_data_temp)
single_obj.score.append(get_msg.score[i])
single_obj.classname.append("plate1")
if get_msg.classname[i] == "plate":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > plate2[0] and center_x < plate2[
1] and center_y > plate2[2] and center_y < plate2[3]:
temp_list1 = []
temp_list1 = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list1 != []:
#element_data.id_info = []
element_data.id_info = temp_list1
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
single_obj.classname.append("plate2")
if get_msg.classname[i] == "plate":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > plate3[0] and center_x < plate3[
1] and center_y > plate3[2] and center_y < plate3[3]:
temp_list2 = []
temp_list2 = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list2 != []:
element_data.id_info = temp_list2
#print("wo-2-cia",temp_list)
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
single_obj.classname.append("plate3")
if get_msg.classname[i] == "beef" and get_msg.score[
i] == beef_score[0]:
temp_list3 = []
temp_list3 = prcess_contours(
cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset,
get_msg.roi[i].height, get_msg.roi[i].width,
get_msg.score[i])
if temp_list3 != []:
element_data.id_info = temp_list3
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "pan" and get_msg.score[i] == pan_score[
0]:
temp_list4 = []
temp_list4 = prcess_contours(
cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset,
get_msg.roi[i].height, get_msg.roi[i].width,
get_msg.score[i])
if temp_list4 != []:
element_data.id_info = temp_list4
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[0]:
temp_list5 =[]
temp_list = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list5!=[]:
element_data.id_info = temp_list5
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[1]:
temp_list6 = []
temp_list6 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list6!=[]:
element_data.id_info = temp_list6
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[2]:
temp_list7 = []
temp_list7 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list7!=[]:
element_data.id_info = temp_list7
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "souppothandle" and get_msg.score[i] == souppothandle_score[0]:
temp_list8 = []
temp_list8 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list8!=[]:
element_data.id_info = temp_list8
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
'''
if get_msg.classname[i] == "nethandle" and get_msg.score[i] == nethandle_score[0]:
temp_list9 = []
temp_list9 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list9!=[]:
element_data.id_info = temp_list9
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "vegetablebowl" and get_msg.score[i] == vegetablebowl_score[0]:
temp_list10 = []
temp_list10 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list10!=[]:
element_data.id_info = temp_list10
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
if get_msg.classname[i] == "seasoningbowl" and get_msg.score[
i] == seasoningbowl_score[0]:
temp_list11 = []
temp_list11 = prcess_contours(
cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset,
get_msg.roi[i].height, get_msg.roi[i].width,
get_msg.score[i])
if temp_list11 != []:
element_data.id_info = temp_list11
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "seasoningbottle":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > bottle1[0] and center_x < bottle1[
1] and center_y > bottle1[2] and center_y < bottle1[3]:
temp_list12 = []
temp_list12 = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list12 != []:
single_obj.classname.append("seasoningbottle1")
element_data.id_info = temp_list12
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "seasoningbottle":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > bottle2[0] and center_x < bottle2[
1] and center_y > bottle2[2] and center_y < bottle2[3]:
temp_list13 = []
temp_list13 = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list13 != []:
single_obj.classname.append("seasoningbottle2")
element_data.id_info = temp_list13
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
if get_msg.classname[i] == "panhandle" and get_msg.score[i] == panhandle_score[0]:
temp_list14 = []
temp_list14 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list14!=[]:
element_data.id_info = temp_list14
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
#print("88888",single_obj)
if len(single_obj.score) > 0:
#print("1111:",single_obj)
temp_single_obj = single_obj
#print("****",temp_single_obj)
list_obj.single_obj_info.append(single_obj)
else:
list_obj.single_obj_info.append(temp_single_obj)
#print("1111:",list_obj.single_obj_info)
list_obj.header.stamp = data.header.stamp
list_obj.header.frame_id = data.header.frame_id
print("---" * 20)
print("--ww--", list_obj.single_obj_info)
list_obj.rgb_img_to_pos = bridge.cv2_to_imgmsg(rgb_img_to_pos,
encoding="bgr8")
list_obj.depth_img_to_pos = bridge.cv2_to_imgmsg(depth_img_to_pos,
"passthrough")
temp_det_bar_pos = Float32MultiArray(data=det_bar_pos)
#print("1111:",list_obj)
list_obj_pub = rospy.Publisher("process_uv/listobjs",
listobj,
queue_size=1)
temp_det_bar_pos_pub = rospy.Publisher("process_uv/bar_pos",
Float32MultiArray,
queue_size=1)
list_broc_pub = rospy.Publisher("process_uv/listbroc",
broclist,
queue_size=1)
list_broc_pub.publish(brocili_list)
list_obj_pub.publish(list_obj)
temp_det_bar_pos_pub.publish(temp_det_bar_pos)
cv2.rectangle(cv_image, (plate2[0], plate2[2]),
(plate2[0] + (plate2[1] - plate2[0]), plate2[2] +
(plate2[3] - plate2[2])), (0, 0, 255), 2)
cv2.rectangle(cv_image, (plate1[0], plate1[2]),
(plate1[0] + (plate1[1] - plate1[0]), plate1[2] +
(plate1[3] - plate1[2])), (0, 0, 255), 2)
cv2.rectangle(cv_image, (plate3[0], plate3[2]),
(plate3[0] + (plate3[1] - plate3[0]), plate3[2] +
(plate3[3] - plate3[2])), (0, 0, 255), 2)
#cv2.rectangle(cv_image,(bottle1[0], bottle1[2]),(bottle1[0]+(bottle1[1]-bottle1[0])/2, bottle1[2]+(bottle1[3]-bottle1[2])/2),(0,0,255),2)
#cv2.rectangle(cv_image,(bottle2[0], bottle2[2]),(bottle2[0]+(bottle2[1]-bottle2[0])/2, bottle2[2]+(bottle2[3]-bottle2[2])/2),(0,0,255),2)
cv2.imshow("img2", cv_image)
cv2.waitKey(1)
class MakeBoundingBox():
def __init__(self):
rospy.loginfo("pointcloud object detection is running...")
# frame size
self.frame_x = 640
self.frame_y = 480
self.bridge = CvBridge()
# cv_image and pcl variables
self.cv_image = np.zeros([self.frame_x, self.frame_y])
self.pcl = None
# transform config
# self.tf_pub = tf.TransformBroadcaster()
# load torch
weights_path = os.path.abspath(os.path.dirname(__file__)) + '/weights'
model_lst = [
x for x in sorted(os.listdir(weights_path)) if x.endswith('.pkl')
]
if len(model_lst) == 0:
print('No previous model found, please train first!')
exit()
else:
print('Using previous model %s' % model_lst[-1])
my_vgg = vgg.vgg19_bn(pretrained=True)
self.model = Model.Model(features=my_vgg.features, bins=2).cuda()
checkpoint = torch.load(weights_path + '/%s' % model_lst[-1])
self.model.load_state_dict(checkpoint['model_state_dict'])
self.model.eval()
# load yolo
yolo_path = os.path.abspath(os.path.dirname(__file__)) + '/weights'
self.yolo = cv_Yolo(yolo_path)
self.averages = ClassAverages.ClassAverages()
# TODO: clean up how this is done. flag?
self.angle_bins = generate_bins(2)
calib_path = os.path.abspath(
os.path.dirname(__file__)) + "/" + "camera_cal/"
self.calib_file = calib_path + "calib_cam_to_cam.txt"
# subscribers
self.img_sub = rospy.Subscriber("/kitti/camera_color_right/image_raw",
Image, self.rgb_callback)
#self.pcl_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.pcl_callback)
# publishers
self.img_detected_pub = rospy.Publisher(
"ROS_3D_BBox/img_detected_frame", Image, queue_size=100)
self.location_pub = rospy.Publisher("ROS_3D_BBox/location_array",
LocationArray,
queue_size=100)
self.rate = rospy.Rate(1)
def plot_regressed_3d_bbox(self,
img,
cam_to_img,
box_2d,
dimensions,
alpha,
theta_ray,
img_2d=None):
# the math! returns X, the corners used for constraint
location, X = calc_location(dimensions, cam_to_img, box_2d, alpha,
theta_ray)
orient = alpha + theta_ray
if img_2d is not None:
plot_2d_box(img_2d, box_2d)
ret_img = plot_3d_box(img, cam_to_img, orient, dimensions,
location) # 3d boxes
return location, ret_img
def rgb_callback(self, img_data):
try:
truth_img = self.bridge.imgmsg_to_cv2(img_data, 'bgr8')
except CvBridgeError as e:
print(e)
img = np.copy(truth_img)
yolo_img = np.copy(truth_img)
start_time = time.time()
detections = self.yolo.detect(yolo_img)
locations = []
for detection in detections:
if not self.averages.recognized_class(detection.detected_class):
continue
# this is throwing when the 2d bbox is invalid
# TODO: better check
try:
detectedObject = DetectedObject(img, detection.detected_class,
detection.box_2d,
self.calib_file)
except:
continue
theta_ray = detectedObject.theta_ray
input_img = detectedObject.img
proj_matrix = detectedObject.proj_matrix
box_2d = detection.box_2d
detected_class = detection.detected_class
input_tensor = torch.zeros([1, 3, 224, 224]).cuda()
input_tensor[0, :, :, :] = input_img
[orient, conf, dim] = self.model(input_tensor)
orient = orient.cpu().data.numpy()[0, :, :]
conf = conf.cpu().data.numpy()[0, :]
dim = dim.cpu().data.numpy()[0, :]
dim += self.averages.get_item(detected_class)
argmax = np.argmax(conf)
orient = orient[argmax, :]
cos = orient[0]
sin = orient[1]
alpha = np.arctan2(sin, cos)
alpha += self.angle_bins[argmax]
alpha -= np.pi
location, ret_img = self.plot_regressed_3d_bbox(
img, proj_matrix, box_2d, dim, alpha, theta_ray, truth_img)
loc_msg = Location()
loc_msg.point.x, loc_msg.point.y, loc_msg.point.z = location[
0], location[1], location[2]
loc_msg.size.x, loc_msg.size.y, loc_msg.size.z = dim[0], dim[
1], dim[2]
loc_msg.alpha = alpha
loc_msg.theta_ray = theta_ray
loc_msg.object_type = detection.detected_class
locations.append(loc_msg)
try:
img_msg = self.bridge.cv2_to_imgmsg(ret_img, 'bgr8')
except CvBridgeError as e:
print(e)
loc_msg_header = Header()
loc_msg_header.frame_id = 'locations array'
loc_msg_header.stamp = img_msg.header.stamp
loc_arr_msg = LocationArray(loc_msg_header, locations)
self.img_detected_pub.publish(img_msg)
self.location_pub.publish(loc_arr_msg)
print("\n")
print('Got %s poses in %.3f seconds FPS: %.2f' %
(len(detections), time.time() - start_time, 1 /
(time.time() - start_time)))
print('-------------')
with open('data.csv', 'a') as f:
f.write(
str(len(detections)) + ',' + str(time.time() - start_time) +
',' + str(1 / (time.time() - start_time)) + '\n')
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image)
self.bridge = CvBridge()
#self.image_sub = rospy.Subscriber("camera/rgb/image_raw",Image,self.callback)
#self.image_sub = rospy.Subscriber("usb_cam/image_raw", Image, self.callback)
self.fgbg = cv2.createBackgroundSubtractorMOG2()
self.blur = np.ones((5, 5), np.float32) / 25
cv2.namedWindow('camera')
# create trackbars for color change
#cv2.createTrackbar('R', 'image', 0, 255, nothing)
#cv2.createTrackbar('G', 'image', 0, 255, nothing)
#cv2.createTrackbar('B', 'image', 0, 255, nothing)
# Creating track bar
default_tapis_low = [0, 3, 0]
default_tapis_high = [152, 247, 255]
cv2.createTrackbar('H_low', 'camera', default_tapis_low[0], 179,
nothing)
cv2.createTrackbar('S_low', 'camera', default_tapis_low[1], 255,
nothing)
cv2.createTrackbar('V_low', 'camera', default_tapis_low[2], 255,
nothing)
cv2.createTrackbar('H_high', 'camera', default_tapis_high[0], 179,
nothing)
cv2.createTrackbar('S_high', 'camera', default_tapis_high[1], 255,
nothing)
cv2.createTrackbar('V_high', 'camera', default_tapis_high[2], 255,
nothing)
# create switch for ON/OFF functionality
self.switch = '0 : OFF \n1 : ON'
cv2.createTrackbar(self.switch, 'camera', 0, 1, nothing)
self.first = True
#switch = '0 : OFF \n1 : ON'
#cv2.createTrackbar(switch, 'image', 0, 1, nothing)
def add_blobs(self, crop_frame):
frame = cv2.GaussianBlur(crop_frame, (3, 3), 0)
# Convert BGR to HSV
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# define range of green color in HSV
lower_green = np.array([70, 50, 50])
upper_green = np.array([85, 255, 255])
# Threshold the HSV image to get only blue colors
mask = cv2.inRange(hsv, lower_green, upper_green)
mask = cv2.erode(mask, None, iterations=1)
mask = cv2.dilate(mask, None, iterations=1)
# Bitwise-AND mask and original image
res = cv2.bitwise_and(frame, frame, mask=mask)
detector = cv2.SimpleBlobDetector_create()
# Detect blobs.
reversemask = 255 - mask
keypoints = detector.detect(reversemask)
if keypoints:
print
"found blobs"
if len(keypoints) > 4:
keypoints.sort(key=(lambda s: s.size))
keypoints = keypoints[0:3]
# Draw detected blobs as red circles.
# cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob
im_with_keypoints = cv2.drawKeypoints(
frame, keypoints, np.array([]), (0, 0, 255),
cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
else:
print
"no blobs"
im_with_keypoints = crop_frame
return im_with_keypoints # , max_blob_dist, blob_center, keypoint_in_orders
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
mask = np.zeros(cv_image.shape[:2], np.uint8)
bgdModel = np.zeros((1, 65), np.float64)
fgdModel = np.zeros((1, 65), np.float64)
x1 = 10
x2 = 400
y1 = 10
y2 = 400
rect = (x1, x2, y1, y2)
# BACKGROUND SUBSTRACTION
cv_image = cv2.filter2D(cv_image, -1, self.blur)
#cv_image = self.fgbg.apply(cv_image)
#cv_image = cv2.medianBlur(cv_image, 5)
# COLOR DETECTION
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
cv_image = cv2.filter2D(cv_image, -1, self.blur)
lower_green = np.array([70, 50, 50])
upper_green = np.array([85, 255, 255])
mask = cv2.inRange(hsv, lower_green, upper_green)
mask = cv2.medianBlur(mask, 5)
cv_image = cv2.bitwise_and(cv_image, cv_image, mask=mask)
imgray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
im2, contours, hierarchy = cv2.findContours(imgray, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
if len(contours) != 0:
# draw in blue the contours that were founded
cv2.drawContours(cv_image, contours, -1, 255, 3)
# find the biggest area
c = max(contours, key=cv2.contourArea)
x, y, w, h = cv2.boundingRect(c)
# draw the book contour (in green)
cv2.rectangle(cv_image, (x, y), (x + w, y + h), (0, 255, 0), 2)
#cv2.drawContours(cv_image, contours, -1, (0, 255, 0), 3)
#cv2.grabCut(cv_image, mask, rect, bgdModel, fgdModel, 5, cv2.GC_INIT_WITH_RECT)
#mask2 = np.where((mask == 2) | (mask == 0), 0, 1).astype('uint8')
#cv_image = cv_image * mask2[:, :, np.newaxis]
#cv_image = self.add_blobs(cv_image)
#cv2.rectangle(cv_image, (x1,y1), (x2,y2), (255,0,0), 2)
cv2.imshow('image', imgray)
cv2.waitKey(1)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def callback_no_topic(self):
data = rospy.wait_for_message("/camera/rgb/image_raw", Image)
#data = rospy.wait_for_message("usb_cam/image_raw", Image)
try:
cv_image_full = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
if self.first:
self.r = cv2.selectROI(cv_image_full)
self.first = False
if cv2.getTrackbarPos(self.switch, 'camera') == 1:
self.r = cv2.selectROI(cv_image_full)
cv_image = cv_image_full[int(self.r[1]):int(self.r[1] + self.r[3]),
int(self.r[0]):int(self.r[0] + self.r[2])]
# BACKGROUND SUBSTRACTION
output = cv_image
cv_image = cv2.filter2D(cv_image, -1, self.blur)
# CONVERT TO HSV
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
# BLUR TO GET RID OF NOISE
cv_image = cv2.filter2D(cv_image, -1, self.blur)
# get info from track bar and appy to result
h_low = cv2.getTrackbarPos('H_low', 'camera')
s_low = cv2.getTrackbarPos('S_low', 'camera')
v_low = cv2.getTrackbarPos('V_low', 'camera')
h_high = cv2.getTrackbarPos('H_high', 'camera')
s_high = cv2.getTrackbarPos('S_high', 'camera')
v_high = cv2.getTrackbarPos('V_high', 'camera')
lower_green = np.array([h_low, s_low, v_low])
upper_green = np.array([h_high, s_high, v_high])
mask = cv2.inRange(hsv, lower_green, upper_green)
mask2 = mask
#mask = cv2.medianBlur(mask, 5)
#mask = cv2.medianBlur(mask, 5)
mask3 = mask
mask3 = cv2.bitwise_not(mask3)
invert = mask3
#cv_image3 = cv_image
#cv_image2 = cv2.bitwise_and(cv_image, cv_image, mask=mask)
#cv_image = cv_image2
#imgray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# TEST EXTRAIRE OBJET
im2, contours, hierarchy = cv2.findContours(mask3, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
#im2, contours, hierarchy = cv2.findContours(imgray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) != 0:
# draw in blue the contours that were founded
#cv2.drawContours(output, contours, -1, 255, 3)
# find the biggest area
c = max(contours, key=cv2.contourArea)
cv2.drawContours(output, c, -1, 255, 3)
x, y, w, h = cv2.boundingRect(c)
# draw the book contour (in green)
cv2.rectangle(output, (x, y), (x + w, y + h), (0, 255, 0), 2)
#cv2.drawContours(cv_image, contours, -1, (0, 255, 0), 3)
#cv2.grabCut(cv_image, mask, rect, bgdModel, fgdModel, 5, cv2.GC_INIT_WITH_RECT)
#mask2 = np.where((mask == 2) | (mask == 0), 0, 1).astype('uint8')
#cv_image = cv_image * mask2[:, :, np.newaxis]
#cv_image = self.add_blobs(cv_image)
#cv2.rectangle(cv_image, (x1,y1), (x2,y2), (255,0,0), 2)
cv2.imshow('inRange', mask2)
cv2.imshow('invert', invert)
cv2.imshow('camera', output)
cv2.waitKey(1)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
class MyNode(DTROS):
def __init__(self, node_name):
# initialize the DTROS parent class
super(MyNode, self).__init__(node_name=node_name)
# construct publisher and subsriber
self.pub = rospy.Publisher('/duckiesam/chatter', String, queue_size=10)
self.sub_image = rospy.Subscriber(
"/duckiesam/camera_node/image/compressed",
CompressedImage,
self.find_marker,
buff_size=921600,
queue_size=1)
self.pub_image = rospy.Publisher('/duckiesam/modified_image',
Image,
queue_size=1)
self.sub_info = rospy.Subscriber("/duckiesam/camera_node/camera_info",
CameraInfo,
self.get_camera_info,
queue_size=1)
self.pub_move = rospy.Publisher("/duckiesam/joy_mapper_node/car_cmd",
Twist2DStamped,
queue_size=1)
self.leader_detected = rospy.Publisher("/duckiesam/detection",
BoolStamped,
queue_size=1)
#values for detecting marker
self.starting = 0
self.ending = 0
self.camerainfo = PinholeCameraModel()
self.bridge = CvBridge()
self.gotimage = False
self.imagelast = None
self.processedImg = None
self.detected = False
#values for calculating pose of robot
self.solP = False
self.rotationvector = None
self.translationvector = None
self.axis = np.float32([[0.0125, 0, 0], [0, 0.0125, 0],
[0, 0, -0.0375]]).reshape(-1, 3)
self.distance = None
self.angle_f = None
self.angle_l = None
#values for driving the robot
self.maxdistance = 0.2
self.speedN = 0
self.e_vB = 0
self.rotationN = 0
self.mindistance = 0.1
self.d_e = 0 #distance error
self.d_e_1 = 5
self.d_e_2 = 10
self.y2 = 0
self.controltime = rospy.Time.now()
self.Kp = 1
self.Ki = 0.1
self.Kd = 0
self.I = 0
self.Rp = 1
self.Ri = 1
self.Rd = 1
def initialvalues(self):
self.default_v = 0.22
self.maxdistance = 0.3
self.speedN = 0
self.e_vB = 0
self.rotationN = 0
self.mindistance = 0.2
self.d_e = 0 #distance error
self.d_e_1 = 5
self.d_e_2 = 10
self.y2 = 0
self.controltime = rospy.Time.now()
self.Kp = 1
self.Ki = 0.1
self.Kd = 0
self.I = 0
self.Rp = 1
self.Ri = 1
self.Rd = 1
#get camera info for pinhole camera model
def get_camera_info(self, camera_msg):
self.camerainfo.fromCameraInfo(camera_msg)
#step 1 : find the back circle grids using cv2.findCirclesGrid
##### set (x,y) for points and flag for detection
def find_marker(self, image_msg):
try:
self.imagelast = self.bridge.compressed_imgmsg_to_cv2(
image_msg, "bgr8")
except CvBridgeError as e:
print(e)
if self.gotimage == False:
self.gotimage = True
#time checking
self.starting = rospy.Time.now()
#from_last_image = (self.starting - self.ending).to_sec()
gray = cv2.cvtColor(self.imagelast, cv2.COLOR_BGR2GRAY)
detection, corners = cv2.findCirclesGrid(gray, (7, 3))
self.processedImg = self.imagelast.copy()
cmd = Twist2DStamped()
cmd.header.stamp = self.starting
if detection:
cv2.drawChessboardCorners(self.processedImg, (7, 3), corners,
detection)
self.detected = True
#self.controltime = rospy.Time.now()
twoone = []
for i in range(0, 21):
point = [corners[i][0][0], corners[i][0][1]]
twoone.append(point)
twoone = np.array(twoone)
self.originalmatrix()
self.gradient(twoone)
self.detected = self.solP
img_out = self.bridge.cv2_to_imgmsg(self.processedImg, "bgr8")
self.pub_image.publish(img_out)
self.find_distance()
###self.move(self.y2, self.angle_l, self.distance)
self.ending = rospy.Time.now()
else:
self.detected = False
img_out = self.bridge.cv2_to_imgmsg(gray, "bgr8")
self.pub_image.publish(img_out)
self.ending = rospy.Time.now()
cmd.v = 0
cmd.omega = 0
####self.pub_move.publish(cmd)
#step 2 : makes matrix for 3d original shape
def originalmatrix(self):
#coners and points
self.originalmtx = np.zeros([21, 3])
for i in range(0, 7):
for j in range(0, 3):
self.originalmtx[i + j * 7, 0] = 0.0125 * i - 0.0125 * 3
self.originalmtx[i + j * 7, 1] = 0.0125 * j - 0.0125
#step 3 : use intrinsic matrix and solvePnP, return rvec and tvec, print axis
def gradient(self, imgpts):
#using solvePnP to find rotation vector and translation vector and also find 3D point to the image plane
self.solP, self.rotationvector, self.translationvector = cv2.solvePnP(
self.originalmtx, imgpts, self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
if self.solP:
pointsin3D, jacoB = cv2.projectPoints(
self.originalmtx, self.rotationvector, self.translationvector,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
pointaxis, _ = cv2.projectPoints(
self.axis, self.rotationvector, self.translationvector,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[0].ravel()),
(255, 0, 0), 2)
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[1].ravel()),
(0, 255, 0), 2)
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[2].ravel()),
(0, 0, 255), 3)
#textdistance = "x = %s, y = %s, z = %s" % (self.distance, self.angle_f, self.angle_l, self.y2)
#rospy.loginfo("%s" % textdistance)
#step 4 : find distance between robot and following robot print out distance and time
def find_distance(self):
#use tvec to calculate distance
tvx = self.translationvector[0]
tvy = self.translationvector[1]
tvz = self.translationvector[2]
self.distance = math.sqrt(tvx * tvx + tvz * tvz)
self.angle_f = np.arctan2(tvx[0], tvz[0])
R, _ = cv2.Rodrigues(self.rotationvector)
R_inverse = np.transpose(R)
self.angle_l = np.arctan2(
-R_inverse[2, 0],
math.sqrt(R_inverse[2, 1]**2 + R_inverse[2, 2]**2))
T = np.array([-np.sin(self.angle_l), np.cos(self.angle_l)])
tvecW = -np.dot(R_inverse, self.translationvector)
x_y = np.array([tvz[0], tvx[0]])
self.y2 = -np.dot(T, x_y) - 0.01 * np.sin(self.angle_l)
textdistance = "Distance = %s, Angle of Follower = %s, Angle of Leader = %s, y = %s" % (
self.distance, self.angle_f, self.angle_l, self.y2)
rospy.loginfo("%s" % textdistance)
#self.pub.publish(textdistance)
#step 5 : use joy mapper to control the robot PID controller
def move(self, y_to, angle_to, d):
#y_to is needed y value to be parallel to leader's center line
#angle_to is angle needed to rotate
#d is distance between required position and now position
cmd = Twist2DStamped()
time = rospy.Time.now()
cmd.header.stamp = time
dt = (time - self.controltime).to_sec()
if dt > 3:
if d < self.maxdistance:
cmd.v = 0
cmd.omega = 0
else:
self.d_e = d - self.maxdistance
error_d = (self.d_e - self.d_e_1) / dt
errorB = (self.d_e_1 - self.d_e_2) / dt
e_v = error_d - errorB
P = self.Kp * (e_v)
self.I = self.I + self.Ki * (e_v + self.e_vB) / 2 * dt
D = self.Kd * (e_v + self.e_vB) / dt
self.speedN = P + self.I + D
self.rotationN = self.Rp * (y_to) + self.Ri * (
angle_to) + self.Rd * (np.sin(angle_to))
cmd.v = self.speedN
cmd.omega = self.rotationN
self.e_vB = e_v
self.d_e_2 = self.d_e_1
self.d_e_1 = self.d_e
if self.d_e < 0.05 or self.speedN < 0:
cmd.v = 0
cmd.omega = 0
textdistance = "Velocity = %s, Rotation = %s" % (cmd.v, cmd.omega)
rospy.loginfo("%s" % textdistance)
self.pub_move.publish(cmd)
self.controltime = time
class camera_node(object):
def __init__(self):
'''
Camera node constructor
'''
rospy.init_node(
"camera_node") # Register this as ROS node named 'camera_node'
self.cap = None # Will store opencv capture device
self.bridge = CvBridge(
) # Bridge used to convert CV images into ROS Image messages
self._load_ros_parameters()
self._initialize_camera() # initialize camera
atexit.register(
self._exit_handler) # Register function to be called upon exit
signal.signal(signal.SIGINT,
self._signal_handler) # Register ctrl-c signal handler
self.cap_pub = rospy.Publisher(
CAM_TOPIC, Image,
queue_size=10) # Create publisher for camera images
def _load_ros_parameters(self):
'''
Calling this function tries to update globals for this module using ROS params
'''
global CAM_TOPIC, CAM_DEVICE_INDEX, CV_BRIDGE_IMAGE_ENCODING, SHOW_CAM
try:
CAM_DEVICE_INDEX = rospy.get_param("/CAM_DEVICE_INDEX")
except:
pass
try:
SHOW_CAM = rospy.get_param("/DISPLAY_CAM_WINDOW")
except:
pass
try:
CAM_TOPIC = rospy.get_param("/CAM_TOPIC_NAME")
except:
pass
try:
CV_BRIDGE_IMAGE_ENCODING = rospy.get_param(
"/CV_BRIDGE_IMAGE_ENCODING")
except:
pass
def _initialize_camera(self):
'''
This function is called to initialize the camera sensor.
This function in a helper function for the camera node class.
'''
self.cap = cv2.VideoCapture(CAM_DEVICE_INDEX)
def transmit_video(self):
'''
This function runs until ROS is shutdown or process is killed. Continuously captures
frames and publishes them over ROS topic.
'''
rate = rospy.Rate(50) # create a 10Hz rate
while not rospy.is_shutdown():
ret, frame = self.cap.read() # Grab current frame from webcam
# Only show cv2 image if show cam parameter is true
if (SHOW_CAM):
cv2.imshow("frame", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# publish frame as ROS Image message using cv_bridge
try:
self.cap_pub.publish(
self.bridge.cv2_to_imgmsg(frame, CV_BRIDGE_IMAGE_ENCODING))
except CvBridgeError, e:
# TODO: log error
print(e)
rate.sleep()
class ObjectDetect:
def __init__(self):
print('openCV version:', cv2.__version__)
self.image_pub = rospy.Publisher("/object_detect", Image, queue_size=2)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/cf1/camera/image_raw", Image,
self.callback)
##### DNN stuff #####
# arguments from the argument parser of the example
self.prototxt = '/home/robot/dd2419_ws/src/crazyflie_9/milestone2/include/dnn_models/MobileNetSSD_deploy.prototxt.txt'
self.model = '/home/robot/dd2419_ws/src/crazyflie_9/milestone2/include/dnn_models/MobileNetSSD_deploy.caffemodel'
self.confidence = 0.2 # Minimum probability
# initialize the list of class labels MobileNet SSD was trained to
# detect, then generate a set of bounding box colors for each class
self.CLASSES = [
"background", "aeroplane", "bicycle", "bird", "boat", "bottle",
"bus", "car", "cat", "chair", "cow", "diningtable", "dog", "horse",
"motorbike", "person", "pottedplant", "sheep", "sofa", "train",
"tvmonitor"
]
self.COLORS = np.random.uniform(0, 255, size=(len(self.CLASSES), 3))
# load our serialized model from disk
print("[INFO] loading model...")
self.net = cv2.dnn.readNetFromCaffe(self.prototxt, self.model)
rospy.sleep(5)
def callback(self, data):
# Convert the image from OpenCV to ROS format
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
##
# Do the dnn stuff to the image
##
result = self.dnn_object_detect(cv_image)
# Publish the image
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(result, "bgr8"))
except CvBridgeError as e:
print(e)
def dnn_object_detect(self, image):
(h, w) = image.shape[:2]
blob = cv2.dnn.blobFromImage(cv2.resize(image, (300, 300)), 0.007843,
(300, 300), 127.5)
# pass the blob through the network and obtain the detections and
# predictions
print("[INFO] computing object detections...")
self.net.setInput(blob)
detections = self.net.forward()
# loop over the detections
for i in np.arange(0, detections.shape[2]):
# extract the confidence (i.e., probability) associated with the
# prediction
confidence = detections[0, 0, i, 2]
# filter out weak detections by ensuring the `confidence` is
# greater than the minimum confidence
if confidence > self.confidence:
# extract the index of the class label from the `detections`,
# then compute the (x, y)-coordinates of the bounding box for
# the object
idx = int(detections[0, 0, i, 1])
box = detections[0, 0, i, 3:7] * np.array([w, h, w, h])
(startX, startY, endX, endY) = box.astype("int")
# display the prediction
label = "{}: {:.2f}%".format(self.CLASSES[idx],
confidence * 100)
print("[INFO] {}".format(label))
cv2.rectangle(image, (startX, startY), (endX, endY),
self.COLORS[idx], 2)
y = startY - 15 if startY - 15 > 15 else startY + 15
cv2.putText(image, label, (startX, y),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, self.COLORS[idx], 2)
return image
def image_publisher(self, image):
bridge = CvBridge()
msg = bridge.cv2_to_imgmsg(image)
self.get_logger().info('Publishing something !')
self.publisher.publish(msg)
def detect_video(self, yolo, video_path):
from PIL import Image, ImageFont, ImageDraw
#Start ROS node
# pub, pub_flag, pub_track, pub_frame1, pub_frame2 = start_node()
pub, pub_flag, pub_frame1, pub_frame2 = start_node()
vid = RealsenseCapture()
vid.start()
bridge = CvBridge()
accum_time = 0
curr_fps = 0
fps = "FPS: ??"
prev_time = timer()
worldy = 0.0
flag = 0
while True:
# pub_track.publish(0)
# flag = 0
if self.garbage_in_can == 1:
# print("ゴミを捨てました")
flag = 0
pub_flag.publish(flag)
ret, frames, _ = vid.read()
frame = frames[0]
depth_frame = frames[1]
image = Image.fromarray(frame)
image, bottle, person, right, left, bottom, top, right2, left2, bottom2, top2 = yolo.detect_image(
image, pub)
result = np.asarray(image)
curr_time = timer()
exec_time = curr_time - prev_time
prev_time = curr_time
accum_time = accum_time + exec_time
curr_fps = curr_fps + 1
if accum_time > 1:
accum_time = accum_time - 1
fps = "FPS: " + str(curr_fps)
curr_fps = 0
cv2.putText(result,
text=fps,
org=(3, 15),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.50,
color=(255, 0, 0),
thickness=2)
# cv2.imshow("result", result)
yolo_frame = bridge.cv2_to_imgmsg(result, "bgr8")
pub_frame1.publish(yolo_frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
if (bottle == False) or (person == False):
continue
# ------------------------------Tracking-----------------------------------
# tracker_types = ['BOOSTING', 'MIL','KCF', 'TLD', 'MEDIANFLOW', 'GOTURN', 'MOSSE', 'CSRT']
# tracker_type = tracker_types[7]
tracker = cv2.TrackerCSRT_create()
tracker2 = cv2.TrackerCSRT_create()
# setup initial location of window
left, right, top, bottom = left, right, top, bottom
r, h, ci, w = top, bottom - top, left, right - left # simply hardcoded the values r, h, c, w
frame_b, frame_g, frame_r = frame[:, :, 0], frame[:, :,
1], frame[:, :,
2]
hist_b = cv2.calcHist([frame_b[top:bottom, left:right]], [0], None,
[256], [0, 256])
hist_g = cv2.calcHist([frame_g[top:bottom, left:right]], [0], None,
[256], [0, 256])
hist_r = cv2.calcHist([frame_r[top:bottom, left:right]], [0], None,
[256], [0, 256])
cv2.normalize(hist_b, hist_b, 0, 255, cv2.NORM_MINMAX)
cv2.normalize(hist_g, hist_g, 0, 255, cv2.NORM_MINMAX)
cv2.normalize(hist_r, hist_r, 0, 255, cv2.NORM_MINMAX)
track_window = (ci, r, w, h)
r2, h2, ci2, w2 = top2, bottom2 - top2, left2, right2 - left2 # simply hardcoded the values r, h, c, w
hist_bp = cv2.calcHist([frame_b[top2:bottom2, left2:right2]], [0],
None, [256], [0, 256])
hist_gp = cv2.calcHist([frame_g[top2:bottom2, left2:right2]], [0],
None, [256], [0, 256])
hist_rp = cv2.calcHist([frame_r[top2:bottom2, left2:right2]], [0],
None, [256], [0, 256])
cv2.normalize(hist_bp, hist_bp, 0, 255, cv2.NORM_MINMAX)
cv2.normalize(hist_gp, hist_gp, 0, 255, cv2.NORM_MINMAX)
cv2.normalize(hist_rp, hist_rp, 0, 255, cv2.NORM_MINMAX)
track_window2 = (ci2, r2, w2, h2)
cv2.imwrite('bottledetect.jpg', frame[r:r + h, ci:ci + w])
cv2.imwrite('persondetect.jpg', frame[r2:r2 + h2, ci2:ci2 + w2])
# set up the ROI for tracking
roi = frame[r:r + h, ci:ci + w]
hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_roi, np.array((0., 60., 32.)),
np.array((180., 255., 255.)))
roi_hist = cv2.calcHist([hsv_roi], [0], mask, [180], [0, 180])
cv2.normalize(roi_hist, roi_hist, 0, 255, cv2.NORM_MINMAX)
# Setup the termination criteria, either 10 iteration or move by atleast 1 pt
term_crit = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10,
1)
ok = tracker.init(frame, track_window)
ok2 = tracker2.init(frame, track_window2)
track_thing = 0 #bottle
pts = Point()
pts2 = Point()
untrack = 0
self.emergency_stop = 0
# flag = 0
# pub_flag.publish(flag)
while (1):
ret, frames, depth = vid.read()
frame = frames[0]
depth_frame = frames[1]
if ret == True:
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
dst = cv2.calcBackProject([hsv], [0], roi_hist, [0, 180],
1)
# apply meanshift to get the new location
ok, track_window = tracker.update(frame)
x, y, w, h = track_window
ok, track_window2 = tracker2.update(frame)
x2, y2, w2, h2 = track_window2
# Draw it on image
img2 = cv2.rectangle(frame, (x, y), (x + w, y + h), 255, 2)
if not track_thing:
img2 = cv2.rectangle(img2, (x2, y2),
(x2 + w2, y2 + h2), 255, 2)
else:
img2 = cv2.rectangle(img2, (x2, y2),
(x2 + w2, y2 + h2), (0, 0, 255),
2)
# cv2.imshow('Tracking',img2)
tracking_frame = bridge.cv2_to_imgmsg(img2, "bgr8")
pub_frame2.publish(tracking_frame)
# https://www.intelrealsense.com/wp-content/uploads/2020/06/Intel-RealSense-D400-Series-Datasheet-June-2020.pdf
total, cnt = 0, 0
for i in range(3):
for j in range(3):
dep = depth.get_distance(
np.maximum(0, np.minimum(i + x + w // 2, 639)),
np.maximum(0, np.minimum(j + y + h // 2, 479)))
if (dep) != 0:
total += dep
cnt += 1
if cnt != 0:
worldz = total / cnt
# 人にぶつからないように距離を確保するため
if (worldz < 0.6) or (worldz > 3.0):
worldz = 0
else:
worldz = 0
total2, cnt2 = 0, 0
for i in range(3):
for j in range(3):
dep2 = depth.get_distance(
np.maximum(0,
np.minimum(i + x2 + w2 // 2, 639)),
np.maximum(0,
np.minimum(j + y2 + h2 // 2, 479)))
if dep2 != 0:
total2 += dep2
cnt2 += 1
if cnt2 != 0:
worldz2 = total2 / cnt2
if (worldz2 < 0.6) or (worldz2 > 3.0):
worldz2 = 0
else:
worldz2 = 0
# print('worldz', worldz)
# print('worldz2', worldz2)
# if (worldz == 0) or (worldz2 == 0):
if worldz2 == 0:
# break
worldx, worldy = 0, 0
# worldx = 0
pts.x, pts.y, pts.z = 0.0, 0.0, 0.0
worldx2, worldy2 = 0, 0
pts2.x, pts2.y, pts2.z = 0.0, 0.0, 0.0
else:
# focus length = 1.93mm, distance between depth cameras = about 5cm, a pixel size = 3um
if (track_thing == 0) and (not worldz == 0):
#bottle Tracking
u_ud = (0.05 * 1.88 * 10**(-3)) / (3 * 10**(-6) *
worldz)
# print('u_ud', u_ud)
# print('x, y =', (x+w//2)-(img2.shape[1]//2), (img2.shape[0]//2)-(y+h//2))
# 深度カメラとカラーカメラの物理的な距離を考慮した項(-0.3*u_ud)
# これらの座標は物体を見たときの左の深度カメラを基準とする
worldx = 0.05 * (x + w // 2 -
(img2.shape[1] // 2) -
0.3 * u_ud) / u_ud
worldy = 0.05 * ((img2.shape[0] // 2) -
(y + h)) / u_ud
print('x,y,z = ', worldx, worldy, worldz - 0.6)
pts.y, pts.z, pts.x = -1.0 * float(worldx), float(
worldy), float(worldz) - 0.6
# pts.y, pts.z, pts.x = float(0.0), float(worldy), float(1.0)
else:
#human Tracking
# if worldz==0:
# worldx, worldy = 0, 0
# pts.x, pts.y, pts.z = 0.0, 0.0, 0.0
u_ud = (0.05 * 1.88 * 10**(-3)) / (3 * 10**(-6) *
worldz2)
worldx2 = 0.05 * (x2 + w2 // 2 -
(img2.shape[1] // 2) -
0.3 * u_ud) / u_ud
worldy2 = 0.05 * ((img2.shape[0] // 2) -
(y2 + h2)) / u_ud
print('x2,y2,z2 = ', worldx2, worldy2,
worldz2 - 0.6)
pts2.y, pts2.z, pts2.x = -1.0 * float(
worldx2), float(worldy2), float(worldz2) - 0.6
# pts.y, pts.z, pts.x = float(0.0), float(worldy), float(1.0)
if worldz == 0:
worldx, worldy = 0, 0
pts.x, pts.y, pts.z = 0.0, 0.0, 0.0
print("track_thing = ", track_thing)
frame_b, frame_g, frame_r = frame[:, :,
0], frame[:, :,
1], frame[:, :,
2]
hist_b2 = cv2.calcHist([frame_b[y:y + h, x:x + w]], [0],
None, [256], [0, 256])
hist_g2 = cv2.calcHist([frame_g[y:y + h, x:x + w]], [0],
None, [256], [0, 256])
hist_r2 = cv2.calcHist([frame_r[y:y + h, x:x + w]], [0],
None, [256], [0, 256])
cv2.normalize(hist_b2, hist_b2, 0, 255, cv2.NORM_MINMAX)
cv2.normalize(hist_g2, hist_g2, 0, 255, cv2.NORM_MINMAX)
cv2.normalize(hist_r2, hist_r2, 0, 255, cv2.NORM_MINMAX)
hist_bp2 = cv2.calcHist([frame_b[y2:y2 + h2, x2:x2 + w2]],
[0], None, [256], [0, 256])
hist_gp2 = cv2.calcHist([frame_g[y2:y2 + h2, x2:x2 + w2]],
[0], None, [256], [0, 256])
hist_rp2 = cv2.calcHist([frame_r[y2:y2 + h2, x2:x2 + w2]],
[0], None, [256], [0, 256])
cv2.normalize(hist_bp2, hist_bp2, 0, 255, cv2.NORM_MINMAX)
cv2.normalize(hist_gp2, hist_gp2, 0, 255, cv2.NORM_MINMAX)
cv2.normalize(hist_rp2, hist_rp2, 0, 255, cv2.NORM_MINMAX)
comp_b = cv2.compareHist(hist_b, hist_b2,
cv2.HISTCMP_CORREL)
comp_g = cv2.compareHist(hist_g, hist_g2,
cv2.HISTCMP_CORREL)
comp_r = cv2.compareHist(hist_r, hist_r2,
cv2.HISTCMP_CORREL)
comp_bp = cv2.compareHist(hist_bp, hist_bp2,
cv2.HISTCMP_CORREL)
comp_gp = cv2.compareHist(hist_gp, hist_gp2,
cv2.HISTCMP_CORREL)
comp_rp = cv2.compareHist(hist_rp, hist_rp2,
cv2.HISTCMP_CORREL)
# print('compareHist(b)', comp_b)
# print('compareHist(g)', comp_g)
# print('compareHist(r)', comp_r)
# print('compareHist(bp)', comp_bp)
# print('compareHist(gp)', comp_gp)
# print('compareHist(rp)', comp_rp)
# print("garbage_in_can", garbage_in_can)
# 追跡を止める条件は，bottle追跡中にヒストグラムが大きく変化するか枠が無くなるまたはpersonを見失う，もしくはperson追跡中にヒストグラムが大きく変化するか枠が無くなるまたはゴミがゴミ箱に入れられた，
if ((track_thing == 0 and
((comp_b <= 0.1) or (comp_g <= 0.1) or
(comp_r <= 0.1) or track_window == (0, 0, 0, 0)))
or (track_window2 == (0, 0, 0, 0))
or (track_thing == 1 and
((comp_bp <= 0.) or (comp_gp <= 0.) or
(comp_rp <= 0.)))):
untrack += 1
print("untrack = ", untrack)
if untrack >= 30:
print("追跡が外れた！\n")
break
elif (track_thing == 0 and (x + w > 640 or x < 0) and
(y + h > 480
or y < 0)) or (track_thing == 1 and
(x2 + w2 > 640 or x2 < 0) and
(y2 + h2 > 480 or y2 < 0)):
untrack += 1
print("untrack = ", untrack)
if untrack >= 50:
print("枠が画面外で固まった")
break
# elif (track_thing==1 and self.garbage_in_can==1):
elif self.garbage_in_can == 1:
print("ゴミを捨てたため追跡を終えます")
flag = 0
break
else:
untrack = 0
# print('garbage_in_can', self.garbage_in_can)
# ポイ捨ての基準はbottleを追跡していて，地面から10cmのところまで落ちたか，bottleを見失ったかつ見失う前のフレームでの高さがカメラの10cmより下
# print('track_window = ', track_window)
if (((worldy <= -0.01) or
((track_window == (0, 0, 0, 0) or untrack >= 1) and
(worldy < 0.5))) and (not track_thing)):
print("ポイ捨てした！\n")
track_thing = 1 #human
# if track_thing==0:
# tracking_point = pts
# if not (pts.x==0.0 and pts.y==0.0 and pts.z==0.0):
# pub.publish(tracking_point)
# flag = 0 #bottle
# else:
# tracking_point = pts2
# if not (pts2.x==0.0 and pts2.y==0.0 and pts2.z==0.0):
# pub.publish(tracking_point)
# flag = 1 #person
if track_thing == 1:
tracking_point = pts2
if not (pts2.x == 0.0 and pts2.y == 0.0
and pts2.z == 0.0):
pub.publish(tracking_point)
flag = 1
# else:
# flag = 0
pub_flag.publish(flag)
k = cv2.waitKey(1) & 0xff
# print("emergency_stop", self.emergency_stop)
# if (k == 27) or self.emergency_stop==1: # dev
if self.emergency_stop == 1: # ops
print('emergency_stop == 1')
break
else:
break
# pub_track.publish(1)
# pub_flag.publish(flag)
yolo.close_session()
class CPM(object):
"""
This class takes in image data and finds / annotates objects within the image
"""
def __init__(self):
rospy.init_node('rail_pose_estimator_node')
self.person_keypoints = []
self.keypoint_arrays = []
self.image_datastream = None
self.input_image = None
self.bridge = CvBridge()
self.detector = pose_estimation.PoseMachine()
self.debug = rospy.get_param('~debug', default=False)
self.image_sub_topic_name = rospy.get_param(
'~image_sub_topic_name', default='/camera/rgb/image_raw')
self.use_compressed_image = rospy.get_param('~use_compressed_image',
default=False)
self.part_str = [
'nose', 'neck', 'right_shoulder', 'right_elbow', 'right_wrist',
'left_shoulder', 'left_elbow', 'left_wrist', 'Rhip', 'Rkne',
'Rank', 'Lhip', 'Lkne', 'Lank', 'right_eye', 'left_eye',
'right_ear', 'left_ear'
]
def _convert_msg_to_image(self, image_msg):
"""
Convert an incoming image message (compressed or otherwise) into a cv2
image
"""
if not self.use_compressed_image:
try:
image_cv = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
except CvBridgeError as e:
print e
return None
else:
image_np = np.fromstring(image_msg.data, np.uint8)
image_cv = cv2.imdecode(image_np, cv2.CV_LOAD_IMAGE_COLOR)
return image_cv
def _parse_image(self, image_msg):
header = image_msg.header
image_cv = self._convert_msg_to_image(image_msg)
if image_cv is None:
return
# self.person_keypoints = self.detector.estimate_keypoints(image_cv)
# candidate, subset = self.detector.estimate_keypoints(image_cv)
people = self.detector.estimate_keypoints(image_cv)
#### DEBUG ####
if self.debug:
# out_image = self.detector.visualize_keypoints(image_cv, candidate, subset)
out_image = self.detector.visualize_keypoints(image_cv, people)
try:
image_msg = self.bridge.cv2_to_imgmsg(out_image, "bgr8")
except CvBridgeError as e:
print e
image_msg.header = header
self.image_pub.publish(image_msg)
#### END DEBUG ####
# Instantiate poses object
obj_arr = Poses()
obj_arr.header = header
for person in people:
msg = Keypoints()
nose = person.get('nose', [-1, -1])
msg.nose_y = nose[0]
msg.nose_x = nose[1]
neck = person.get('neck', [-1, -1])
msg.neck_y = neck[0]
msg.neck_x = neck[1]
right_shoulder = person.get('right_shoulder', [-1, -1])
msg.right_shoulder_y = right_shoulder[0]
msg.right_shoulder_x = right_shoulder[1]
left_shoulder = person.get('left_shoulder', [-1, -1])
msg.left_shoulder_y = left_shoulder[0]
msg.left_shoulder_x = left_shoulder[1]
right_elbow = person.get('right_elbow', [-1, -1])
msg.right_elbow_y = right_elbow[0]
msg.right_elbow_x = right_elbow[1]
left_elbow = person.get('left_elbow', [-1, -1])
msg.left_elbow_y = left_elbow[0]
msg.left_elbow_x = left_elbow[1]
right_wrist = person.get('right_wrist', [-1, -1])
msg.right_wrist_y = right_wrist[0]
msg.right_wrist_x = right_wrist[1]
left_wrist = person.get('left_wrist', [-1, -1])
msg.left_wrist_y = left_wrist[0]
msg.left_wrist_x = left_wrist[1]
Lhip = person.get('Lhip', [-1, -1])
msg.left_hip_y = Lhip[0]
msg.left_hip_x = Lhip[1]
Rhip = person.get('Rhip', [-1, -1])
msg.right_hip_y = Rhip[0]
msg.right_hip_x = Rhip[1]
left_eye = person.get('left_eye', [-1, -1])
msg.left_eye_y = left_eye[0]
msg.left_eye_x = left_eye[1]
right_eye = person.get('right_eye', [-1, -1])
msg.right_eye_y = right_eye[0]
msg.right_eye_x = right_eye[1]
right_ear = person.get('right_ear', [-1, -1])
msg.right_ear_y = right_ear[0]
msg.right_ear_x = right_ear[1]
left_ear = person.get('left_ear', [-1, -1])
msg.left_ear_y = left_ear[0]
msg.left_ear_x = left_ear[1]
Rkne = person.get('Rkne', [-1, -1])
msg.right_knee_y = Rkne[0]
msg.right_knee_x = Rkne[1]
Lkne = person.get('Lkne', [-1, -1])
msg.left_knee_y = Lkne[0]
msg.left_knee_x = Lkne[1]
Rank = person.get('Rank', [-1, -1])
msg.right_ankle_y = Rank[0]
msg.right_ankle_x = Rank[1]
Lank = person.get('Lank', [-1, -1])
msg.left_ankle_y = Lank[0]
msg.left_ankle_x = Lank[1]
obj_arr.people.append(msg)
self.object_pub.publish(obj_arr)
def read_points(self, points):
# print points
pass
def pcl_callback(self, pcl_msg):
all_points = np.array([3])
print all_points
for point in pc2.read_points(pcl_msg, skip_nans=True):
pt_x = point[0]
pt_y = point[1]
pt_z = point[2]
points = np.array([pt_x, pt_y, pt_z])
all_points = np.append(all_points, points)
self.read_points(all_points)
def run(self,
pub_image_topic='~debug/poses_image',
pub_object_topic='~poses'):
if not self.use_compressed_image:
rospy.Subscriber(self.image_sub_topic_name, Image,
self._parse_image)
rospy.Subscriber("/pcl_table",
pc2.PointCloud2,
self.pcl_callback,
queue_size=1)
else:
rospy.Subscriber(self.image_sub_topic_name + '/compressed',
CompressedImage, self._parse_image)
if self.debug:
self.image_pub = rospy.Publisher(pub_image_topic,
Image,
queue_size=2) # image publisher
self.object_pub = rospy.Publisher(pub_object_topic,
Poses,
queue_size=2) # objects publisher
rospy.spin()
class ImageListener:
def __init__(self,
object_list,
cfg_list,
checkpoint_list,
codebook_list,
modality,
cad_model_dir,
category='ycb',
refine_single=True,
refine_multiple=False,
use_depth=True,
use_self_supervised_ckpts=True):
print(' *** Initializing PoseRBPF ROS Node ... ')
# variables
self.cv_bridge = CvBridge()
self.count = 0
self.objects = []
self.frame_names = []
self.frame_lost = []
self.renders = dict()
self.n_renders = 0
self.num_lost = 50
self.queue_size = 10
self.scene = 1
self.use_depth = use_depth
self.posecnn_classes = ('__background__', '002_master_chef_can', '003_cracker_box', '004_sugar_box', '005_tomato_soup_can', \
'006_mustard_bottle', '007_tuna_fish_can', '008_pudding_box', '009_gelatin_box', '010_potted_meat_can', \
'011_banana', '019_pitcher_base', '021_bleach_cleanser', '024_bowl', '025_mug', '035_power_drill', \
'036_wood_block', '037_scissors', '040_large_marker', '052_extra_large_clamp', '061_foam_brick')
# initialize poserbpf with cfg_file
self.object_list = object_list
self.cfg_list = cfg_list
self.ckpt_list = checkpoint_list
self.codebook_list = codebook_list
self.pose_rbpf = PoseRBPF(self.object_list,
self.cfg_list,
self.ckpt_list,
self.codebook_list,
category,
modality,
cad_model_dir,
refine=refine_single)
self.ssv_ckpts = use_self_supervised_ckpts
# initial sdf refinement for multiple objects at once
self.refine_multiple = refine_multiple
if refine_multiple:
print('loading SDFs')
sdf_files = []
for cls in self.object_list:
sdf_file = '{}/ycb_models/{}/textured_simple_low_res.pth'.format(
cad_model_dir, cls)
sdf_files.append(sdf_file)
reg_trans = 1000.0
reg_rot = 10.0
self.sdf_optimizer = sdf_multiple_optimizer(
self.object_list, sdf_files, reg_trans, reg_rot)
# target list
self.target_list = range(len(self.pose_rbpf.obj_list))
self.target_object = cfg.TEST.OBJECTS[0]
self.pose_rbpf.add_object_instance(self.target_object)
self.target_cfg = self.pose_rbpf.set_target_obj(0)
self.class_info = torch.ones((1, 1, 128, 128), dtype=torch.float32)
self.init_failure_steps = 0
self.input_rgb = None
self.input_depth = None
self.input_seg = None
self.input_rois = None
self.input_stamp = None
self.input_frame_id = None
self.input_joint_states = None
self.input_robot_joint_states = None
self.main_thread_free = True
self.kf_time_stamp = None
# roi information
self.prefix = '00_'
self.target_frame = 'measured/base_link'
# initialize a node
rospy.init_node('poserbpf_image_listener')
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.pose_pub = rospy.Publisher('poserbpf_image',
ROS_Image,
queue_size=1)
# target detection
self.flag_detected = False
# forward kinematics (base to camera link transformation)
self.Tbr_now = np.eye(4, dtype=np.float32)
self.Tbr_prev = np.eye(4, dtype=np.float32)
self.Trc = np.load('./ros/extrinsics_D415.npy')
self.Tbc_now = np.eye(4, dtype=np.float32)
# dry run poserbpf to initialize the numba modules
self.run_poserbpf_initial()
print(' *** PoseRBPF Ready ... ')
# subscriber for camera information
msg = rospy.wait_for_message('/camera/color/camera_info', CameraInfo)
K = np.array(msg.K).reshape(3, 3)
self.intrinsic_matrix = K
self.pose_rbpf.set_intrinsics(K, 640, 480)
print('Intrinsics matrix : ')
print(self.intrinsic_matrix)
# subscriber for rgbd images and joint states
rgb_sub = message_filters.Subscriber('/camera/color/image_raw',
ROS_Image,
queue_size=1)
depth_sub = message_filters.Subscriber(
'/camera/aligned_depth_to_color/image_raw',
ROS_Image,
queue_size=1)
# subscriber for posecnn label
label_sub = message_filters.Subscriber('/posecnn_label',
ROS_Image,
queue_size=1)
queue_size = 10
slop_seconds = 0.2
ts = message_filters.ApproximateTimeSynchronizer(
[rgb_sub, depth_sub, label_sub], queue_size, slop_seconds)
ts.registerCallback(self.callback)
def visualize_poses(self, rgb):
image_rgb = rgb.astype(np.float32) / 255.0
Tco_list = []
object_initialized = []
for i in range(len(self.pose_rbpf.rbpf_list)):
if self.pose_rbpf.rbpf_ok_list[i]:
Tco = np.eye(4, dtype=np.float32)
Tco[:3, :3] = self.pose_rbpf.rbpf_list[i].rot_bar
Tco[:3, 3] = self.pose_rbpf.rbpf_list[i].trans_bar
Tco_list.append(Tco.copy())
object_initialized.append(
self.pose_rbpf.obj_list.index(
self.pose_rbpf.instance_list[i]))
image_est_render, _, _ = self.pose_rbpf.renderer.render_pose_multiple(
self.intrinsic_matrix, Tco_list, object_initialized)
image_est_disp = image_est_render[0].permute(1, 2, 0).cpu().numpy()
image_disp = 0.4 * image_rgb + 0.6 * image_est_disp
image_disp = np.clip(image_disp, 0, 1.0)
return image_disp
def run_poserbpf_initial(self):
image_rgb = torch.zeros((640, 480, 3), dtype=torch.float32)
image_depth = torch.zeros((640, 480, 1), dtype=torch.float32)
roi = np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)
self.pose_rbpf.set_target_obj(0)
self.pose_rbpf.pose_estimation_single(0,
roi,
image_rgb,
image_depth,
dry_run=True)
# reset lists
self.pose_rbpf.rbpf_list = []
self.pose_rbpf.rbpf_ok_list = []
self.pose_rbpf.instance_list = []
self.pose_rbpf.mope_Tbo_list = []
self.pose_rbpf.mope_pc_b_list = []
def query_posecnn_detection(self, classes):
# detection information of the target object
rois_est = np.zeros((0, 7), dtype=np.float32)
# TODO look for multiple object instances
max_objects = 5
for i in range(len(classes)):
for object_id in range(max_objects):
# check posecnn frame
cls = classes[i]
suffix_frame = '_%02d_roi' % (object_id)
source_frame = 'posecnn/' + cls + suffix_frame
try:
# print('look for posecnn detection ' + source_frame)
trans, rot = self.listener.lookupTransform(
self.target_frame, source_frame, rospy.Time(0))
n = trans[0]
secs = trans[1]
now = rospy.Time.now()
if abs(now.secs - secs) > 1.0:
print 'posecnn pose for %s time out %f %f' % (
source_frame, now.secs, secs)
continue
roi = np.zeros((1, 7), dtype=np.float32)
roi[0, 0] = 0
roi[0, 1] = i
roi[0, 2] = rot[0] * n
roi[0, 3] = rot[1] * n
roi[0, 4] = rot[2] * n
roi[0, 5] = rot[3] * n
roi[0, 6] = trans[2]
rois_est = np.concatenate((rois_est, roi), axis=0)
print('find posecnn detection ' + source_frame)
except:
continue
if rois_est.shape[0] > 0:
# non-maximum suppression within class
index = nms(rois_est, 0.2)
rois_est = rois_est[index, :]
return rois_est
def callback(self, rgb, depth, label):
# decode image
if depth is not None:
if depth.encoding == '32FC1':
depth_cv = self.cv_bridge.imgmsg_to_cv2(depth)
elif depth.encoding == '16UC1':
depth_cv = self.cv_bridge.imgmsg_to_cv2(depth)
else:
rospy.logerr_throttle(
1,
'Unsupported depth type. Expected 16UC1 or 32FC1, got {}'.
format(depth.encoding))
return
else:
depth_cv = None
with lock:
self.input_depth = depth_cv
# rgb image used for posecnn detection
self.input_rgb = self.cv_bridge.imgmsg_to_cv2(rgb, 'rgb8')
self.input_seg = self.cv_bridge.imgmsg_to_cv2(label, 'mono8')
# other information
self.input_stamp = rgb.header.stamp
self.input_frame_id = rgb.header.frame_id
def process_data(self):
# callback data
with lock:
input_stamp = self.input_stamp
input_rgb = self.input_rgb.copy()
input_depth = self.input_depth.copy()
input_seg = self.input_seg.copy()
# subscribe the transformation
try:
source_frame = 'measured/camera_color_optical_frame'
target_frame = 'measured/base_link'
trans, rot = self.listener.lookupTransform(target_frame,
source_frame,
input_stamp)
Tbc = ros_qt_to_rt(rot, trans)
self.Tbc_now = Tbc.copy()
self.Tbr_now = Tbc.dot(np.linalg.inv(self.Trc))
if np.linalg.norm(self.Tbr_prev[:3, 3]) == 0:
self.pose_rbpf.T_c1c0 = np.eye(4, dtype=np.float32)
else:
Tbc0 = np.matmul(self.Tbr_prev, self.Trc)
Tbc1 = np.matmul(self.Tbr_now, self.Trc)
self.pose_rbpf.T_c1c0 = np.matmul(np.linalg.inv(Tbc1), Tbc0)
self.Tbr_prev = self.Tbr_now.copy()
except:
# print('missing forward kinematics info')
return
# call object detection
'''
if len(self.pose_rbpf.obj_list) == 1:
rois = np.array([[0, 0, 447, 167, 447, 167]], dtype=np.float32)
else:
rois = np.array([[0, 0, 332, 150, 332, 150],
[0, 1, 480, 193, 480, 193],
[0, 2, 407, 370, 407, 370]], dtype=np.float32)
'''
rois = self.query_posecnn_detection(self.pose_rbpf.obj_list)
rois = rois[:, :6]
# call pose estimation function
self.pose_estimation(input_rgb, input_depth, input_seg, rois)
for idx_tf in range(len(self.pose_rbpf.rbpf_list)):
if not self.pose_rbpf.rbpf_ok_list[idx_tf]:
continue
Tco = np.eye(4, dtype=np.float32)
Tco[:3, :3] = self.pose_rbpf.rbpf_list[idx_tf].rot_bar
Tco[:3, 3] = self.pose_rbpf.rbpf_list[idx_tf].trans_bar
Tbo = self.Tbc_now.dot(Tco)
# publish tf
t_bo = Tbo[:3, 3]
q_bo = mat2quat(Tbo[:3, :3])
instance_count = 0
for i in range(idx_tf):
if self.pose_rbpf.instance_list[
i] == self.pose_rbpf.instance_list[idx_tf]:
instance_count += 1
self.br.sendTransform(
t_bo, [q_bo[1], q_bo[2], q_bo[3], q_bo[0]], self.input_stamp,
'poserbpf/{}_{}'.format(self.pose_rbpf.instance_list[idx_tf],
instance_count), 'measured/base_link')
# visualization
image_disp = self.visualize_poses(input_rgb) * 255.0
image_disp = image_disp.astype(np.uint8)
image_disp = np.clip(image_disp, 0, 255)
pose_msg = self.cv_bridge.cv2_to_imgmsg(image_disp)
pose_msg.header.stamp = self.input_stamp
pose_msg.header.frame_id = self.input_frame_id
pose_msg.encoding = 'rgb8'
self.pose_pub.publish(pose_msg)
def pose_estimation(self, rgb, depth, label, rois):
image_rgb = torch.from_numpy(rgb).float() / 255.0
image_depth = torch.from_numpy(depth.astype(
np.float32)).float() / 1000.0
im_depth = image_depth.cuda()
image_depth = image_depth.unsqueeze(2)
im_label = torch.from_numpy(label).cuda()
if self.ssv_ckpts:
image_input = image_rgb[:, :, [2, 1, 0]]
else:
image_input = image_rgb
if not self.use_depth:
image_depth = None
# propagate particles for all the initialized objects
for i in range(len(self.pose_rbpf.instance_list)):
if self.pose_rbpf.rbpf_ok_list[i]:
self.pose_rbpf.propagate_with_forward_kinematics(i)
# collect rois from rbpfs
rois_rbpf = np.zeros((0, 6), dtype=np.float32)
index_rbpf = []
for i in range(len(self.pose_rbpf.instance_list)):
if self.pose_rbpf.rbpf_ok_list[i]:
roi = self.pose_rbpf.rbpf_list[i].roi
rois_rbpf = np.concatenate((rois_rbpf, roi), axis=0)
index_rbpf.append(i)
self.pose_rbpf.rbpf_list[i].roi_assign = None
# data association based on bounding box overlap
num_rois = rois.shape[0]
num_rbpfs = rois_rbpf.shape[0]
assigned_rois = np.zeros((num_rois, ), dtype=np.int32)
if num_rbpfs > 0 and num_rois > 0:
# overlaps: (rois x gt_boxes) (batch_id, x1, y1, x2, y2)
overlaps = bbox_overlaps(
np.ascontiguousarray(rois_rbpf[:, (1, 2, 3, 4, 5)],
dtype=np.float),
np.ascontiguousarray(rois[:, (1, 2, 3, 4, 5)], dtype=np.float))
# assign rois to rbpfs
assignment = overlaps.argmax(axis=1)
max_overlaps = overlaps.max(axis=1)
unassigned = []
for i in range(num_rbpfs):
if max_overlaps[i] > 0.2:
self.pose_rbpf.rbpf_list[index_rbpf[i]].roi_assign = rois[
assignment[i]]
assigned_rois[assignment[i]] = 1
else:
unassigned.append(i)
# check if there are un-assigned rois
index = np.where(assigned_rois == 0)[0]
# if there is un-assigned rbpfs
if len(unassigned) > 0 and len(index) > 0:
for i in range(len(unassigned)):
for j in range(len(index)):
if assigned_rois[
index[j]] == 0 and self.pose_rbpf.rbpf_list[
index_rbpf[unassigned[i]]].roi[
0, 1] == rois[index[j], 1]:
self.pose_rbpf.rbpf_list[index_rbpf[
unassigned[i]]].roi_assign = rois[index[j]]
assigned_rois[index[j]] = 1
elif num_rbpfs == 0 and num_rois == 0:
return
# filter tracked objects
for i in range(len(self.pose_rbpf.instance_list)):
if self.pose_rbpf.rbpf_ok_list[i]:
roi = self.pose_rbpf.rbpf_list[i].roi_assign
Tco, max_sim = self.pose_rbpf.pose_estimation_single(
i, roi, image_input, image_depth, visualize=False)
# initialize new object
for i in range(num_rois):
if assigned_rois[i]:
continue
roi = rois[i]
obj_idx = int(roi[1])
target_obj = self.pose_rbpf.obj_list[obj_idx]
add_new_instance = True
for j in range(len(self.pose_rbpf.instance_list)):
if self.pose_rbpf.instance_list[
j] == target_obj and self.pose_rbpf.rbpf_ok_list[
j] == False:
add_new_instance = False
Tco, max_sim = self.pose_rbpf.pose_estimation_single(
j, roi, image_input, image_depth, visualize=False)
if add_new_instance:
self.pose_rbpf.add_object_instance(target_obj)
Tco, max_sim = self.pose_rbpf.pose_estimation_single(
len(self.pose_rbpf.instance_list) - 1,
roi,
image_input,
image_depth,
visualize=False)
# SDF refinement for multiple objects
if self.refine_multiple and self.use_depth:
# backproject depth
fx = self.intrinsic_matrix[0, 0]
fy = self.intrinsic_matrix[1, 1]
px = self.intrinsic_matrix[0, 2]
py = self.intrinsic_matrix[1, 2]
im_pcloud = posecnn_cuda.backproject_forward(
fx, fy, px, py, im_depth)[0]
index_sdf = []
for i in range(len(self.pose_rbpf.instance_list)):
if self.pose_rbpf.rbpf_ok_list[i]:
index_sdf.append(i)
if len(index_sdf) > 0:
self.pose_rbpf.pose_refine_multiple(self.sdf_optimizer,
self.posecnn_classes,
index_sdf,
im_depth,
im_pcloud,
im_label,
steps=50)
# Se define el dispositivo aa utilizar
cap = cv.VideoCapture(1)
#La funcion obtiene la imagen proveniente de la camara
def get_Image():
ret, img = cap.read()
array = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
return img
#Configuracion de nodo y publicador
rospy.init_node('camara')
pub = rospy.Publisher('rgb/image', Image, queue_size=100)
bridge = CvBridge()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
frame = get_Image()
cv.imshow('frame', frame)
cv.waitKey(1)
frame = np.uint8(frame)
#e convierte la imagen a un formato soportado por ROS
image_msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")
pub.publish(image_msg)
rate.sleep()
except TypeError:
continue
class OpenPosePreviewNode(Node):
openpose_wrapper: OpenPoseWrapper
def __init__(self):
super().__init__('openpose_node')
# ros params
is_debug_mode_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_BOOL,
description='If true, run debug mode.')
self.declare_parameter('is_debug_mode', False,
is_debug_mode_descriptor)
self.is_debug_mode: bool = self.get_parameter(
"is_debug_mode").get_parameter_value().bool_value
openpose_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description='The path of openpose project root.')
self.declare_parameter('openpose_root', '/openpose',
openpose_descriptor)
openpose_root: str = self.get_parameter(
"openpose_root").get_parameter_value().string_value
is_image_compressed_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_BOOL,
description='Is input image compressed?')
self.declare_parameter('is_image_compressed', False,
is_image_compressed_descriptor)
is_image_compressed: bool = self.get_parameter(
"is_image_compressed").get_parameter_value().bool_value
image_node_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description='The node name of input image.')
self.declare_parameter('image_node', '/image', image_node_descriptor)
image_node: str = self.get_parameter(
"image_node").get_parameter_value().string_value
self.openpose_wrapper = OpenPoseWrapper(openpose_root)
self.bridge = CvBridge()
# show info
self.get_logger().info('IsDebugMode : ' + str(self.is_debug_mode))
self.get_logger().info('OpenposeRoot : ' + openpose_root)
self.get_logger().info('ImageNode : ' + image_node)
self.get_logger().info('IsImageCompressed : ' +
str(is_image_compressed))
if self.is_debug_mode:
self._publisher = self.create_publisher(Image, '/openpose/preview',
10)
self._publisher_compressed = self.create_publisher(
CompressedImage, '/openpose/preview/compressed', 10)
self._pose_publisher = self.create_publisher(
PoseKeyPointsList, '/openpose/pose_key_points', 10)
if is_image_compressed:
self.subscription = self.create_subscription(
CompressedImage, image_node, self.get_img_compressed_callback,
10)
else:
self.subscription = self.create_subscription(
Image, image_node, self.get_img_callback, 10)
def publish_from_img(self,
img: np.ndarray,
timestamp: Time,
frame_id: str = ""):
result = self.openpose_wrapper.body_from_image(img)
if self.is_debug_mode:
result_image: Image = self.bridge.cv2_to_imgmsg(
result.cvOutputData, "rgb8")
result_image_compressed: CompressedImage = self.bridge.cv2_to_compressed_imgmsg(
result.cvOutputData)
result_image.header.stamp = timestamp
result_image.header.frame_id = frame_id
result_image_compressed.header.stamp = timestamp
result_image_compressed.header.frame_id = frame_id
self._publisher.publish(result_image)
self._publisher_compressed.publish(result_image_compressed)
# Convert to KeyPointsList
pose_key_points_list_obj = PoseKeyPointsList()
if isinstance(result.poseKeypoints, np.ndarray):
pose_key_points_list = []
for result_pose_key_points in result.poseKeypoints:
pose_key_points = []
for result_pose_key_point in result_pose_key_points:
x, y, score = result_pose_key_point
pose_key_points.append(
PoseKeyPoint(x=x.item(),
y=y.item(),
score=score.item()))
pose_key_points_list.append(
PoseKeyPoints(pose_key_points=pose_key_points))
pose_key_points_list_obj = PoseKeyPointsList(
pose_key_points_list=pose_key_points_list)
pose_key_points_list_obj.header.stamp = timestamp
pose_key_points_list_obj.header.frame_id = frame_id
self._pose_publisher.publish(pose_key_points_list_obj)
def get_img_callback(self, image_raw: Image) -> None:
try:
print('[' + str(datetime.datetime.now()) + '] Image received',
end='\r')
image: np.ndarray = self.bridge.imgmsg_to_cv2(image_raw)
self.publish_from_img(image, image_raw.header.stamp,
image_raw.header.frame_id)
except Exception as err:
self.get_logger().error(err)
def get_img_compressed_callback(self, image_raw: CompressedImage) -> None:
try:
print('[' + str(datetime.datetime.now()) +
'] Compressed image received',
end='\r')
image: np.ndarray = self.bridge.compressed_imgmsg_to_cv2(image_raw)
self.publish_from_img(image, image_raw.header.stamp,
image_raw.header.frame_id)
except Exception as err:
self.get_logger().error(err)
class SSDPrediction(object):
def __init__(self):
self.net = build_ssd('test', 300, 2)
self.bridge = CvBridge()
self.image1 = None
self.image2 = None
self.image_to_plot = None
self.rospack = rospkg.RosPack()
pkg_path = self.rospack.get_path('visual_system')
self.model_name = rospy.get_param("~model_name",
"ssd300_BARCODE_21500.pth")
rospy.loginfo("model: %s" % (self.model_name))
# Only considered as target when confidence greater than
self.confidence_thres = rospy.get_param("~confidence_thres", 0.5)
rospy.loginfo("confidence_threshold: %f" % (self.confidence_thres))
self.model_path = pkg_path + "/model/" + self.model_name
self.net.load_weights(self.model_path)
if torch.cuda.is_available():
self.net.cuda()
self.sub_img1 = rospy.Subscriber("/camera1/color/image_raw", Image,
self.img1_cb)
self.sub_img2 = rospy.Subscriber("/camera2/color/image_raw", Image,
self.img2_cb)
self.pub_img_with_bb = rospy.Publisher("~prediction_res",
Image,
queue_size=10)
self.detection_service = rospy.Service("~barcode_detection",
barcode_detect, self.service_cb)
rospy.loginfo("Finish loading weights...")
# Subscriber callback for camera 1, save image in self.image1
def img1_cb(self, msg):
try:
self.image1 = self.bridge.imgmsg_to_cv2(msg, "rgb8") # In RGB
except CvBridgeError as e:
print(e)
# Subscriber callback for camera 2, save image in self.image2
def img2_cb(self, msg):
try:
self.image2 = self.bridge.imgmsg_to_cv2(msg, "rgb8") # In RGB
except CvBridgeError as e:
print(e)
# Service callback
def service_cb(self, req):
res = barcode_detectResponse()
res.result = False
if (req.camera_id): # camera1
if (self.image1 is not None):
self.img_to_net = self.img_preprocessing(self.image1)
self.image_to_plot = self.image1
else:
rospy.logerr("No image receive from camera 1 yet, aborting...")
return res
else: # camera 2
if (self.image2 is not None):
self.img_to_net = self.img_preprocessing(self.image2)
self.image_to_plot = self.image2
else:
rospy.logerr("No image receive from camera 2 yet, aborting...")
return res
var = Variable(self.img_to_net.unsqueeze(0)) # Wrap tensor in Variable
if torch.cuda.is_available():
var = var.cuda()
prediction_res = self.net(var) # Forward pass
detection = prediction_res.data
scale = torch.Tensor(self.image_to_plot.shape[1::-1]).repeat(2)
for i in range(detection.size(1)):
j = 0
while detection[0, i, j, 0].cpu().numpy() >= self.confidence_thres:
res.result = True
score = detection[0, i, j, 0]
display_txt = '%d' % (score * 100.)
pt = (detection[0, i, j, 1:] * scale).cpu().numpy()
pt = [int(p) for p in pt]
# Draw rectangle
cv2.rectangle(self.image_to_plot, (pt[0], pt[1]), (pt[2], pt[3]), \
(0, 0, 255), 3)
# Put text
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(self.image_to_plot, display_txt, (pt[0], pt[1]), font, \
3, (255, 0, 0), cv2.LINE_AA)
j += 1
if (res.result == True):
rospy.loginfo("Detect barcode")
else:
rospy.loginfo("No barcode detected")
# Publish drawed image
try:
self.image_to_plot = cv2.cvtColor(self.image_to_plot,
cv2.COLOR_BGR2RGB)
self.pub_img_with_bb.publish(
self.bridge.cv2_to_imgmsg(self.image_to_plot))
except CvBridgeError as e:
print(e)
return res
# Preprocessing for SSD
def img_preprocessing(self, img):
res = cv2.resize(img, (300, 300)).astype(np.float32)
res -= (104., 117., 123.)
res = res.astype(np.float32)
res = res[:, :, ::-1].copy()
res = torch.from_numpy(res).permute(2, 0, 1)
return res
