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)
def callback(self, data):
bridge=CvBridge()
try:
cv_image=bridge.img_to_cv(data, "bgr8")
except CvBridgeError, e:
#print won't work, need to use rospy.log_ format
rospy.loginfo("Media failed to convert to OpenCV format")
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 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 )
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 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 shapeCallback(req): rosimg = req.im; cvbridge = CvBridge() try: bw_img = cvbridge.imgmsg_to_cv2(rosimg, desired_encoding="passthrough") except CvBridgeError, e: print e
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
class PatchSaver(object):
def __init__(self):
rospy.init_node('patch_saver',log_level=rospy.DEBUG)
self.patch_array_sub = rospy.Subscriber('patch_array', PatchArray,
self.patch_array_callback, None, 1)
self.debug_img_pub = rospy.Publisher('patch_array_debug_img', Image)
self.bridge = CvBridge()
self.path = rospy.get_param('~save_path')
def patch_array_callback(self, PatchArray):
rospy.loginfo("Patch Array Callback")
num = np.random.random()
if ((len(PatchArray.patch_array) > 1) and num > 0.9) or (num > 0.95):
for idx, patch in enumerate(PatchArray.patch_array):
image = np.asarray(self.bridge.imgmsg_to_cv2(patch.image,'bgr8'))
mask = np.asarray(self.bridge.imgmsg_to_cv2(patch.mask,'mono8'))
cv2.imwrite(self.path +
str(int(PatchArray.header.stamp.to_sec() * 1000))
+ "_" + str(idx) + "_image.png", image)
cv2.imwrite(self.path +
str(int(PatchArray.header.stamp.to_sec() * 1000))
+ "_" + str(idx) + "_mask.png", mask)
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
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 save_images(bagfile):
if not os.path.isdir("video_images"):
os.mkdir("video_images")
if not os.path.isdir("depth_images"):
os.mkdir("depth_images")
bag = rosbag.Bag(bagfile)
bridge = CvBridge()
count = 0
o = open('image_log.csv','w')
o.write('im,secs,nsecs,extra\n')
for topic, msg, t in bag.read_messages():
if topic == '/camera/rgb/image_rect_color':
img = bridge.imgmsg_to_cv(msg, "bgr8")
img_name = "video_images/img_%06d.png" %count
o.write('im,%d,%d,\n' % (t.secs,t.nsecs))
count += 1
cv.SaveImage(img_name, img)
elif topic == '/camera/depth_registered/image_rect':
img = bridge.imgmsg_to_cv(msg, "bgr8")
img_name = "depth_images/img_%06d.png" %count
#o.write('de,%d,%d,\n' % (t.secs,t.nsecs))
#count += 1
cv.SaveImage(img_name, img)
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 get_img(self, bag):
bridge = CvBridge()
for topic, msg, t in bag.read_messages(topics=['/cv_camera/image_raw/']):
img = bridge.imgmsg_to_cv2(msg, "bgr8")
rects = self.getCircles(img, 35)
for rect in rects:
yield rect
def on_enter(self,userdata):
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(userdata.Image, desired_encoding="passthrough")
self._filename = os.path.expanduser('~/picture_'+str(rospy.Time.now())+'.jpg')
print 'Saving file to ' , self._filename
cv2.imwrite(self._filename,cv_image)
print 'Picture has been saved to the home folder'
class ImageDisplay:
def __init__(self, device_num=0, color=True):
cv.NamedWindow("Display",1)
self.bridge = CvBridge()
self.color = color
self.device_num = device_num
sub_name = 'camera_' + str(self.device_num)
self.image_sub = rospy.Subscriber(sub_name,Image,self.image_callback)
node_name = 'image_display' + str(self.device_num)
rospy.init_node(node_name, anonymous=True)
def image_callback(self,data):
try:
if self.color:
cv_image = self.bridge.imgmsg_to_cv(data, "rgb8")
else:
cv_image = self.bridge.imgmsg_to_cv(data, "mono8")
except CvBridgeError, e:
print e
cv.ShowImage("Display", cv_image)
cv.WaitKey(3)
class ImageReducer:
# # Convert a ROS Image to the Numpy matrix used by cv2 functions
# def rosimg2cv(self, ros_image):
# # Convert from ROS Image to old OpenCV image
# frame = self.cvbridge.imgmsg_to_cv(ros_image, ros_image.encoding)
# # Convert from old OpenCV image to Numpy matrix
# return np.array(frame, dtype=np.uint8) #TODO: find out actual dtype
def __init__(self, topics, factor):
self.cvbridge = CvBridge()
self.topics = topics
self.factor = factor
for topic in topics:
outTopic = "/debug" + topic
callback = self.reducerCallback(topic, outTopic)
rospy.Subscriber(topic, Image, callback)
# Returns a callback for a particular Image topic which reduces the image
# and publishes it
def reducerCallback(self, imageTopic, outTopic):
publisher = rospy.Publisher(outTopic, Image)
factor = self.factor
def callback(rosImage):
try:
cvimg = self.cvbridge.imgmsg_to_cv(rosImage, desired_encoding="passthrough")
outimg = cv2.cv.CreateMat(int(cvimg.rows * factor), int(cvimg.cols * factor), cvimg.type)
cv2.cv.Resize(cvimg, outimg)
publisher.publish(self.cvbridge.cv_to_imgmsg(outimg, encoding="bgr8")) #TODO: figure out actual encoding
except CvBridgeError, e:
print e
return callback
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):
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)
def buffer_data(bag, input_topic, compressed):
image_buff = []
time_buff = []
start_time = None
depthData = []
bridge = CvBridge()
#bag = rosbag.Bag(bagFile)
#Buffer the images, timestamps from the rosbag
for topic, msg, t in bag.read_messages(topics=[input_topic]):
depthData+=msg.data
if start_time is None:
start_time = t
#Get the image
if not compressed:
try:
cv_image = bridge.imgmsg_to_cv2(msg, "32FC1")
except CvBridgeError as e:
print e
else:
nparr = np.fromstring(msg.data, np.uint8)
cv_image = cv2.imdecode(nparr, cv2.CV_LOAD_IMAGE_COLOR)
# normalize depth image 0 to 255
depthImg = np.array(cv_image, dtype=np.float32)
cv2.normalize(depthImg, depthImg, 0, 255, cv2.NORM_MINMAX)
time_buff.append(t.to_sec() - start_time.to_sec())
image_buff.append(depthImg)
return image_buff, time_buff
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 callback(data): bridge = CvBridge() # cv_image = bridge.imgmsg_to_cv2(data, "bgr8") cv_image = bridge.imgmsg_to_cv2(data, "mono8") cv2.flip(cv_image,1) color_presentation(cv_image)
def __init__(self):
rospy.init_node('GoForward', anonymous=False)
rospy.loginfo("To stop TurtleBot CTRL + C")
rospy.on_shutdown(self.shutdown)
###----------- IMAGE ----------###
# turtlebot
#self.image_sub = rospy.Subscriber("/camera/depth/image",Image, self.callback, queue_size = 1)
# gazebo
#self.image_sub = rospy.Subscriber("/camera/rgb/image_raw",Image, self.callback, queue_size = 1)
#self.image_sub = rospy.Subscriber("/camera/depth/image_raw",Image, self.callback, queue_size = 1)
self.bridge = CvBridge()
self.time = time.time()
rospy.Subscriber('scan', LaserScan, self.laser_callback, queue_size = 1)
#print scan
scan = np.random.uniform(low=-10.0, high=10.0, size=(640,1))
self.scanim = self.scan2im(scan)
self.counter = 0
self.r = rospy.Rate(10);
self.move_cmd = Twist()
#self.move_cmd.linear.x = 0.2
#self.move_cmd.angular.z = 0
self.bridge = CvBridge()
while not rospy.is_shutdown():
#self.cmd_vel.publish(self.move_cmd)
self.r.sleep()
def test_encode_decode_cv2_compressed(self):
import numpy as np
# from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm",
"jp2", "sr", "ras", "tif", "tiff"] # this formats rviz is not support
cvb_en = CvBridge()
cvb_de = CvBridge()
for w in range(100, 800, 100):
for h in range(100, 800, 100):
for f in formats:
for channels in ([], 1, 3):
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)))
compress_rosmsg = cvb_en.cv2_to_compressed_imgmsg(original, f)
newimg = cvb_de.compressed_imgmsg_to_cv2(compress_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 face_detection(image):
bridge = CvBridge()
try:
cv_image = bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print e
new_size = (int(cv_image.shape[1]/2), int(cv_image.shape[0]/2))
#cv_array = np.array(cv_image, dtype=np.uint8)
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
small_img = cv2.resize(gray, new_size, interpolation = cv2.INTER_LINEAR)
small_img = cv2.equalizeHist(small_img)
faces = faceCascade.detectMultiScale(
small_img,
scaleFactor=1.2,
minNeighbors=2,
minSize=(45, 45),
flags=0
)
print len(faces)
# Draw a rectangle around the faces
for (x, y, w, h) in faces:
#print "test"
point1=(int(x*2), int(y*2))
point2=(int((x+w)*2), int((y+h)*2))
cv2.rectangle(cv_image, point1, point2, (0, 255, 0), 3, 8, 0)
cv2.imshow("Image window", cv_image)
cv2.waitKey(1)
def map_segmentation_action_client_(self): ######## this function is called
mat = cv.fromarray(self.map_)
# cv.ShowImage( "map_image", mat )
# cv.WaitKey()
#creates a action client object
client = actionlib.SimpleActionClient( 'segment_map', autopnp_scenario.msg.MapSegmentationAction )
cv_image = CvBridge()
#filling the goal msg format for map segmentation action server
goal = autopnp_scenario.msg.MapSegmentationGoal( input_map = cv_image.cv_to_imgmsg( mat , "mono8"),
map_resolution = self.map_resolution_,
map_origin_x = self.map_origin_x_ ,
map_origin_y = self.map_origin_y_,
return_format_in_pixel = True )
rospy.loginfo("waiting for the map segmentation action server to start.....")
client.wait_for_server()
rospy.loginfo("map segmentation action server started, sending goal.....")
client.send_goal(goal)
finished_before_timeout = client.wait_for_result(rospy.Duration(30.0))
if finished_before_timeout:
state = client.get_state()
if state is 3:
state = 'SUCCEEDED'
rospy.loginfo("action finished: %s " % state)
else:
rospy.loginfo("action finished: %s " % state)
else:
rospy.loginfo("Action did not finish before the time out.")
return client.get_result()
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()))
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 WebcamBridge:
"""
class for moving webcam images from opencv to ros
"""
def __init__(self):
self.image_pub = rospy.Publisher("/tennisballs", Image)
# self.circ_ctr_pub = rospy.Publisher("/circ_ctr", )
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/image_raw", Image, self.callback)
self.size = None
rospy.sleep(1)
def callback(self, data):
"""When the camera publishes a new frame, it sends through the cv
processing, and publishes a new output image
"""
print('In WebcamBridge.callback')
# cv.WaitKey(3)
try:
raw = self.bridge.imgmsg_to_cv(data, 'bgr8')
found = self.find_tennisball(raw)
msg = self.bridge.cv_to_imgmsg(found, 'bgr8')
self.image_pub.publish(msg)
except CvBridgeError, 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)
class AllSensorBot(object):
def __init__(self,
use_lidar=False, use_camera=True, use_imu=False,
use_odom=False, use_joint_states=False):
# velocity publisher
self.vel_pub = rospy.Publisher('cmd_vel', Twist,queue_size=1)
# image publisher 追加した
self.image_pub = rospy.Publisher('processed_image', Image, queue_size=10)
# enemy dircetion publisher
self.enemydir_pub = rospy.Publisher('enemy_direction', String,queue_size=1)
# lidar scan subscriber
if use_lidar:
self.scan = LaserScan()
self.lidar_sub = rospy.Subscriber('scan', LaserScan, self.lidarCallback)
# camera subscribver
# please uncoment out if you use camera
if use_camera:
# for convert image topic to opencv obj
# self.img = None # うまくいかなかった
cols = 640
rows = 480
self.img = np.full((rows, cols, 3), 0, dtype=np.uint8)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber('image_raw', Image, self.imageCallback)
# imu subscriber
if use_imu:
self.imu_sub = rospy.Subscriber('imu', Imu, self.imuCallback)
# odom subscriber
if use_odom:
self.odom_sub = rospy.Subscriber('odom', Odometry, self.odomCallback)
# joint_states subscriber
if use_joint_states:
self.odom_sub = rospy.Subscriber('joint_states', JointState, self.jointstateCallback)
def strategy(self):
'''
calc Twist and publish cmd_vel topic
'''
r = rospy.Rate(1)
while not rospy.is_shutdown():
# update twist
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
# publish twist topic
self.vel_pub.publish(twist)
r.sleep()
# lidar scan topic call back sample
# update lidar scan state
def lidarCallback(self, data):
self.scan = data
rospy.loginfo(self.scan)
# camera image call back sample
# comvert image topic to opencv object and show
def imageCallback(self, data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
""" # dockerの人はコメントアウト
cv2.imshow("Image window", self.img)
cv2.waitKey(1)
"""
# imu call back sample
# update imu state
def imuCallback(self, data):
self.imu = data
rospy.loginfo(self.imu)
# odom call back sample
# update odometry state
def odomCallback(self, data):
self.pose_x = data.pose.pose.position.x
self.pose_y = data.pose.pose.position.y
rospy.loginfo("odom pose_x: {}".format(self.pose_x))
rospy.loginfo("odom pose_y: {}".format(self.pose_y))
# jointstate call back sample
# update joint state
def jointstateCallback(self, data):
self.wheel_rot_r = data.position[0]
self.wheel_rot_l = data.position[1]
rospy.loginfo("joint_state R: {}".format(self.wheel_rot_r))
rospy.loginfo("joint_state L: {}".format(self.wheel_rot_l))
def __init__(self):
self.image_pub = rospy.Publisher("/usb_cam/image_raw",
Image,
queue_size=1)
self.bridge = CvBridge()
import smach import smach_ros import time import numpy as np import cv2 from cv_bridge import CvBridge, CvBridgeError from std_msgs.msg import String, Empty from sensor_msgs.msg import Image, CompressedImage from geometry_msgs.msg import Twist, Vector3, Pose, Vector3Stamped ################################################################################ bridge = CvBridge() # Trackers tracker_types = ['BOOSTING', 'MIL', 'KCF', 'TLD', 'MEDIANFLOW', 'GOTURN'] tracker_type = tracker_types[2] tracker = cv2.TrackerKCF_create() atraso = 1.5 # Atraso aceitavel pela Camera xTela = 300 yTela = 200 Move_X = 0 Move_Y = 0 velRobot = 0.2
def __init__(self, gen_pcl=True):
"""
Constructor
\param gen_pcl (bool) whether generate point cloud, if set to true the node will subscribe to depth image
"""
self.real_sense = rospy.get_param('/semantic_pcl/real_sense')
self.imgCounter = 0
self.imgSemanticCounter = 0
#self.labels_pub = rospy.Publisher("/semantic_pcl/labels", OverlayText, queue_size=1)
self.labels_list = []
#self.text = OverlayText()
# Get image size
self.img_width, self.img_height = rospy.get_param(
'/camera/width'), rospy.get_param('/camera/height')
self.throttle_rate = rospy.get_param('/semantic_pcl/throttle_rate')
self.last_time = rospy.Time.now()
# Set up CNN is use semantics
print('Setting up CNN model...')
# Set device
#self.device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
# Get dataset
self.dataset = rospy.get_param('/semantic_pcl/dataset')
# Setup model
model_name = 'vgg_unet'
model_path = rospy.get_param('/semantic_pcl/model_path')
model_json_path = rospy.get_param('/semantic_pcl/model_json_path')
test_image_path_input = rospy.get_param(
'/model_params/test_image_path_input')
test_image_path_output = rospy.get_param(
'/model_params/test_image_path_output')
model_input_height = rospy.get_param(
'/model_params/model_input_height')
model_input_width = rospy.get_param('/model_params/model_input_width')
model_output_height = rospy.get_param(
'/model_params/model_output_height')
model_output_width = rospy.get_param(
'/model_params/model_output_width')
model_n_classes = rospy.get_param('/model_params/model_n_classes')
model_checkpoints_path = rospy.get_param(
'/model_params/model_checkpoints_path')
self.test_image_path_output_folder = rospy.get_param(
'/model_params/test_image_path_output_folder')
if self.dataset == 'kucarsRisk':
self.n_classes = 7 # Semantic class number
# load the model + weights
# Recreate the exact same model, including its weights and the optimizer
#self.new_model = model_from_json(model_path,custom_objects=None)
#clear_session()
self.new_model = load_model(model_path) #,custom_objects=None)
self.new_model.input_width = model_input_width
self.new_model.input_height = model_input_height
self.new_model.output_width = model_output_width
self.new_model.output_height = model_output_height
self.new_model.n_classes = model_n_classes
# Show the model architecture
self.new_model.summary()
print("Loaded model from disk")
self.new_model.compile(loss='categorical_crossentropy',
optimizer='adadelta',
metrics=['accuracy'])
##### One Image Prediction ####
img = cv2.imread(test_image_path_input)
predict(model=self.new_model,
inp=test_image_path_input,
out_fname=test_image_path_output)
out = cv2.imread(test_image_path_output)
out_rgb = cv2.cvtColor(out, cv2.COLOR_BGR2RGB)
#plt.imshow(out)
#plt.show()
self.cmap = color_map(
N=self.n_classes,
normalized=True) # Color map for semantic classes
# Declare array containers
# Set up ROS
print('Setting up ROS...')
self.bridge = CvBridge(
) # CvBridge to transform ROS Image message to OpenCV image
# Semantic image publisher
self.sem_img_pub = rospy.Publisher(
"/semantic_pcl/semantic_hazard_image", Image, queue_size=1)
# Set up ros image subscriber
# Set buff_size to average msg size to avoid accumulating delay
if gen_pcl:
# Point cloud frame id
frame_id = rospy.get_param('/semantic_pcl/frame_id')
# Camera intrinsic matrix
fx = rospy.get_param('/camera/fx')
fy = rospy.get_param('/camera/fy')
cx = rospy.get_param('/camera/cx')
cy = rospy.get_param('/camera/cy')
intrinsic = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 1]],
dtype=np.float32)
self.pcl_pub = rospy.Publisher("/semantic_pcl/semantic_pcl",
PointCloud2,
queue_size=1)
self.color_sub = message_filters.Subscriber(
rospy.get_param('/semantic_pcl/color_image_topic'),
Image,
queue_size=1,
buff_size=30 * 480 * 640)
self.depth_sub = message_filters.Subscriber(
rospy.get_param('/semantic_pcl/depth_image_topic'),
Image,
queue_size=1,
buff_size=40 * 480 * 640
) # increase buffer size to avoid delay (despite queue_size = 1)
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.color_sub, self.depth_sub], queue_size=1, slop=0.3
) # Take in one color image and one depth image with a limite time gap between message time stamps
self.ts.registerCallback(self.color_depth_callback)
#self.cloud_generator = ColorPclGenerator(intrinsic, self.img_width,self.img_height, frame_id , self.point_type)
self.cloud_generator = ColorPclSemanticGenerator(
intrinsic, self.img_width, self.img_height, frame_id)
else:
print("No Cloud generation")
self.image_sub = rospy.Subscriber(
rospy.get_param('/semantic_pcl/color_image_topic'),
Image,
self.color_callback,
queue_size=1,
buff_size=30 * 480 * 640)
semantic_colored_labels_srv = rospy.Service(
'get_semantic_colored_labels', GetSemanticColoredLabels,
self.get_semantic_colored_labels)
print('Ready.')
class SemanticCloud:
"""
Class for ros node to take in a color image (bgr) and do semantic segmantation on it to produce an image with semantic class colors (chair, desk etc.)
Then produce point cloud based on depth information
CNN: PSPNet (https://arxiv.org/abs/1612.01105) (with resnet50) pretrained on ADE20K, fine tuned on SUNRGBD or not
"""
def __init__(self, gen_pcl=True):
"""
Constructor
\param gen_pcl (bool) whether generate point cloud, if set to true the node will subscribe to depth image
"""
self.real_sense = rospy.get_param('/semantic_pcl/real_sense')
self.imgCounter = 0
self.imgSemanticCounter = 0
#self.labels_pub = rospy.Publisher("/semantic_pcl/labels", OverlayText, queue_size=1)
self.labels_list = []
#self.text = OverlayText()
# Get image size
self.img_width, self.img_height = rospy.get_param(
'/camera/width'), rospy.get_param('/camera/height')
self.throttle_rate = rospy.get_param('/semantic_pcl/throttle_rate')
self.last_time = rospy.Time.now()
# Set up CNN is use semantics
print('Setting up CNN model...')
# Set device
#self.device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
# Get dataset
self.dataset = rospy.get_param('/semantic_pcl/dataset')
# Setup model
model_name = 'vgg_unet'
model_path = rospy.get_param('/semantic_pcl/model_path')
model_json_path = rospy.get_param('/semantic_pcl/model_json_path')
test_image_path_input = rospy.get_param(
'/model_params/test_image_path_input')
test_image_path_output = rospy.get_param(
'/model_params/test_image_path_output')
model_input_height = rospy.get_param(
'/model_params/model_input_height')
model_input_width = rospy.get_param('/model_params/model_input_width')
model_output_height = rospy.get_param(
'/model_params/model_output_height')
model_output_width = rospy.get_param(
'/model_params/model_output_width')
model_n_classes = rospy.get_param('/model_params/model_n_classes')
model_checkpoints_path = rospy.get_param(
'/model_params/model_checkpoints_path')
self.test_image_path_output_folder = rospy.get_param(
'/model_params/test_image_path_output_folder')
if self.dataset == 'kucarsRisk':
self.n_classes = 7 # Semantic class number
# load the model + weights
# Recreate the exact same model, including its weights and the optimizer
#self.new_model = model_from_json(model_path,custom_objects=None)
#clear_session()
self.new_model = load_model(model_path) #,custom_objects=None)
self.new_model.input_width = model_input_width
self.new_model.input_height = model_input_height
self.new_model.output_width = model_output_width
self.new_model.output_height = model_output_height
self.new_model.n_classes = model_n_classes
# Show the model architecture
self.new_model.summary()
print("Loaded model from disk")
self.new_model.compile(loss='categorical_crossentropy',
optimizer='adadelta',
metrics=['accuracy'])
##### One Image Prediction ####
img = cv2.imread(test_image_path_input)
predict(model=self.new_model,
inp=test_image_path_input,
out_fname=test_image_path_output)
out = cv2.imread(test_image_path_output)
out_rgb = cv2.cvtColor(out, cv2.COLOR_BGR2RGB)
#plt.imshow(out)
#plt.show()
self.cmap = color_map(
N=self.n_classes,
normalized=True) # Color map for semantic classes
# Declare array containers
# Set up ROS
print('Setting up ROS...')
self.bridge = CvBridge(
) # CvBridge to transform ROS Image message to OpenCV image
# Semantic image publisher
self.sem_img_pub = rospy.Publisher(
"/semantic_pcl/semantic_hazard_image", Image, queue_size=1)
# Set up ros image subscriber
# Set buff_size to average msg size to avoid accumulating delay
if gen_pcl:
# Point cloud frame id
frame_id = rospy.get_param('/semantic_pcl/frame_id')
# Camera intrinsic matrix
fx = rospy.get_param('/camera/fx')
fy = rospy.get_param('/camera/fy')
cx = rospy.get_param('/camera/cx')
cy = rospy.get_param('/camera/cy')
intrinsic = np.matrix([[fx, 0, cx], [0, fy, cy], [0, 0, 1]],
dtype=np.float32)
self.pcl_pub = rospy.Publisher("/semantic_pcl/semantic_pcl",
PointCloud2,
queue_size=1)
self.color_sub = message_filters.Subscriber(
rospy.get_param('/semantic_pcl/color_image_topic'),
Image,
queue_size=1,
buff_size=30 * 480 * 640)
self.depth_sub = message_filters.Subscriber(
rospy.get_param('/semantic_pcl/depth_image_topic'),
Image,
queue_size=1,
buff_size=40 * 480 * 640
) # increase buffer size to avoid delay (despite queue_size = 1)
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.color_sub, self.depth_sub], queue_size=1, slop=0.3
) # Take in one color image and one depth image with a limite time gap between message time stamps
self.ts.registerCallback(self.color_depth_callback)
#self.cloud_generator = ColorPclGenerator(intrinsic, self.img_width,self.img_height, frame_id , self.point_type)
self.cloud_generator = ColorPclSemanticGenerator(
intrinsic, self.img_width, self.img_height, frame_id)
else:
print("No Cloud generation")
self.image_sub = rospy.Subscriber(
rospy.get_param('/semantic_pcl/color_image_topic'),
Image,
self.color_callback,
queue_size=1,
buff_size=30 * 480 * 640)
semantic_colored_labels_srv = rospy.Service(
'get_semantic_colored_labels', GetSemanticColoredLabels,
self.get_semantic_colored_labels)
print('Ready.')
def get_semantic_colored_labels(self, GetSemanticColoredLabels):
print("Get Semantic Colored Labels Service called")
ret = GetSemanticColoredLabelsResponse()
scls = SemanticColoredLabels()
scl = SemanticColoredLabel()
for i in range(0, self.n_classes):
scl = SemanticColoredLabel()
label = labels[i]
scl.label = label
print(self.cmap[i, 0])
print(self.cmap[i, 1])
print(self.cmap[i, 2])
scl.color_r = self.cmap[i, 0]
scl.color_g = self.cmap[i, 1]
scl.color_b = self.cmap[i, 2]
scls.semantic_colored_labels.append(scl)
print(scls)
ret = scls
return ret
'''
label = labels[0];
scl.label = label
scl.color_r = np.uint8(107.0) #self.cmap[i,0]
scl.color_g = np.uint8(194.0) #self.cmap[i,1]
scl.color_b = np.uint8(216.0) #self.cmap[i,2]
scls.semantic_colored_labels.append(scl)
scl = SemanticColoredLabel()
label = labels[1];
scl.label = label
scl.color_r = np.uint8(122.0) #self.cmap[i,0]
scl.color_g = np.uint8(77.0) #self.cmap[i,1]
scl.color_b = np.uint8(200.0) #self.cmap[i,2]
scls.semantic_colored_labels.append(scl)
scl = SemanticColoredLabel()
label = labels[2];
scl.label = label
scl.color_r = np.uint8(103.0) #self.cmap[i,0]
scl.color_g = np.uint8(130.0) #self.cmap[i,1]
scl.color_b = np.uint8(66.0) #self.cmap[i,2]
scls.semantic_colored_labels.append(scl)
scl = SemanticColoredLabel()
label = labels[3];
scl.label = label
scl.color_r = np.uint8(158.0) #self.cmap[i,0]
scl.color_g = np.uint8(193.0) #self.cmap[i,1]
scl.color_b = np.uint8(72.0) #self.cmap[i,2]
scls.semantic_colored_labels.append(scl)
scl = SemanticColoredLabel()
label = labels[4];
scl.label = label
scl.color_r = np.uint8(129.0) #self.cmap[i,0]
scl.color_g = np.uint8(232.0) #self.cmap[i,1]
scl.color_b = np.uint8(149.0) #self.cmap[i,2]
scls.semantic_colored_labels.append(scl)
ret = scls
scl = SemanticColoredLabel()
label = labels[5];
scl.label = label
scl.color_r = np.uint8(251.0) #self.cmap[i,0]
scl.color_g = np.uint8(232.0) #self.cmap[i,1]
scl.color_b = np.uint8(64.0) #self.cmap[i,2]
scls.semantic_colored_labels.append(scl)
scl = SemanticColoredLabel()
label = labels[6];
scl.label = label
scl.color_r = np.uint8(79.0) #self.cmap[i,0]
scl.color_g = np.uint8(230.0) #self.cmap[i,1]
scl.color_b = np.uint8(207.0) #self.cmap[i,2]
scls.semantic_colored_labels.append(scl)
print(scls)
ret = scls
return ret
'''
def get_label(self, pred_label):
#print(" ============= ")
unique_labels = np.unique(pred_label)
count = 0
self.text.width = 200
self.text.height = 800
self.text.left = 10
self.text.top = 10 + 20 * count
self.text.text_size = 12
self.text.line_width = 2
self.text.font = "DejaVu Sans Mono"
self.text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
self.text.bg_color = ColorRGBA(255, 255, 255, 0.5)
for color_index in unique_labels:
label = ''
label = labels[color_index]
count += 1
if not label in self.labels_list:
self.labels_list.append(label)
self.text.text += """<span style="color: rgb(%d,%d,%d);">%s</span>
""" % (self.cmap[color_index, 0], self.cmap[color_index, 1],
self.cmap[color_index, 2], label.capitalize())
#print("Published Label Name with index:" + str(color_index) + " is:" + label)
self.labels_pub.publish(self.text)
def color_callback(self, color_img_ros):
"""
Callback function for color image, do semantic segmantation and show the decoded image. For test purpose
\param color_img_ros (sensor_msgs.Image) input ros color image message
"""
print('callback')
try:
color_img = self.bridge.imgmsg_to_cv2(
color_img_ros, "bgr8") # Convert ros msg to numpy array
except CvBridgeError as e:
print(e)
# Do semantic segmantation
seg = predict(model=self.new_model, inp=color_img)
seg = seg.astype(np.uint8)
#print (seg.shape)
#print (color_img.shape)
# Do semantic segmantation
'''
class_probs = self.predict(color_img)
confidence, label = class_probs.max(1)
confidence, label = confidence.squeeze(0).numpy(), label.squeeze(0).numpy()
label = resize(label, (self.img_height, self.img_width), order = 0, mode = 'reflect', preserve_range = True) # order = 0, nearest neighbour
label = label.astype(np.int)
# Add semantic class colors
decoded = decode_segmap(label, self.n_classes, self.cmap) # Show input image and decoded image
confidence = resize(confidence, (self.img_height, self.img_width), mode = 'reflect', preserve_range = True)
'''
cv2.imshow('Camera image', color_img)
cv2.imshow('seg', seg)
#cv2.imshow('confidence', confidence)
#cv2.imshow('Semantic segmantation', decoded)
cv2.waitKey(3)
def color_depth_callback(self, color_img_ros, depth_img_ros):
"""
Callback function to produce point cloud registered with semantic class color based on input color image and depth image
\param color_img_ros (sensor_msgs.Image) the input color image (bgr8)
\param depth_img_ros (sensor_msgs.Image) the input depth image (registered to the color image frame) (float32) values are in meters
"""
tic = rospy.Time.now()
diff = tic - self.last_time
if diff.to_sec() < self.throttle_rate:
return
self.last_time = rospy.Time.now()
# Convert ros Image message to numpy array
try:
color_img = self.bridge.imgmsg_to_cv2(color_img_ros, "bgr8")
depth_img = self.bridge.imgmsg_to_cv2(depth_img_ros, "32FC1")
except CvBridgeError as e:
print(e)
# Resize depth
if depth_img.shape[0] is not self.img_height or depth_img.shape[
1] is not self.img_width:
depth_img = resize(
depth_img, (self.img_height, self.img_width),
order=0,
mode='reflect',
preserve_range=True) # order = 0, nearest neighbour
depth_img = depth_img.astype(np.float32)
# realsense camera gives depth measurements in mm
if self.real_sense:
depth_img = depth_img / 1000.0
################## Save all the Images for debugging #####################
'''
filename = self.test_image_path_output_folder + "image" + str(self.imgCounter) + ".png"
print (filename)
cv2.imwrite(filename, color_img)
self.imgCounter = self.imgCounter + 1 ;
'''
##########################################################################
semantic_color = predict(model=self.new_model, inp=color_img)
################## Save all the Images for debugging #####################
'''
filename = self.test_image_path_output_folder + "semanticimage" + str(self.imgSemanticCounter) + ".png"
print (filename)
cv2.imwrite(filename, semantic_color)
self.imgSemanticCounter = self.imgSemanticCounter + 1 ;
'''
##########################################################################
#label_d = semantic_color.max(1)
#label_d = resize(label_d, (self.img_height, self.img_width), order = 0, mode = 'reflect', preserve_range = True) # order = 0, nearest neighbour
#label_d = label_d.astype(np.uint8)
semantic_color = semantic_color.astype(np.uint8)
#decoded = decode_segmap(label_d, self.n_classes, self.cmap) # Show input image and decoded image
# Publish semantic image
if self.sem_img_pub.get_num_connections() > 0:
try:
semantic_color_msg = self.bridge.cv2_to_imgmsg(semantic_color,
encoding="bgr8")
#rgb_img = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
self.sem_img_pub.publish(semantic_color_msg)
except CvBridgeError as e:
print(e)
cloud_ros = self.cloud_generator.generate_cloud_semantic(
color_img, depth_img, semantic_color, color_img_ros.header.stamp)
#Publish point cloud
#self.get_label(pred_labels)
self.pcl_pub.publish(cloud_ros)
'''
class Camera(object):
def __init__(self):
self.node_name = rospy.get_name()
# get ROS parameters
self.resource = rospy.get_param('~rtsp_resource')
self.new_resource = ""
self.image_topic = rospy.get_param('~image_topic')
# open RTSP stream
self.cap = cv2.VideoCapture(self.resource)
if not self.cap.isOpened():
rospy.logerr("Error opening resource `%s`. Please check." %
self.resource)
exit(0)
rospy.loginfo("Resource successfully opened")
# create publishers
self.image_pub = rospy.Publisher(self.image_topic,
CompressedImage,
queue_size=1)
self.camera = rospy.Subscriber("~camera_switch",
String,
self.camera_switch_cb,
queue_size=1)
# initialize ROS_CV_Bridge
self.ros_cv_bridge = CvBridge()
self.change = False
self.is_shutdown = False
def startCapturing(self):
# initialize variables
print "Correctly opened resource, starting to publish feed."
rval, cv_image = self.cap.read()
# process frames
while rval:
if self.change:
rospy.loginfo("Change now")
self.cap.release()
self.resource = self.new_resource
self.cap = cv2.VideoCapture(self.resource)
self.change = False
rospy.loginfo("Changed")
# get new frame
rval, cv_image = self.cap.read()
# handle Ctrl-C
key = cv2.waitKey(20)
if rospy.is_shutdown(
) or self.is_shutdown or key == 27 or key == 1048603:
break
# convert CV image to ROS message
image_msg = self.ros_cv_bridge.cv2_to_compressed_imgmsg(cv_image)
image_msg.header.stamp = rospy.Time.now()
self.image_pub.publish(image_msg)
def camera_switch_cb(self, data):
rospy.loginfo(data.data)
self.new_resource = data.data
self.change = True
def onShutdown(self):
rospy.loginfo("[%s] Closing ip camera." % (self.node_name))
self.is_shutdown = True
rospy.loginfo("[%s] Shutdown." % (self.node_name))
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("/camera/rgb/image_mono",Image)
#cv2.namedWindow("Image window", 1)
#cv2.namedWindow("Depth window" , 1)
self.c = 0
self.x = 0
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw",Image,self.callback)
self.depth_sub = rospy.Subscriber("/camera/depth_registered/image_raw", Image, self.depth_callback)
def callback(self,data):
global reddetect
global targetpos
self.c = self.c+1
if self.c==5 :
try:
frame = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError, e:
print e
(rows,cols,channels) = frame.shape
frame = np.array(frame, dtype=np.uint8)
#blue mask
# Convert BGR to HSV
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# define range of blue color in HSV
lower_blue = np.array([150,168,20])
upper_blue = np.array([179,255,255])
# Threshold the HSV image to get only blue colors
mask = cv2.inRange(hsv, lower_blue, upper_blue)
# Bitwise-AND mask and original image
#res = cv2.bitwise_and(frame,frame, mask= mask)
res = cv2.morphologyEx(mask,cv2.MORPH_OPEN,(12,12))
count = np.sum(res)/255
self.x = self.findCOM(res)
if (self.x>5 and self.x<595 and count>900):
reddetect = 1
targetpos = self.x
for i in range(self.x-5,self.x+5):
for j in range(240-5,240+5):
res[j][i] = 150
else:
reddetect = 0
#rospy.loginfo("avg: " + str(self.x) + "total: " + str(count))
# Bitwise-AND mask and original image
#res = cv2.bitwise_and(frame,frame, mask= mask)
#res = cv2.morphologyEx(res,cv2.MORPH_CLOSE,(5,5))
#cv2.imshow('frame',frame)
#cv2.imshow('mask',mask)
cv2.imshow('res',res)
#k = cv2.waitKey(5) & 0xFF
#cv2.imshow("Image window", frame)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
except CvBridgeError, e:
print e
self.c=0
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):
self.server('throttle', data={'throttle': str(data.pedal_cmd)})
def callback_brake(self, data):
rospy.loginfo('[Server.send] brake = %.2f', data.pedal_cmd)
self.server('brake', data={'brake': str(data.pedal_cmd)})
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2",Image, queue_size = 10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/sensors/camera/infra1/image_rect_raw",Image,self.callback)
class KinectHelper(object):
def __init__(self, color_cam, range_cam, timestep):
self.kinect_range = range_cam
self.kinect_color = color_cam
# Enable cameras
self.kinect_range.enable(2 * timestep)
self.kinect_color.enable(2 * timestep)
# Init messages
self.init_messages()
# ROS publishers
self.camera_rgb_pub = rospy.Publisher('rgb/image_raw',
Image,
queue_size=1)
self.camera_info_rgb_pub = rospy.Publisher('rgb/camera_info',
CameraInfo,
queue_size=1)
self.camera_depth_pub = rospy.Publisher('depth/image_raw',
Image,
queue_size=1)
self.camera_info_depth_pub = rospy.Publisher('depth/camera_info',
CameraInfo,
queue_size=1)
# OpenCV bridge
self.bridge = CvBridge()
def init_image(self, camera):
msg = CameraInfo()
msg.width = camera.getWidth()
msg.height = camera.getHeight()
f = camera.getWidth() / (2 * math.tan(camera.getFov() / 2))
msg.K[0] = f
msg.K[4] = f
msg.K[2] = camera.getWidth() / 2
msg.K[5] = camera.getHeight() / 2
msg.K[8] = 1
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
f, 0.0,
camera.getWidth() / 2, 0.0, 0.0, f,
camera.getHeight() / 2, 0.0, 0.0, 0.0, 1.0, 0.0
]
return msg
def init_messages(self):
self.info_rgb_msg = self.init_image(self.kinect_color)
self.info_depth_msg = self.init_image(self.kinect_range)
def broadcast_depth_image(self):
time_now = rospy.Time.now()
raw = self.kinect_range.getRangeImageArray()
if raw is None: return
img = np.transpose(np.array(raw, dtype=np.float32))
image = self.bridge.cv2_to_imgmsg(img, "passthrough")
image.header.stamp = time_now
image.header.frame_id = "camera_depth_optical_frame"
self.camera_depth_pub.publish(image)
# Camera info
self.info_depth_msg.header.stamp = time_now
self.info_depth_msg.header.frame_id = "camera_depth_optical_frame"
self.camera_info_depth_pub.publish(self.info_depth_msg)
def broadcast_color_image(self):
time_now = rospy.Time.now()
raw = self.kinect_color.getImageArray()
if raw is None: return
img = np.transpose(np.array(raw, dtype=np.uint8), (1, 0, 2))
image = self.bridge.cv2_to_imgmsg(img, "rgb8")
image.header.stamp = time_now
image.header.frame_id = "camera_rgb_optical_frame"
self.camera_rgb_pub.publish(image)
# Camera info
self.info_rgb_msg.header.stamp = time_now
self.info_rgb_msg.header.frame_id = "camera_rgb_optical_frame"
self.camera_info_rgb_pub.publish(self.info_rgb_msg)
import numpy as np
import cv2
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
CAMERA_WIDTH = 1280
CAMERA_HEIGHT = 720
BRIDGE = CvBridge()
def set_cam_params(cam_num):
cap = cv2.VideoCapture(cam_num)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT)
cap.set(cv2.CAP_PROP_FPS, 5)
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
return cap
left_image_pub = rospy.Publisher("/sj_camera/left_image_raw",
Image,
queue_size=1)
right_image_pub = rospy.Publisher("/sj_camera/right_image_raw",
Image,
queue_size=1)
down_image_pub = rospy.Publisher("/sj_camera/down_image_raw",
Image,
queue_size=1)
def __init__(self):
super(ImageContainer, self).__init__()
self.Image = None
self._bridge = CvBridge()
def __init__(self):
##USER PARAMETERS
self.angle_setpoint = 90
self.angle = 0
self.fin_time = 0
self.integral = 0
self.prev_error = 0
self.logs = [0, 0]
##Initiate variables
self.leftLine = 0
self.midLine = 0
self.rightLine = 0
self.distance = 0
self.leftSpeed = 0
self.rightSpeed = 0
self.pan = 0
self.tilt = 0
self.bridge = CvBridge()
#Setup Publishers
self.motorPub = rospy.Publisher('motors', motors, queue_size=10)
self.servoPub = rospy.Publisher('servos', servos, queue_size=10)
self.blobpub = rospy.Publisher('imageProc', Image, queue_size=10)
#Create Subscriber callbacks
def lineCallback(data):
self.leftLine = data.leftLine
self.midLine = data.midLine
self.rightLine = data.rightLine
def distanceCallback(data):
self.distance = data.distance
def imageProcessing(data):
# st_time = time.time()
try:
frame = self.bridge.imgmsg_to_cv2(
data, desired_encoding="passthrough")
except CvBridgeError as e:
print(e)
##Place image processing code here!
frame, self.logs = imageProcessQueue(frame)
print(self.logs)
self.angle = self.logs[1]
self.blobpub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
# print("Time diff: %f\n Steering angle: %f\n" % (((st_time - self.fin_time)*1000), self.angle))
# print(logs)
# self.fin_time = st_time
def speed_callback(data):
## timing code
st_time = time.time()
# print("Time diff: %f\n" % ((st_time - self.fin_time)*1000))
turn_speed_here = 0.3
turn_speed = self.pid()
self.leftSpeed = (0.2 + turn_speed) * 0.9
self.rightSpeed = (0.2 - turn_speed) * 0.9
self.fin_time = st_time
if self.logs[0] == 0 or self.distance <= 0.4:
self.leftSpeed = 0
self.rightSpeed = 0
self.integral = 0
self.prev_error = 0
# if only one lane is detected
# if self.logs[0] == 1:
# if self.logs[2][0] == 1:
# self.leftSpeed = turn_speed_here
# self.rightSpeed = -turn_speed_here
# self.integral = 0
# self.prev_error = 0
# if self.logs[2][1] == 1:
# self.leftSpeed = -turn_speed_here
# self.rightSpeed = turn_speed_here
# self.integral = 0
# self.prev_error = 0
##Leave these lines at the end
self.publishMotors()
#Subscribe to topics
rospy.Subscriber('raspicam_node/image', Image, imageProcessing)
rospy.Subscriber('imageProc', Image, speed_callback)
rospy.Subscriber('distance', distance, distanceCallback)
# initialize
rospy.init_node('core', anonymous=True)
self.rate = rospy.Rate(10)
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2",Image, queue_size = 10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/sensors/camera/infra1/image_rect_raw",Image,self.callback)
def callback(self,data):
#convert the ros image to a cv image
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# the intrinsic_parameters from the CameraInfo topic
intrinsic_parameters = np.float32([[383.7944641113281, 0.0, 322.3056945800781],[0.0, 383.7944641113281, 241.67051696777344],[0.0, 0.0, 1.0]])
# the distortion coefficients from the CameraInfo topic
dist_coeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
#convert the image to a grayscale image
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# hide bigger white places on the field by occulude them with black rectangles
rows, cols = cv_image.shape
cv_image = cv2.rectangle(cv_image, (0,0), (cols-1,106), (0,0,0), -1)
cv_image = cv2.rectangle(cv_image, (0,rows-130), (cols-1,rows-1), (0,0,0), -1)
cv_image = cv2.rectangle(cv_image, ((cols/2)-120,rows-230), ((cols/2) + 150,rows-1), (0,0,0), -1)
#convert the image to binary
binary_img = cv2.threshold(cv_image, 240, 255, cv2.THRESH_BINARY)[1]
# find contours in the binary image
im2, contours, hierarchy = cv2.findContours(binary_img,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
#index for the points to increment in the loop
counter = 0
#dict to store the 6 points
points_on_ground = np.float32([[0,0],[0,0],[0,0],[0,0],[0,0],[0,0]])
points_on_car = np.array([[0.5,0.2,0],[0.5,-0.2,0],[0.8,0.2,0],[0.8,-0.2,0],[1.1,0.2,0],[1.1,-0.2,0]])
for c in contours:
# calculate moments for each contour
M = cv2.moments(c)
# calculate x,y coordinate of center
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.circle(binary_img, (cX, cY), 1, (0, 0, 0), -1)
points_on_ground[counter] = [cX, cY]
counter += 1
#compute extrinsic parameters with solvePnP
#ret, rvec, tvec = cv2.solvePnP(object_points, marker_points, camera_matrix, dist_coeff)
ret, rvec, tvec = cv2.solvePnP(points_on_car, points_on_ground, intrinsic_parameters, dist_coeffs)
rotation_matrix = np.arange(3,3)
rotation_matrix, wasanderes = cv2.Rodrigues(rvec)
transformation_matrix = [[rotation_matrix[0][0], rotation_matrix[0][1], rotation_matrix[0][2],tvec[0][0]],
[rotation_matrix[1][0], rotation_matrix[1][1], rotation_matrix[1][2],tvec[1][0]],
[rotation_matrix[2][0], rotation_matrix[2][1], rotation_matrix[2][2],tvec[2][0]],
[0.0, 0.0, 0.0, 1.0]]
#print("transformation matrix: ",transformation_matrix)
#print("\n\n\n\n\n\n")
inverse_transformation = np.linalg.inv(transformation_matrix)
print("\n******************************** Output solvePnP **********************************\n")
print("rvec: \n",rvec)
#print("inverse transformation: ",inverse_transformation)
print("\n\n")
print("tvec: \n",tvec)
#print("inverse transformation: ",inverse_transformation)
print("\n\n")
print("\n******************************* Transofmation Matrix an Inverse ***********************************\n")
print("Transformation Matrix: \n",transformation_matrix)
#print("inverse transformation: ",inverse_transformation)
print("\n\n")
print("Inverse Transformation Matrix: \n",inverse_transformation)
#print("inverse transformation: ",inverse_transformation)
print("\n\n")
print("\n************************ Check Inverse Matrix with a Point P ***************************************\n")
print("Point P for check inverse: ",[points_on_ground[0][0],points_on_ground[0][1],0,1])
pointtransform = np.dot(transformation_matrix,[points_on_ground[0][0],points_on_ground[0][1],1,1])
print("\n\n")
print("Dot Product (P * Transformation Matrix) = D :",pointtransform)
print("\n\n")
inversetransformp = np.dot(inverse_transformation, pointtransform)
print("Dot Product (D * Inverse Transformation Matrix) = P ",inversetransformp)
'''
transformation_matrix:
[0.016633132510232462, -0.99773441110706473, -0.065187297809735922, 0.03167831,
-0.52853957600128143, 0.046568970831419976, -0.84763037201134284, 0.25024891,
0.84874569542548017, 0.048552815042389619, -0.52656753474571261, 0.09192414,
0.0, 0.0, 0.0, 1.0 ]
'''
cv2.imshow("Image window", binary_img)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
class RandomBot():
def __init__(self, bot_name="NoName"):
# bot name
self.name = bot_name
#wall distance
self.near_wall_range = 0.2 # [m]
# lidar scan
self.scan = []
# target_id
self.id_list = np.zeros(1)
# position
self.pose_x = 0
self.pose_y = 0
self.th = 0
# speed [m/s]
self.speed = 0.1
self.enemy_dist = []
self.enemy_list = []
self.bridge = CvBridge()
# publisher
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
# subscriber
self.lidar_sub = rospy.Subscriber('scan', LaserScan,
self.LidarCallback)
self.id_sub = rospy.Subscriber('target_id', MarkerArray,
self.IdCallback)
self.pose_sub = rospy.Subscriber('odom', Odometry, self.PoseCallback)
self.image_sub = rospy.Subscriber('image_raw', Image,
self.ImageCallback)
#actionlib
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.twist = Twist()
self.twist.linear.x = 0
self.twist.linear.y = 0.
self.twist.linear.z = 0.
self.twist.angular.x = 0.
self.twist.angular.y = 0.
self.twist.angular.z = 0.
#self.enemy_detector = EnemyDetector()
self.image_state = 0
def LidarCallback(self, data):
scan = data.ranges
self.scan = scan
#print(len(self.scan),self.scan[0],self.scan[280])
self.near_wall = self.NearWall(scan)
def NearWall(self, scan):
if not len(scan) == 360:
return False
forword_scan = scan[:20] + scan[-20:]
back_scan = scan[160:200]
# drop too small value ex) 0.0
forword_scan = [x for x in forword_scan if x > 0.1]
back_scan = [x for x in back_scan if x > 0.1]
if min(forword_scan) < self.near_wall_range:
return 1
elif min(back_scan) < self.near_wall_range:
return 2
return False
def IdCallback(self, data):
id = data.markers[0].id
if self.id_list[-1] != id:
self.id_list = np.append(self.id_list, id)
else:
pass
def PoseCallback(self, data):
'''
pose topic from amcl localizer
update robot twist
'''
pose_x = data.pose.pose.position.x
self.pose_x = pose_x
pose_y = data.pose.pose.position.y
self.pose_y = pose_y
quaternion = data.pose.pose.orientation
rpy = tf.transformations.euler_from_quaternion(
(quaternion.x, quaternion.y, quaternion.z, quaternion.w))
th = rpy[2]
self.th = th
#print(self.pose_x,self.pose_y,self.th)
def ImageCallback(self, image):
try:
frame = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError, e:
print e
input_image = np.array(frame, dtype=np.uint8)
rects = self.process_image(input_image)
#print(rects)
if rects != []:
if rects[0][0] + rects[0][2] / 2 < 220:
self.image_state = 3
elif rects[0][0] + rects[0][2] / 2 > 260:
self.image_state = 4
else:
self.image_state = 5
else:
self.image_state = 6
cv2.waitKey(1)
class DroneStabilization:
image_sub = None
def __init__(self):
rospy.Subscriber("/state_machine/state", String, self.state_callback)
self.pub_condition = rospy.Publisher(
"/state_machine/window/find_condition", String, queue_size=10)
rospy.init_node('window', anonymous=True)
self.yolo = YOLO()
self.count_stable = 0
def setup_window(self):
self.image_pub = rospy.Publisher("image_topic_2", Image, queue_size=10)
self.vel_pub = rospy.Publisher('controle/velocity',
Vector3D,
queue_size=10)
self.bridge = CvBridge()
# self.stab = camStab()
self.detect = detect_window()
self.vec_vel = Vector3D()
self.vec_vel.x = 0
self.vec_vel.y = 0
self.vec_vel.z = 0
self.pixel_movement = [0, 0]
self.frame = 0
self.count_callbacks = 0
self.opt_flow = camStab()
self.image_sub = rospy.Subscriber("/bebop/image_raw",
Image,
self.callback,
queue_size=None)
rospy.loginfo("setup ok")
# self.pub_condition.publish("ok")
def state_callback(self, data):
if (data.data == "find_window"):
rospy.loginfo(" WINDOW !!!")
self.setup_window()
# condition!!
elif (self.image_sub != None):
rospy.loginfo(" end! ")
cv2.destroyAllWindows()
self.image_sub.unregister()
self.image_sub = None
def callback(self, data):
# if self.count_callbacks < 10:
# self.count_callbacks += 1
# return
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
if not (cols > 60 and rows > 60): # returns if data have unvalid shape
return
# self.detect.update(cv_image)
if self.frame == 0:
pil_image = PIL.Image.fromarray(cv_image)
r_image, out_boxes, out_score = self.yolo.detect_image(pil_image)
# window detected with min precision of (defined on yolo file)
if out_score.shape[0] > 0:
rospy.loginfo("detect")
m_box = out_boxes.mean(0)
self.mean_box_cord = m_box[:]
self.image_h = r_image.shape[0]
self.image_w = r_image.shape[1]
self.image_center = np.array(
[self.image_w / 2, self.image_h / 2])
box_margin = 30
max_score_index = np.argmax(out_score)
box_lt = (max(
int(out_boxes[max_score_index][1]) - box_margin,
0), max(
int(out_boxes[max_score_index][0]) - box_margin, 0))
box_rb = (min(
int(out_boxes[max_score_index][3]) + box_margin,
self.image_w),
min(
int(out_boxes[max_score_index][2]) + box_margin,
self.image_h))
img = cv2.rectangle(r_image, box_lt, box_rb, (0, 220, 200), 2)
self.opt_flow.resetFeatures(
cv_image,
interval=[box_lt[1], box_rb[1], box_lt[0], box_rb[0]],
get_corners_only=True)
# print(init_feature + np.array(box_lt))
# self.opt_flow.set_p0(init_feature + np.array(box_lt))
# self.opt_flow.set_initial_img(cv_image)
self.frame += 1
cv2.imshow("window", img)
else:
rospy.loginfo("no windows on view, trying again")
# self.vel_pub.publish(self.vec_vel)
cv2.imshow("window", r_image)
else:
self.opt_flow.update(cv_image)
print(self.opt_flow.get_square_shape())
self.adjust_position(self.opt_flow.get_square_center(),
self.opt_flow.get_square_shape())
k = cv2.waitKey(30) & 0xff
if k == 27:
exit()
elif k == 'r' or k == ord('r'):
print("r pressed ")
self.frame = 0
elif k == ord('p') or k == 'p':
print("through_window ")
self.pub_condition.publish("ok")
try:
# self.vel_pub.publish(self.vec_vel)
# print(self.vec_vel.x)
# self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
pass
except CvBridgeError as e:
print(e)
self.count_callbacks = 0
def adjust_position(self,
window_center_pos,
window_shape,
ideal_window_image_rate=0.60):
delta = self.image_center - window_center_pos
# rospy.loginfo(delta)
self.vec_vel.x = 0
self.vec_vel.y = 0
self.vec_vel.z = 0
if abs(delta[0]) > 5 or abs(delta[1]) > 5:
self.vec_vel.y = np.sign(delta[0]) * min(
np.abs(delta[0]) / 20, 0.2)
self.vec_vel.z = np.sign(delta[1]) * min(
np.abs(delta[1]) / 20, 0.2)
self.count_stable = 0
rate = window_shape[1] / self.image_h
delta_rate = ideal_window_image_rate - rate
# (x,y) goes back
if abs(delta_rate) > 0.02 and abs(delta[0]) < 20 and abs(
delta[1]) < 20:
self.vec_vel.x = delta_rate * 2
self.count_stable = 0
self.vel_pub.publish(self.vec_vel)
self.count_stable += 1
if self.count_stable > 3:
rospy.loginfo("GOOD!!")
self.pub_condition.publish("ok")
def __init__(self):
# type: () -> None
"""
Initiate VisualCompassHandler
return: None
"""
# Init ROS package
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('bitbots_visual_compass')
rospy.init_node('bitbots_visual_compass_setup')
rospy.loginfo('Initializing visual compass setup')
self.bridge = CvBridge()
self.config = {}
self.image_msg = None
self.compass = None
self.hostname = socket.gethostname()
# TODO: docs
self.base_frame = 'base_footprint'
self.camera_frame = 'camera_optical_frame'
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(50))
self.listener = tf2.TransformListener(self.tf_buffer)
self.pub_head_mode = rospy.Publisher('head_mode',
HeadMode,
queue_size=1)
# Register VisualCompassConfig server for dynamic reconfigure and set callback
Server(VisualCompassConfig, self.dynamic_reconfigure_callback)
rospy.logwarn("------------------------------------------------")
rospy.logwarn("|| WARNING ||")
rospy.logwarn("||Please remove the LAN cable from the Robot, ||")
rospy.logwarn("||after pressing 'YES' you have 10 Seconds ||")
rospy.logwarn("||until the head moves OVER the LAN port!!! ||")
rospy.logwarn("------------------------------------------------\n\n")
try:
input = raw_input
except NameError:
pass
accept = input("Do you REALLY want to start? (YES/n)")
if accept == "YES":
rospy.logwarn("REMOVE THE LAN CABLE NOW!!!!!")
rospy.sleep(10)
head_mode = HeadMode()
head_mode.headMode = 10
self.pub_head_mode.publish(head_mode)
rospy.loginfo("Head mode has been set!")
rospy.spin()
else:
rospy.signal_shutdown(
"You aborted the process! Shuting down correctly.")
class autonomy(object):
def __init__(self):
##USER PARAMETERS
self.angle_setpoint = 90
self.angle = 0
self.fin_time = 0
self.integral = 0
self.prev_error = 0
self.logs = [0, 0]
##Initiate variables
self.leftLine = 0
self.midLine = 0
self.rightLine = 0
self.distance = 0
self.leftSpeed = 0
self.rightSpeed = 0
self.pan = 0
self.tilt = 0
self.bridge = CvBridge()
#Setup Publishers
self.motorPub = rospy.Publisher('motors', motors, queue_size=10)
self.servoPub = rospy.Publisher('servos', servos, queue_size=10)
self.blobpub = rospy.Publisher('imageProc', Image, queue_size=10)
#Create Subscriber callbacks
def lineCallback(data):
self.leftLine = data.leftLine
self.midLine = data.midLine
self.rightLine = data.rightLine
def distanceCallback(data):
self.distance = data.distance
def imageProcessing(data):
# st_time = time.time()
try:
frame = self.bridge.imgmsg_to_cv2(
data, desired_encoding="passthrough")
except CvBridgeError as e:
print(e)
##Place image processing code here!
frame, self.logs = imageProcessQueue(frame)
print(self.logs)
self.angle = self.logs[1]
self.blobpub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
# print("Time diff: %f\n Steering angle: %f\n" % (((st_time - self.fin_time)*1000), self.angle))
# print(logs)
# self.fin_time = st_time
def speed_callback(data):
## timing code
st_time = time.time()
# print("Time diff: %f\n" % ((st_time - self.fin_time)*1000))
turn_speed_here = 0.3
turn_speed = self.pid()
self.leftSpeed = (0.2 + turn_speed) * 0.9
self.rightSpeed = (0.2 - turn_speed) * 0.9
self.fin_time = st_time
if self.logs[0] == 0 or self.distance <= 0.4:
self.leftSpeed = 0
self.rightSpeed = 0
self.integral = 0
self.prev_error = 0
# if only one lane is detected
# if self.logs[0] == 1:
# if self.logs[2][0] == 1:
# self.leftSpeed = turn_speed_here
# self.rightSpeed = -turn_speed_here
# self.integral = 0
# self.prev_error = 0
# if self.logs[2][1] == 1:
# self.leftSpeed = -turn_speed_here
# self.rightSpeed = turn_speed_here
# self.integral = 0
# self.prev_error = 0
##Leave these lines at the end
self.publishMotors()
#Subscribe to topics
rospy.Subscriber('raspicam_node/image', Image, imageProcessing)
rospy.Subscriber('imageProc', Image, speed_callback)
rospy.Subscriber('distance', distance, distanceCallback)
# initialize
rospy.init_node('core', anonymous=True)
self.rate = rospy.Rate(10)
def publishMotors(self):
motorMsg = motors()
motorMsg.leftSpeed = self.leftSpeed
motorMsg.rightSpeed = self.rightSpeed
# rospy.loginfo(motorMsg)
self.motorPub.publish(motorMsg)
def publishServo(self):
servoMsg = servos()
servoMsg.pan = self.pan
servoMsg.tilt = self.tilt
# rospy.loginfo(servoMsg)
self.servoPub.publish(servoMsg)
def runner(self):
while not rospy.is_shutdown():
## timing code
# st_time = time.time()
# # print("Time diff: %f\n" % ((st_time - self.fin_time)*1000))
# turn_speed = self.pid()
# self.leftSpeed = 0.18 + turn_speed
# self.rightSpeed = 0.18 - turn_speed
# self.fin_time = st_time
# if self.logs[0] == 0:
# self.leftSpeed = 0
# self.rightSpeed = 0
# self.integral = 0
# ##Leave these lines at the end
# self.publishMotors()
# self.publishServo()
self.rate.sleep()
def pid(self):
kp = 0.9
ki = 0
kd = 0.5
# if np.sum(self.logs[2]) == 1:
# kp=0.9
# ki= 0.001
# kd=0
# integral clamping
if self.integral > 2:
self.integral = 2
elif self.integral < -2:
self.integral = -2
# pid control
error = (self.angle - self.angle_setpoint) / (self.angle_setpoint)
self.integral += (error / 10)
derivative = (error - self.prev_error) * 10
speed = kp * error + ki * self.integral + kd * derivative
self.prev_error = error
speed /= 3
# speed clamping
if speed > 0.2:
speed = 0.2
elif speed < -0.2:
speed = -0.2
# print("Speed: ", speed/10000)
if self.logs[0] == 1:
if self.logs[2][0] == 1:
speed = abs(speed) / 1.1
if self.logs[2][1] == 1:
speed = -abs(speed) / 1.1
print(speed)
return (speed)
def move(self):
## copying.....................................................
## ...............................................................
# Calculate the error in the x and y directions
max_speed = 0.2
min_speed = 0.05
kp_f = 0.8
kd_f = 40
# A fiducial was detected since last iteration of this loop
if self.found:
forward_error = self.arucoZ - 0.8
lateral_error = self.arucoX
# Calculate the amount of turning needed towards the fiducial
# atan2 works for any point on a circle (as opposed to atan)
# angular_error = math.degrees(math.atan2(self.arucoX, self.arucoZ))
if abs(lateral_error) > self.lateral_error_thres:
self.stopCar()
self.wait(0.5)
self.fix_angle()
# Set the forward speed based distance
linSpeed = kp_f * forward_error + kd_f * (forward_error -
self.previous_error) / 10
if linSpeed <= -max_speed:
self.forward(-max_speed)
elif linSpeed > -max_speed and linSpeed < -min_speed:
self.forward(linSpeed)
elif linSpeed >= max_speed:
self.forward(max_speed)
elif linSpeed > min_speed and linSpeed < max_speed:
self.forward(linSpeed)
else:
self.stopCar()
print "Errors: forward %f lateral %f speed %f" % \
(forward_error, lateral_error, linSpeed)
self.previous_error = forward_error
else:
self.stopCar()
self.rate.sleep()
def fix_angle(self):
forward_error = self.arucoZ - 0.8
lateral_error = self.arucoX
while abs(lateral_error) > self.lateral_error_thres:
print "Errors: forward %f lateral %f " % (forward_error,
lateral_error)
if lateral_error > self.lateral_error_thres:
self.turnRight(0.2)
elif lateral_error < -self.lateral_error_thres:
self.turnLeft(0.2)
else:
print("Done Alignment................................")
self.stopCar()
self.wait(1)
exit
self.rate.sleep()
lateral_error = self.arucoX
def forward(self, speed):
self.leftSpeed = speed
self.rightSpeed = speed
self.publishMotors()
def turnLeft(self, speed):
self.leftSpeed = -speed
self.rightSpeed = speed
self.publishMotors()
def turnRight(self, speed):
self.leftSpeed = speed
self.rightSpeed = -speed
self.publishMotors()
def stopCar(self):
self.leftSpeed = 0
self.rightSpeed = 0
self.publishMotors()
def wait(self, dur):
start = time.time()
while True:
if time.time() - start > dur:
break
self.stopCar()
class image_converter:
def __init__(self):
############# #zum testen
self.image_pub = rospy.Publisher("/control/", Image, queue_size=1)
#############
self.error_pub = rospy.Publisher("/error", Float32, queue_size=1)
rate = rospy.Rate(100)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/color/image_raw",
Image,
self.callback,
queue_size=1,
buff_size=2**24)
rate.sleep()
def callback(self, data):
curve = 1
array = []
help_array = []
try:
image1 = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
#gray
image1 = cv2.cvtColor(image1, cv2.COLOR_BGR2GRAY)
height = image1.shape[0]
width = image1.shape[1]
#get coordinates for ransac
for y in range(200, height - 1):
for x in range(100, width - 200):
rgb = image1[y, x]
if (rgb >= (200)):
help_array.append((x, y))
#mittleren Punkt der weissen Linie nehmen
if (help_array != []):
array.append(help_array[(len(help_array) / 2) - 1])
help_array = []
print len(array)
#drive curve if line is left and not in front of the car
for i in range(0, len(array)):
if (array[i][0] > 200):
curve = 0
if curve == 1:
self.error_pub.publish(-4)
rospy.sleep(5)
else:
#begin of ransac
n = 60
d = 1
t1 = len(array) / 4
set_final = []
set_tmp = []
a_final = (0, 0)
b_final = (0, 0)
while (n > 0):
set_tmp = []
a = random.choice(array)
b = random.choice(array)
while (b == a):
b = random.choice(array)
for i in range(0, len(array)):
a_np = np.asarray(a)
b_np = np.asarray(b)
c = np.asarray(array[i])
d1 = numpy.norm(np.cross(
b_np - a_np, a_np - c)) / numpy.norm(b_np - a_np)
if (d1 < d):
set_tmp.append(array[i])
if (len(set_tmp) >= t1 and len(set_tmp) > len(set_final)):
a_final = a
b_final = b
set_final = set_tmp
n -= 1
#################### #nur zum testen
print a_final
print b_final
cv2.line(image1, a_final, b_final, (0), 3)
cv2.imwrite('img_lines.png', image1)
#####################
m1 = (b_final[1] - a_final[1]) / float(b_final[0] - a_final[0])
b1 = a_final[1] - m1 * a_final[0]
print "\n"
print "m: "
print m1
print "b: "
print b1
print "\n"
row = 240
column = 320
intersection = int((row - b1) / m1)
#intersection
image1[row, intersection] = 150
error = intersection - column
print error
#publish error
self.error_pub.publish(error)
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(image1, "mono8"))
except CvBridgeError as e:
print(e)
Classes to handle Carla sensors
"""
import math
import numpy as np
import tf
from cv_bridge import CvBridge
from geometry_msgs.msg import TransformStamped
from sensor_msgs.msg import CameraInfo
from sensor_msgs.point_cloud2 import create_cloud_xyz32
from std_msgs.msg import Header
from carla.sensor import Camera, Lidar, Sensor
from carla_ros_bridge.transforms import carla_transform_to_ros_transform
cv_bridge = CvBridge(
) # global cv bridge to convert image between opencv and ros
class SensorHandler(object):
"""
Generic Sensor Handler
A sensor handler compute the associated ros message to a carla sensor data, and the transform asssociated to the
sensor.
These messages are passed to a *process_msg_fun* which will take care of publishing them.
"""
def __init__(self,
name,
params=None,
carla_sensor_class=Sensor,
carla_settings=None,
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.has_image = False
self.lights = []
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.stop_wp_mapping = {}
self.tl_initialized = False
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic
light in 3D map space and helps you acquire an accurate ground truth
data source for the traffic light classifier by sending the current
color state of all traffic lights in the simulator. When testing on
the vehicle, the color state will not be available. You'll need to rely
on the position of the light and the camera image to predict it.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb,
queue_size=1)
sub6 = rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=1,
buff_size=2**24)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.tl_initialized = True
rospy.logwarn("TLClassifier initialized")
# self.listener = tf.TransformListener()
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1,
latch=True)
self.debug_image_pub = rospy.Publisher('/debug_image',
Image,
queue_size=1,
latch=True)
def spin(self):
rate = rospy.Rate(5)
while not rospy.is_shutdown():
rate.sleep()
self.publish_tl()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
if self.waypoints is None:
self.waypoints = waypoints
if len(self.waypoints.waypoints) != len(waypoints.waypoints):
rospy.logwarn("Map changed!")
self.stop_wp_mapping = {}
self.waypoints = waypoints
def traffic_cb(self, msg):
self.lights = msg.lights
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image and publishes
the index of the waypoint closest to the red light's stop line
to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
self.has_image = True
self.camera_image = msg
def publish_tl(self):
if not self.tl_initialized:
rospy.logwarn("Waiting for TLClassifier")
return
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def get_closest_waypoint(self, x, y):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
# DONE(see--): implement
dists = [
dist_fn(x, wp.pose.pose.position.x, y, wp.pose.pose.position.y)
for wp in self.waypoints.waypoints
]
min_index = min(range(len(dists)), key=dists.__getitem__)
return min_index
def get_light_state(self, light):
"""Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in
styx_msgs/TrafficLight)
"""
if not self.has_image:
rospy.logwarn("TLDetector: No image")
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
# Get classification
tl_state, debug_image = self.light_classifier.get_classification(
cv_image)
self.debug_image_pub.publish(
self.bridge.cv2_to_imgmsg(debug_image, encoding="rgb8"))
# tl_state = TrafficLight.GREEN
return tl_state
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a
traffic light (-1 if none exists)
int: ID of traffic light color (specified
in styx_msgs/TrafficLight)
"""
light = None
car_wp = None
# List of positions that correspond to the line to stop in front
# of for a given intersection
stop_line_positions = self.config['stop_line_positions']
if self.pose and self.waypoints:
min_index_diff = 100000
car_wp = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
for tl_pos in stop_line_positions:
tl_key = str(tl_pos[0]) + str(tl_pos[1])
if tl_key not in self.stop_wp_mapping:
stop_wp = self.get_closest_waypoint(tl_pos[0], tl_pos[1])
self.stop_wp_mapping[tl_key] = stop_wp
else:
stop_wp = self.stop_wp_mapping[tl_key]
tl_wp_index = min(stop_wp + STOP_TL_OFFSET,
len(self.waypoints.waypoints) - 1)
index_diff = tl_wp_index - car_wp
# check that traffic light is in front: index_diff >= 0
if index_diff < min_index_diff \
and index_diff >= 0:
light = tl_wp_index
min_index_diff = index_diff
elif self.pose is None:
rospy.logwarn_throttle(2, "TLDetector: No pose")
elif self.waypoints is None:
rospy.logwarn_throttle(2, "TLDetector: No wps")
# TODO find the closest visible traffic light (if one exists)
if light is not None:
state = self.get_light_state(light)
return light, state
# self.waypoints = None
return -1, TrafficLight.UNKNOWN
class VisualCompassSetup():
# type: () -> None
"""
TODO docs
Subscribes to raw image
This sets the head behavior to a special head mode, where it scans for image features above the fieldboundary.
The head behavior sends a trigger message, if it reaches predefined points. If this node gets trigged the current image features are saved in our feature map.
Afterwards the map is saved on the robot and the robot handler can download it. Than hes is able to share it with the other robots.
"""
def __init__(self):
# type: () -> None
"""
Initiate VisualCompassHandler
return: None
"""
# Init ROS package
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('bitbots_visual_compass')
rospy.init_node('bitbots_visual_compass_setup')
rospy.loginfo('Initializing visual compass setup')
self.bridge = CvBridge()
self.config = {}
self.image_msg = None
self.compass = None
self.hostname = socket.gethostname()
# TODO: docs
self.base_frame = 'base_footprint'
self.camera_frame = 'camera_optical_frame'
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(50))
self.listener = tf2.TransformListener(self.tf_buffer)
self.pub_head_mode = rospy.Publisher('head_mode',
HeadMode,
queue_size=1)
# Register VisualCompassConfig server for dynamic reconfigure and set callback
Server(VisualCompassConfig, self.dynamic_reconfigure_callback)
rospy.logwarn("------------------------------------------------")
rospy.logwarn("|| WARNING ||")
rospy.logwarn("||Please remove the LAN cable from the Robot, ||")
rospy.logwarn("||after pressing 'YES' you have 10 Seconds ||")
rospy.logwarn("||until the head moves OVER the LAN port!!! ||")
rospy.logwarn("------------------------------------------------\n\n")
try:
input = raw_input
except NameError:
pass
accept = input("Do you REALLY want to start? (YES/n)")
if accept == "YES":
rospy.logwarn("REMOVE THE LAN CABLE NOW!!!!!")
rospy.sleep(10)
head_mode = HeadMode()
head_mode.headMode = 10
self.pub_head_mode.publish(head_mode)
rospy.loginfo("Head mode has been set!")
rospy.spin()
else:
rospy.signal_shutdown(
"You aborted the process! Shuting down correctly.")
def dynamic_reconfigure_callback(self, config, level):
# type: (dict, TODO) -> None
"""
TODO docs
"""
self.compass = VisualCompass(config)
if self.changed_config_param(config, 'compass_type') or \
self.changed_config_param(config, 'compass_matcher') or \
self.changed_config_param(config, 'compass_multiple_map_image_count'):
self.feature_map_images_count = 0
self.processed_set_all_feature_map_images = False
rospy.loginfo(
'Loaded configuration: compass type: %(type)s | matcher type: %(matcher)s | map images: %(feature_map_count)d'
% {
'type': config['compass_type'],
'matcher': config['compass_matcher'],
'feature_map_count':
config['compass_multiple_map_image_count']
})
# Subscribe to Image-message
if self.changed_config_param(config, 'img_msg_topic') or \
self.changed_config_param(config, 'img_msg_queue_size'):
if hasattr(self, 'sub_image_msg'):
self.sub_image_msg.unregister()
self.sub_image_msg = rospy.Subscriber(
config['img_msg_topic'],
Image,
self.image_callback,
queue_size=config['img_msg_queue_size'],
tcp_nodelay=True,
buff_size=60000000)
# https://github.com/ros/ros_comm/issues/536
# Register message server to call set truth callback
if self.changed_config_param(config, 'feature_map_trigger_topic') or \
self.changed_config_param(config, 'feature_map_trigger_queue_size'):
if hasattr(self, 'sub_trigger_set_feature_map'):
self.sub_image_msg.unregister()
self.sub_trigger_set_feature_map = rospy.Subscriber(
config['feature_map_trigger_topic'],
Header,
self.set_truth_callback,
queue_size=config['feature_map_trigger_queue_size'])
self.config = config
self.check_image_count()
return self.config
def set_truth_callback(self, request):
if self.image_msg:
# TODO: check timestamps
orientation = self.tf_buffer.lookup_transform(
self.base_frame,
self.camera_frame,
self.image_msg.header.stamp,
timeout=rospy.Duration(0.5)).transform.rotation
yaw_angle = (euler_from_quaternion(
(orientation.x, orientation.y, orientation.z,
orientation.w))[2] + 0.5 * math.pi) % (2 * math.pi)
image = self.bridge.imgmsg_to_cv2(self.image_msg, 'bgr8')
self.compass.set_truth(yaw_angle, image)
self.feature_map_images_count += 1
self.check_image_count()
else:
rospy.logwarn('No image received yet.')
def image_callback(self, image_msg):
# type: (Image) -> None
"""
TODO docs
"""
# Drops old images
# TODO: fix
# image_age = rospy.get_rostime() - image_msg.header.stamp
# if image_age.to_sec() > 0.1:
# print("Visual Compass: Dropped Image-message") # TODO debug printer
# return
self.image_msg = image_msg
def check_image_count(self):
# type: () -> None
"""
TODO docs
"""
config_feature_map_images_count = self.config[
'compass_multiple_map_image_count']
if self.feature_map_images_count != config_feature_map_images_count:
rospy.loginfo(
'Visual compass: %(var)d of %(config)d map images set. More images are needed.'
% {
'var': self.feature_map_images_count,
'config': config_feature_map_images_count
})
self.processed_set_all_feature_map_images = False
else:
if not (self.processed_set_all_feature_map_images):
rospy.loginfo(
'Visual compass: All map images have been processed.')
self.save_feature_map(self.config['feature_map_file_path'])
self.processed_set_all_feature_map_images = True
def save_feature_map(self, feature_map_file_path):
# type (str) -> None
"""
TODO docs
"""
converter = KeyPointConverter()
# get keypoints and mean feature count per image
features = self.compass.get_feature_map()
mean_feature_count = self.compass.get_mean_feature_count()
# convert keypoints to basic values
keypoints = features[0]
keypoint_values = [converter.key_point2values(kp) for kp in keypoints]
descriptors = features[1]
meta = {
'field':
self.config['feature_map_field'],
'date':
datetime.now(),
'device':
self.hostname,
'compass_type':
self.config['compass_type'],
'compass_matcher':
self.config['compass_matcher'],
'compass_multiple_map_image_count':
self.config['compass_multiple_map_image_count'],
'keypoint_count':
len(keypoint_values),
'descriptor_count':
len(descriptors),
'mean_feature_count':
mean_feature_count,
}
dump_features = {
'keypoint_values': keypoint_values,
'descriptors': descriptors,
'meta': meta
}
# generate file path
file_path = self.package_path + feature_map_file_path
# warn, if file does exist allready
if path.isfile(file_path):
rospy.logwarn(
'Map file at: %(path)s does ALLREADY EXIST. This will be overwritten.'
% {'path': file_path})
# save keypoints in pickle file
with open(file_path, 'wb') as f:
pickle.dump(dump_features, f)
info_str = "\n\t-----------------------------------------------------------------------------------------------------------------\n" + \
"\tSaved map file at: %(path)s\n" % {'path': file_path} + \
"\tRUN the following command on your system (NOT THE ROBOT) to save the map file in your current directory:\n" + \
"\n\tscp bitbots@%(host)s:%(path)s .\n" % {'path': file_path, 'host': self.hostname} + \
"\t-----------------------------------------------------------------------------------------------------------------"
rospy.loginfo(info_str)
# shutdown setup process
rospy.signal_shutdown('Visual compass setup finished cleanly.')
def changed_config_param(self, config, param_name):
# type: (dict, str) -> bool
"""
TODO
"""
return param_name not in self.config or config[
param_name] != self.config[param_name]
class RecognizeEmotionSM(ActionSMBase):
def __init__(self, timeout=120.0,
image_topic='/cam3d/rgb/image_raw',
emotion_model_path=None,
labels=None,
image_size=(0, 0, 0),
max_recovery_attempts=1):
super(RecognizeEmotionSM, self).__init__('RecognizeEmotion', [], max_recovery_attempts)
self.timeout = timeout
self.labels = labels
self.image_size = image_size
self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1)
self.emotion_model_path = emotion_model_path
self.bridge = CvBridge()
self.emotion_model = None
self.computation_graph = None
def init(self):
try:
rospy.loginfo('[recognize_emotion] Loading model %s', self.emotion_model_path)
self.emotion_model = load_model(emotion_model_path)
# the following two lines are necessary for avoiding
# https://github.com/keras-team/keras/issues/2397
self.emotion_model._make_predict_function()
self.computation_graph = tf.get_default_graph()
except:
rospy.logerr('[recognize_emotion] Model %s could not be loaded', self.emotion_model_path)
return FTSMTransitions.INITIALISED
def running(self):
number_of_faces = self.goal.number_of_faces
bounding_boxes = self.goal.bounding_boxes
emotions = []
rospy.loginfo('[recognize_emotion] Recognizing emotions')
rgb_image = self.__ros2cv(self.goal.image)
gray_image = self.__rgb2gray(rgb_image)
for face in bounding_boxes:
x, y, w, h = face.bounding_box_coordinates
face = gray_image[y: (y + h), x: (x + w)]
face = cv2.resize(face, self.image_size[0:2])
face = np.expand_dims(face, 0)
face = np.expand_dims(face, -1)
face = self.__preprocess_image(face)
predicted_emotion = self.__predict_emotion(face)
emotions.append(predicted_emotion)
rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h),
(0, 0, 255), 2)
cv2.putText(rgb_image, predicted_emotion, (x, y - 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0),
1, cv2.LINE_AA)
output_ros_image = self.bridge.cv2_to_imgmsg(rgb_image, 'bgr8')
self.image_publisher.publish(output_ros_image)
self.result = self.set_result(True, emotions)
return FTSMTransitions.DONE
def set_result(self, success, emotions):
result = RecognizeEmotionResult()
result.success = success
result.emotions = emotions
return result
def __preprocess_image(self, image):
image = image / 255.0
return image
def __predict_emotion(self, face):
label = -1
with self.computation_graph.as_default():
class_predictions = self.emotion_model.predict(face)
label = self.labels[np.argmax(class_predictions)]
return label
def __ros2cv(self, ros_image):
cv_image = self.bridge.imgmsg_to_cv2(ros_image, 'bgr8')
return np.array(cv_image, dtype=np.uint8)
def __rgb2gray(self, image):
gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
return gray_image
import rospy
from sensor_msgs.msg import Image
if __name__ == '__main__':
rgb_image_root = '../data/rgb_image_0048'
file_list = []
tmp_list = os.listdir(rgb_image_root)
tmp_list.sort()
for f in tmp_list:
cur_file = os.path.join(rgb_image_root, f)
file_list.append(cur_file)
rospy.init_node('pub_rgb_image')
pub_velo = rospy.Publisher("rgb_image", Image, queue_size=1)
rate = rospy.Rate(0.2)
bridge = CvBridge()
pc_num_counter = 0
while not rospy.is_shutdown():
rospy.loginfo(file_list[pc_num_counter])
# pc_data = np.fromfile(file_list[pc_num_counter], dtype=np.float32).reshape(-1, 4)
cv_image = cv2.imread(file_list[pc_num_counter])
image_message = bridge.cv2_to_imgmsg(cv_image, encoding="passthrough")
pub_velo.publish(image_message)
# print(pc_data[0,0,:])
#pc_data = pc_data[:, :, :4]
#pc_data = np.fromfile(file_list[pc_num_counter], dtype=np.float32).reshape(-1, 4)
pc_num_counter = pc_num_counter + 1
if pc_num_counter >= len(file_list):
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)
def __init__(self):
self.image_pub = rospy.Publisher("/eyes/right", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera2/usb_cam2/image_raw", Image,
self.callback)
haar_flags = 0
display = True
if __name__ == '__main__':
opencv_dir = '/usr/share/opencv/haarcascades/'
face_cascade = cv2.CascadeClassifier(opencv_dir +
'haarcascade_frontalface_default.xml')
if face_cascade.empty():
print "Could not find face cascade"
sys.exit(-1)
eye_cascade = cv2.CascadeClassifier(opencv_dir + 'haarcascade_eye.xml')
if eye_cascade.empty():
print "Could not find eye cascade"
sys.exit(-1)
br = CvBridge()
rospy.init_node('facedetect')
display = rospy.get_param("~display", True)
def detect_and_draw(imgmsg):
img = br.imgmsg_to_cv2(imgmsg, "bgr8")
# allocate temporary images
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 3)
for (x, y, w, h) in faces:
cv2.rectangle(img, (x, y), (x + w, y + h), (255, 0, 0), 2)
roi_gray = gray[y:y + h, x:x + w]
roi_color = img[y:y + h, x:x + w]
eyes = eye_cascade.detectMultiScale(roi_gray)
for (ex, ey, ew, eh) in eyes:
cv2.rectangle(roi_color, (ex, ey), (ex + ew, ey + eh),
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--in_bag_file", help="input ROS bag")
parser.add_argument("--out_bag_file", help="output ROS bag")
parser.add_argument("--image_topic", help="image topic")
parser.add_argument("--cam_info_topic", help="camera info topic")
parser.add_argument("--desired_width", type=int)
parser.add_argument("--desired_height", type=int)
args = parser.parse_args()
in_bag = rosbag.Bag(args.in_bag_file, "r")
bridge = CvBridge()
count = 0
orig_width = None
orig_height = None
desired_width = args.desired_width
desired_height = args.desired_height
# figure out orig_width and orig_height first
for topic, msg, t in in_bag.read_messages(args.cam_info_topic):
orig_width = msg.width
orig_height = msg.height
break
# scaling factors for camera_info in x and y directions
s_x = desired_width / float(orig_width)
s_y = desired_height / float(orig_height)
with rosbag.Bag(args.out_bag_file, 'w') as outbag:
for topic, msg, t in in_bag.read_messages():
# resize camera_info msgs' K and P accordingly
if (topic == args.cam_info_topic):
orig_K = msg.K
orig_P = msg.P
new_msg = deepcopy(msg)
new_msg.width = desired_width
new_msg.height = desired_height
new_msg.K = [s_x*orig_K[0], orig_K[1], s_x*orig_K[2],\
orig_K[3], s_y*orig_K[4], s_y*orig_K[5],\
orig_K[6], orig_K[7], orig_K[8]]
new_msg.P = [s_x*orig_P[0], orig_P[1], s_x*orig_P[2], orig_P[3],\
orig_P[4], s_y*orig_P[5], s_y*orig_P[6], orig_P[7],\
orig_P[8], orig_P[9], orig_P[10], orig_P[11]]
# print new_msg
outbag.write(topic, new_msg, t)
# resize images accordingly
elif (topic == args.image_topic):
# read bayered image as a long vector
bayer_img = np.frombuffer(msg.data, dtype='uint8')
# resize bayered image to 2D
bayer_img = np.reshape(bayer_img, (orig_height, orig_width, 1))
# debayer the raw image
color_img = cv2.cvtColor(bayer_img, cv2.COLOR_BAYER_BG2BGR)
# resize the debayered image
color_img = cv2.resize(color_img,
(desired_width, desired_height))
# cv2.imshow("color_img", color_img)
# cv2.waitKey(1)
# conver back to imgmsg
new_msg = bridge.cv2_to_imgmsg(color_img, encoding='bgr8')
outbag.write(topic, new_msg, t)
else:
outbag.write(topic, msg, t)
in_bag.close()
class YellowLineFollower:
SPEED = 0.6
QUEUE_SIZE = 1
WHEELBASE = 0.33
def __init__(self):
self.bridge_object = CvBridge()
self.image_sub = rospy.Subscriber('/camera/color/image_raw',
Image, self.camera_callback)
# self.drive_pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/teleop',
# AckermannDriveStamped, queue_size= self.QUEUE_SIZE)
self.drive_pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/output',
AckermannDriveStamped, queue_size= self.QUEUE_SIZE)
# /vesc/low_level/ackermann_cmd_mux/output
self.last_angle = None
self.test1_pub = rospy.Publisher('/test/res_raw', Image, queue_size = self.QUEUE_SIZE)
self.test2_pub = rospy.Publisher('/test/hls_raw', Image, queue_size = self.QUEUE_SIZE)
self.test3_pub = rospy.Publisher('test/mask_raw', Image, queue_size = self.QUEUE_SIZE)
def convert_angular_velocity_to_steering_angle(self, angular_velocity):
if angular_velocity == 0:
return 0
return math.atan(angular_velocity * (self.WHEELBASE/self.SPEED))
def camera_callback(self, data):
try:
cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding='bgr8')
except CvBridgeError as e:
print(e)
height, width, channels = cv_image.shape
top_trunc = int(height/2)
mid_width = width/2
margin = width/4
left_bound = int(mid_width - margin)
right_bound = int(mid_width + margin)
crop_img = cv_image[top_trunc: ,0:width]
# image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
# hls = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)#.astype(np.float)
# lower_yellow = np.array([ 0, 105, 175])
# upper_yellow = np.array([29, 255, 255])
# mask = cv2.inRange(hls, lower_yellow, upper_yellow)
hls = cv2.cvtColor(crop_img, cv2.COLOR_BGR2RGB).astype(np.float)
lower_yellow = np.array([171, 135, 82])
upper_yellow = np.array([255, 255, 125])
mask = cv2.inRange(hls, lower_yellow, upper_yellow)
# rows_to_watch = 100
# top_trunc = int(height/2)
# bot_trunc = int(top_trunc + rows_to_watch)
# crop_img = cv_image[top_trunc:bot_trunc, 0:width]
m = cv2.moments(mask, False)
try:
cx, cy = m['m10']/m['m00'], m['m01']/m['m00']
except ZeroDivisionError:
cx, cy = width / 2, height / 2
res = cv2.bitwise_and(crop_img, crop_img, mask = mask)
cv2.circle(res, (int(cx), int(cy)), 10, (0, 0, 255), -1)
# cv2.imshow("Original", cv_image)
# cv2.imshow("HSV", hls)
# cv2.imshow("MASK", mask)
# cv2.imshow("RES", res)
# cv2.waitKey(0)
error_x = cx - width/2
angular_z = -error_x/100
if angular_z == 0:
steering_angle = self.last_angle
else:
steering_angle = self.convert_angular_velocity_to_steering_angle(angular_z)
self.last_angle = steering_angle
drive_msg = AckermannDriveStamped()
drive_msg.header.stamp = data.header.stamp
drive_msg.drive.speed = self.SPEED
steering_angle = np.clip(steering_angle, -0.4, 0.4)
drive_msg.drive.steering_angle = steering_angle
self.drive_pub.publish(drive_msg)
self.test2_pub.publish(self.bridge_object.cv2_to_imgmsg(hls))
self.test3_pub.publish(self.bridge_object.cv2_to_imgmsg(mask))
self.test1_pub.publish(self.bridge_object.cv2_to_imgmsg(res))
class image_feature:
def __init__(self):
'''Initialize ros publisher, ros subscriber'''
# topic where we publish
self.image_pub = rospy.Publisher("/color_detection/image",
Image, queue_size=10)
self.bridge = CvBridge()
#topic where the coordinates go
self.cam_pose_pub = rospy.Publisher("/color_detection/cam_point", Point, queue_size=1)
self.cam_pose = Point()
# subscribed Topic
self.subscriber = rospy.Subscriber("/uav_camera/image_raw_down", Image, self.callback, queue_size=1)
if VERBOSE :
print "subscribed to /uav_camera/image_raw_down"
def callback(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and features detected'''
if VERBOSE :
print 'received image of type: "%s"' % ros_data.format
###Set boundarys###
lower_range = np.array([0, 255, 255], dtype=np.uint8) #The object color is 255,153,0
upper_range = np.array([0, 0, 255], dtype=np.uint8)
lower_blue = np.array([100, 150, 0])
upper_blue = np.array([140, 255, 255])
yellow_lr = np.array([20, 100, 100], dtype=np.uint8)
yellow_ur = np.array([30, 255, 255], dtype=np.uint8)
try:
cv_image = self.bridge.imgmsg_to_cv2(ros_data, "bgr8")
except CvBridgeError as e:
print(e)
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, lower_blue, upper_blue)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
output = cv2.bitwise_and(cv_image, cv_image, mask=mask)
image, contours, hierarchy = cv2.findContours(mask.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
center = Point()
if len(contours) > 0:
c = max(contours, key=cv2.contourArea)
x, y, w, h = cv2.boundingRect(c)
M = cv2.moments(c)
center.x = float(M["m10"] / M["m00"])
center.y = float(M["m01"] / M["m00"])
self.find_relative_pose(center.x, center.y, w, h)
cv2.rectangle(cv_image, (x, y),(x+w, y+h), (0, 255, 255), 2)
else:
self.cam_pose = Point()
self.cam_pose.x = float("inf")
self.cam_pose.y = float("inf")
self.cam_pose.z = float("inf")
self.cam_pose_pub.publish(self.cam_pose)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def find_relative_pose(self, x, y, w, h):
quad_3d = np.float32([[-0.0895, -0.0895, 0], [0.0895, -0.0895, 0], [0.0895, 0.0895, 0], [-0.0895, 0.0895, 0]])
quad = np.float32([[x - w / 2, y - h / 2], [x - w / 2, y + h / 2], [x + w / 2, y + h / 2], [x + w / 2, y - h / 2]])
K = np.float64([[1004.603218, 0 , 640.5],
[ 0 , 1004.603218, 360.5],
[ 0.0 , 0.0 , 1.0]])
dist_coef = np.zeros(4)
_ret, rvec, tvec = cv2.solvePnP(quad_3d, quad, K, dist_coef)
rmat = cv2.Rodrigues(rvec)[0]
cameraTranslatevector = -np.matrix(rmat).T * np.matrix(tvec)
T0 = np.zeros((4, 4))
T0[:3, :3] = rmat
T0[:4, 3] = [0, 0, 0, 1]
T0[:3, 3] = np.transpose(cameraTranslatevector)
p0 = np.array([-0.0895/2, -0.0895/2, 0, 1])
z0 = np.dot(T0, p0)
self.cam_pose.x = z0.item(0)
self.cam_pose.y = z0.item(1)
self.cam_pose.z = z0.item(2)
self.cam_pose_pub.publish(self.cam_pose)