class VideoManager(multiprocessing.Process):
"""
The VideoManager is a part of the Communicator. It handles the opening of
video devices when requested and serves the images to the vision modules.
The VideoManager is separated from the Communicator to avoid crashing the
communicator when a video module fails. The communicator will automatically
restart the video manager when it crashes.
There are two modes available currently, which can be changed by setting the
__server_mode boolean to True or False. When server mode is enabled, the
VideoManager will set up a socket to accept incoming image requests and will
serve the images over those sockets. When server mode is disabled, instead
the VidMemWriter class will be used to write the images to /dev/shm/images,
where the vision modules can collect them. The latter approach is the old
behavior. While this works, it introduces more delay to the images because
images are not served on request basis but depend on the frequency of the
VideoManager. The new approach requests an image only when the vision module
requires one, bringing the time of processing and the time of generation
closer together. It also gives the opportunity to serve vision modules on
a different host, by connecting over TCP/IP to port 59152. This will of
course result in degraded performance because transmitting over the network
takes a lot longer than over the Unix Domain Socket. Still, performance
should be usable.
"""
def __init__(self, pipe, profile=False):
super(VideoManager, self).__init__()
self.__vidmemwriter = None
self.__pipe = pipe
self.__heartbeat = time.time()
self.__hb_interval = 2
self.__camera = 0
self.__nao_camera = None
self.__logger = None
self.__video_sources = []
self.__profile = profile
self.__server_mode = True
self.__video_modules = {}
self.__last_images = {}
self.__video_locks = {}
self.__image_queue = []
self.__image_handlers = []
try:
import util.openni_kinectvideo
self.__use_openni = False
except:
self.__use_openni = False
def stop(self, signum=None, frame=None):
"""
Process a stop request, usually used as a signal handler
"""
self.__logger.warning("Caught signal, exiting")
self._running = False
def run(self):
"""
The run method is the method started when the new process starts. It
wraps the _run method, using the cProfile when requested to profile
the VideoManager.
"""
global vminstance
iomanager.clear_IOM()
vminstance = self
if self.__profile:
import cProfile, pstats
cProfile.runctx('vminstance._run()', globals(), locals(), 'videomanager.prof')
else:
self._run()
def _run(self):
"""
The main loop of the VideoManager. It will communicate with the
Communicator to start/stop video sources and to check which
sources are available.
"""
# Catch signals, shut down nicely
signal.signal(signal.SIGTERM, self.stop)
signal.signal(signal.SIGINT, self.stop)
# Set up server listening on TCP socket and Unix Domain Socket
self.__UD_server = VideoSourceServer(self, "VideoManager")
# Use compression for TCP socket because that one will most likely
# be used, if at all, by vision modules running on a different machine,
# so bandwidth will be limited.
self.__tcp_server = VideoSourceServer(self, 49953, compress=True, compress_level=6)
# Set up the Queue for the image requests from vision modules
self.__image_queue = Queue.Queue()
self._ticker = Ticker(10)
poller = iopoll.IOPoll()
poller.register(self.__pipe)
self.__logger = logging.getLogger('Borg.Brain.Communicator.VideoManager')
self.__logger.info("VideoManager running")
self._running = True
#try:
while self._running:
self._ticker.tick()
events = poller.poll(0)
for fd, event in events:
if event & iopoll.IO_ERROR:
self._running = False
break
if event & iopoll.IO_READ:
self._handle_command()
if event & iopoll.IO_WRITE:
self._send_heartbeat()
if self.__vidmemwriter:
self.__vidmemwriter.update()
#except:
# self._running = False
self._stop_video_sources()
self.__logger.info("VideoManager stopping")
def _send_heartbeat(self):
"""
Send a heartbeat to the Communicator to indicate that
the VideoManager has not crashed.
"""
if time.time() > self.__heartbeat + self.__hb_interval:
self.__pipe.send({"command": "heartbeat"})
self.__heartbeat = time.time()
def _handle_command(self):
"""
Process a command from the Communicator: starting or stopping
of video modules or returning which video sources are active
or available.
"""
data = self.__pipe.recv()
cmd = data['command']
if cmd == "start":
# Start a video source
source = data['source']
ip = None
inputres = None
outputres = None
if "ip" in data:
ip = data['ip']
if "inputres" in data:
inputres = data['inputres']
if "outputres" in data:
outputres = data['outputres']
if "camera" in data:
self.__camera = data['camera']
result = self._start_video_source(source, ip, inputres, outputres)
self.__check_image_handlers()
self.__pipe.send({"command": "source_started", "result": result})
elif cmd == "stop":
# Stop a video source
source = data['source']
result = self._stop_video_source(source)
self.__pipe.send({"command": "source_stopped", "result": result})
elif cmd == "set_nao_camera":
# Change the camera the NAO is using: chin or top
if "camera" in data:
camera = data['camera']
if self.__nao_camera:
self.__nao_camera.change_camera(camera)
else:
self.__logger.warning("Nao Video camera source is None, cannot change camera")
elif cmd == "check_sources":
# Check which sources are available/connected to this machine
sources = self._check_sources()
self.__pipe.send({"command": "available_sources", "available_sources": sources})
elif cmd == "set_speed":
# Change the running frequency of the VideoManager
if "frequency" in data:
freq = data['frequency']
self.__logger.info("Setting Video Manager speed to %d Hz" % freq)
self._ticker.set_frequency(freq)
elif cmd == "quit":
# End the VideoManager
self._running = False
def _start_video_source(self, source, ip=None, inputres="640x480", outputres="640x480"):
"""
This method starts a new video source, which some optional arguments
specified such as input and output resolution and a IP address.
Currently, these settings only apply to the naovideo source.
"""
if source in self.__video_sources:
return True
if not source or source == "None":
return True
result = self._start_vidmemwriter(source, ip, inputres, outputres)
return result
def _start_vidmemwriter(self, camType, ip=None, inputres="640x480", outputres="640x480"):
"""
Start the vidmemwriter and a video source, if required. If in server
mode, no vidmemwriter will be started. Instead, the video module will
be registered for with by the image_handlers.
"""
if not self.__vidmemwriter and not self.__server_mode:
self.__vidmemwriter = vidmemwriter.VidMemWriter([], [])
if camType in self.__video_sources:
return True
self.__logger.info("I'm starting %s" % camType)
if ros_pattern.match(camType):
#The first 4 characters "ros_" identify that is a specific ros image
#The second part *** in "ros_***/topic" is the encoding:
topic = camType[4:]
encoding = "passthrough"
self.__logger.info("camType !!!!!! %s" % camType)
if not camType[4] == '/':
str_list = camType.split("_")
topic = '_'.join(str_list[2:])
encoding = str_list[1]
ros_image_source = rosimage.RosImage(topic, encoding)
if self.__server_mode:
self.__register_video_source(camType, ros_image_source)
else:
self.__vidmemwriter.add_video_source(ros_image_source, camType)
self.__video_sources.append(camType)
self.__logger.info("rosimage started for topic: %s, with encoding: %s" % (topic, encoding))
return True
elif camType == "webcam":
self.__logger.debug("I'm starting webcam")
webcamsource = takeimages.TakeImages(self.__camera)
img = webcamsource.get_image()
if type(img) is type(""):
self.__logger.error("No camera found. Please check connection!")
return False
if webcamsource.Nocamera:
if self.__camera == -1:
self.__logger.error("No camera found. Please check connection!")
else:
self.__logger.error("Camera %d not found. Please check connection!" % self.__camera)
return False
if self.__server_mode:
self.__register_video_source('webcam', webcamsource)
else:
self.__vidmemwriter.add_video_source(webcamsource, "webcam")
self.__video_sources.append("webcam")
self.__logger.info("Webcam started")
return True
elif camType == 'kinect_openni':
self.__logger.debug("I'm starting kinect using openni")
import util.openni_kinectvideo as kv
depth_source = kv.OpenNIKinect("depth")
rgb_source = kv.OpenNIKinect("rgb")
try:
depth_source.get_image()
except:
self.__logger.error("Kinect not found. Please check connection!")
return False
if self.__server_mode:
self.__register_video_source('kinect_depth', depth_source)
self.__register_video_source('kinect_rgb', rgb_source)
else:
self.__vidmemwriter.add_video_source(depth_source, "kinect_depth")
self.__vidmemwriter.add_video_source(rgb_source, "kinect_rgb")
self.__video_sources.append("kinect_depth")
self.__video_sources.append("kinect_rgb")
self.__video_sources.append("kinect")
self.__video_sources.append("kinect_openni")
self.__logger.info("Kinect started")
return True
elif camType == 'kinect' or camType == 'kinect_rgb' or camType == 'kinect_depth':
if self.__use_openni:
self.__logger.info("I'm starting kinect using openni")
import util.openni_kinectvideo as kv
depth_source = kv.OpenNIKinect("depth")
rgb_source = kv.OpenNIKinect("rgb")
try:
depth_source.get_image()
except:
self.__logger.error("Kinect not found. Please check connection!")
return False
else:
self.__logger.info("I'm starting kinect using freenect")
try:
import util.kinectmemwriter
except:
self.__logger.error("Could not load kinectmemwriter module. Check modules.")
return False
depth_source = util.kinectmemwriter.KinectDepthSource()
rgb_source = util.kinectmemwriter.KinectRGBSource()
try:
depth_source.get_image()
except:
self.__logger.error("Kinect not found. Please check connection!")
return False
if self.__server_mode:
self.__register_video_source('kinect_depth', depth_source)
self.__register_video_source('kinect_rgb', rgb_source)
else:
self.__vidmemwriter.add_video_source(depth_source, "kinect_depth")
self.__vidmemwriter.add_video_source(rgb_source, "kinect_rgb")
self.__video_sources.append("kinect_depth")
self.__video_sources.append("kinect_rgb")
self.__video_sources.append("kinect")
self.__logger.info("Kinect started")
return True
elif camType == "naovideo":
self.__logger.debug("I'm starting naovideo")
try:
import util.naovideo as naovideo
except:
self.__logger.error("Could not load naovideo module. Check modules")
return False
#get ip of nao:
#TODO: fix this dirty hack (it should be read from the config file)
naoip = "129.125.178.232"
if ip:
naoip = ip
self.__logger.warn("Using input resolution %s and output resolution %s" % (inputres, outputres))
#use the naovideo module:
if self.__camera != 0 and self.__camera != 1:
self.__camera = 0
try:
naocamsource = naovideo.VideoModule(naoip, inputres, outputres, camera=self.__camera)
naocamsource.get_image()
except:
self.__logger.error("Something went wrong using the camera of the nao (check connection!)")
traceback.print_exc()
return False
if self.__server_mode:
self.__register_video_source('naovideo', naocamsource)
else:
self.__vidmemwriter.add_video_source(naocamsource, "naovideo")
self.__video_sources.append("naovideo")
self.__nao_camera = naocamsource
self.__logger.info("Naovideo started")
return True
else:
self.__logger.warning("Invalid video source specified: %s" % camType)
return False
def __register_video_source(self, name, source):
"""
Register a running video source, for use in server mode.
"""
self.__video_modules[name] = source
self.__last_images[name] = (time.time(), source.get_image())
self.__video_locks[name] = threading.Lock()
def _stop_video_sources(self):
"""
Stop all video sources.
"""
for source in self.__video_sources:
self._stop_video_source(source)
def _stop_video_source(self, video_source):
"""
Stop the specified video source
"""
if not video_source in self.__video_sources:
self.__logger.warning("Not stopping video source %s - not running" % video_source)
return True
if self.__server_mode:
if video_source == "kinect":
if "kinect_rgb" in self.__video_modules:
del self.__video_modules["kinect_rgb"]
del self.__last_images["kinect_rgb"]
del self.__video_locks["kinect_rgb"]
if "kinect_depth" in self.__video_modules:
del self.__video_modules["kinect_depth"]
del self.__last_images["kinect_depth"]
del self.__video_locks["kinect_depth"]
else:
if video_source in self.__video_modules:
if video_source == "naovideo":
self.__video_modules[video_source].unsubscribe()
del self.__video_modules[video_source]
del self.__last_images[video_source]
del self.__video_locks[video_source]
if video_source == "kinect":
import util.kinectvideo
if not self.__server_mode:
self.__vidmemwriter.stop_video_source("kinect_rgb")
self.__vidmemwriter.stop_video_source("kinect_depth")
for src in ['kinect', 'kinect_rgb', 'kinect_depth', 'kinect_openni']:
if src in self.__video_sources:
self.__video_sources.remove(src)
util.kinectvideo.StopKinect()
else:
self.__video_sources.remove(video_source)
if not self.__server_mode:
self.__vidmemwriter.stop_video_source(video_source)
if not self.__video_sources:
self.__logger.info("No video source active. Stopping vidmemwriter.")
self.__vidmemwriter = None
return True
def _check_sources(self):
"""
Check the available video sources. Use the list of active video sources
as a starting point. Then, for each known video source not currently
active, try to start it, and if this succeeds, add it to the list.
"""
avail_sources = []
for source in self.__video_sources:
avail_sources.append(source)
# Try to start each video source known. Naovideo is not in this list,
# because without an explicit request, no IP and port is known so no
# connection can be made.
for source in ["webcam", "kinect"]:
if source in avail_sources:
continue
# Status unknown, try to start and stop
self.__logger.info("Trying to see if source %s is available" % source)
try:
if self._start_video_source(source):
avail_sources.append(source)
self._stop_video_source(source)
except Exception, e:
self.__logger.info("Error starting source %s, (error: %s) must not be available" % (source, str(e)))
return avail_sources
class VideoManager(multiprocessing.Process):
"""
The VideoManager is a part of the Communicator. It handles the opening of
video devices when requested and serves the images to the vision modules.
The VideoManager is separated from the Communicator to avoid crashing the
communicator when a video module fails. The communicator will automatically
restart the video manager when it crashes.
There are two modes available currently, which can be changed by setting the
__server_mode boolean to True or False. When server mode is enabled, the
VideoManager will set up a socket to accept incoming image requests and will
serve the images over those sockets. When server mode is disabled, instead
the VidMemWriter class will be used to write the images to /dev/shm/images,
where the vision modules can collect them. The latter approach is the old
behavior. While this works, it introduces more delay to the images because
images are not served on request basis but depend on the frequency of the
VideoManager. The new approach requests an image only when the vision module
requires one, bringing the time of processing and the time of generation
closer together. It also gives the opportunity to serve vision modules on
a different host, by connecting over TCP/IP to port 59152. This will of
course result in degraded performance because transmitting over the network
takes a lot longer than over the Unix Domain Socket. Still, performance
should be usable.
"""
def __init__(self, pipe, profile=False):
super(VideoManager, self).__init__()
self.__vidmemwriter = None
self.__pipe = pipe
self.__heartbeat = time.time()
self.__hb_interval = 2
self.__camera = 0
self.__nao_camera = None
self.__logger = None
self.__video_sources = []
self.__profile = profile
self.__server_mode = True
self.__video_modules = {}
self.__last_images = {}
self.__video_locks = {}
self.__image_queue = []
self.__image_handlers = []
try:
import util.openni_kinectvideo
self.__use_openni = False
except:
self.__use_openni = False
def stop(self, signum=None, frame=None):
"""
Process a stop request, usually used as a signal handler
"""
self.__logger.warning("Caught signal, exiting")
self._running = False
def run(self):
"""
The run method is the method started when the new process starts. It
wraps the _run method, using the cProfile when requested to profile
the VideoManager.
"""
global vminstance
iomanager.clear_IOM()
vminstance = self
if self.__profile:
import cProfile, pstats
cProfile.runctx('vminstance._run()', globals(), locals(), 'videomanager.prof')
else:
self._run()
def _run(self):
"""
The main loop of the VideoManager. It will communicate with the
Communicator to start/stop video sources and to check which
sources are available.
"""
# Catch signals, shut down nicely
signal.signal(signal.SIGTERM, self.stop)
signal.signal(signal.SIGINT, self.stop)
# Set up server listening on TCP socket and Unix Domain Socket
self.__UD_server = VideoSourceServer(self, "VideoManager")
# Use compression for TCP socket because that one will most likely
# be used, if at all, by vision modules running on a different machine,
# so bandwidth will be limited.
self.__tcp_server = VideoSourceServer(self, 49953, compress=True, compress_level=6)
# Set up the Queue for the image requests from vision modules
self.__image_queue = Queue.Queue()
self._ticker = Ticker(10)
poller = iopoll.IOPoll()
poller.register(self.__pipe)
self.__logger = logging.getLogger('Borg.Brain.Communicator.VideoManager')
self.__logger.info("VideoManager running")
self._running = True
#try:
while self._running:
self._ticker.tick()
events = poller.poll(0)
for fd, event in events:
if event & iopoll.IO_ERROR:
self._running = False
break
if event & iopoll.IO_READ:
self._handle_command()
if event & iopoll.IO_WRITE:
self._send_heartbeat()
if self.__vidmemwriter:
self.__vidmemwriter.update()
#except:
# self._running = False
self._stop_video_sources()
self.__logger.info("VideoManager stopping")
def _send_heartbeat(self):
"""
Send a heartbeat to the Communicator to indicate that
the VideoManager has not crashed.
"""
if time.time() > self.__heartbeat + self.__hb_interval:
self.__pipe.send({"command": "heartbeat"})
self.__heartbeat = time.time()
def _handle_command(self):
"""
Process a command from the Communicator: starting or stopping
of video modules or returning which video sources are active
or available.
"""
data = self.__pipe.recv()
cmd = data['command']
if cmd == "start":
# Start a video source
source = data['source']
ip = None
inputres = None
outputres = None
if "ip" in data:
ip = data['ip']
if "inputres" in data:
inputres = data['inputres']
if "outputres" in data:
outputres = data['outputres']
if "camera" in data:
self.__camera = data['camera']
result = self._start_video_source(source, ip, inputres, outputres)
self.__check_image_handlers()
self.__pipe.send({"command": "source_started", "result": result})
elif cmd == "stop":
# Stop a video source
source = data['source']
result = self._stop_video_source(source)
self.__pipe.send({"command": "source_stopped", "result": result})
elif cmd == "set_nao_camera":
# Change the camera the NAO is using: chin or top
if "camera" in data:
camera = data['camera']
if self.__nao_camera:
self.__nao_camera.change_camera(camera)
else:
self.__logger.warning("Nao Video camera source is None, cannot change camera")
elif cmd == "check_sources":
# Check which sources are available/connected to this machine
sources = self._check_sources()
self.__pipe.send({"command": "available_sources", "available_sources": sources})
elif cmd == "set_speed":
# Change the running frequency of the VideoManager
if "frequency" in data:
freq = data['frequency']
self.__logger.info("Setting Video Manager speed to %d Hz" % freq)
self._ticker.set_frequency(freq)
elif cmd == "quit":
# End the VideoManager
self._running = False
def _start_video_source(self, source, ip=None, inputres="640x480", outputres="640x480"):
"""
This method starts a new video source, which some optional arguments
specified such as input and output resolution and a IP address.
Currently, these settings only apply to the naovideo source.
"""
if source in self.__video_sources:
return True
if not source or source == "None":
return True
result = self._start_vidmemwriter(source, ip, inputres, outputres)
return result
def _start_vidmemwriter(self, camType, ip=None, inputres="640x480", outputres="640x480"):
"""
Start the vidmemwriter and a video source, if required. If in server
mode, no vidmemwriter will be started. Instead, the video module will
be registered for with by the image_handlers.
"""
if not self.__vidmemwriter and not self.__server_mode:
self.__vidmemwriter = vidmemwriter.VidMemWriter([], [])
if camType in self.__video_sources:
return True
self.__logger.info("I'm starting %s" % camType)
if ros_pattern.match(camType):
#The first 4 characters "ros_" identify that is a specific ros image
#The second part *** in "ros_***/topic" is the encoding:
topic = camType[4:]
encoding = "passthrough"
self.__logger.info("camType !!!!!! %s" % camType)
if not camType[4] == '/':
str_list = camType.split("_")
topic = '_'.join(str_list[2:])
encoding = str_list[1]
ros_image_source = rosimage.RosImage(topic, encoding)
if self.__server_mode:
self.__register_video_source(camType, ros_image_source)
else:
self.__vidmemwriter.add_video_source(ros_image_source, camType)
self.__video_sources.append(camType)
self.__logger.info("rosimage started for topic: %s, with encoding: %s" % (topic, encoding))
return True
elif camType == "webcam":
self.__logger.debug("I'm starting webcam")
webcamsource = takeimages.TakeImages(self.__camera)
img = webcamsource.get_image()
if type(img) is type(""):
self.__logger.error("No camera found. Please check connection!")
return False
if webcamsource.Nocamera:
if self.__camera == -1:
self.__logger.error("No camera found. Please check connection!")
else:
self.__logger.error("Camera %d not found. Please check connection!" % self.__camera)
return False
if self.__server_mode:
self.__register_video_source('webcam', webcamsource)
else:
self.__vidmemwriter.add_video_source(webcamsource, "webcam")
self.__video_sources.append("webcam")
self.__logger.info("Webcam started")
return True
elif camType == 'kinect_openni':
self.__logger.debug("I'm starting kinect using openni")
import util.openni_kinectvideo as kv
depth_source = kv.OpenNIKinect("depth")
rgb_source = kv.OpenNIKinect("rgb")
try:
depth_source.get_image()
except:
self.__logger.error("Kinect not found. Please check connection!")
return False
if self.__server_mode:
self.__register_video_source('kinect_depth', depth_source)
self.__register_video_source('kinect_rgb', rgb_source)
else:
self.__vidmemwriter.add_video_source(depth_source, "kinect_depth")
self.__vidmemwriter.add_video_source(rgb_source, "kinect_rgb")
self.__video_sources.append("kinect_depth")
self.__video_sources.append("kinect_rgb")
self.__video_sources.append("kinect")
self.__video_sources.append("kinect_openni")
self.__logger.info("Kinect started")
return True
elif camType == 'kinect' or camType == 'kinect_rgb' or camType == 'kinect_depth':
if self.__use_openni:
self.__logger.info("I'm starting kinect using openni")
import util.openni_kinectvideo as kv
depth_source = kv.OpenNIKinect("depth")
rgb_source = kv.OpenNIKinect("rgb")
try:
depth_source.get_image()
except:
self.__logger.error("Kinect not found. Please check connection!")
return False
else:
self.__logger.info("I'm starting kinect using freenect")
try:
import util.kinectmemwriter
except:
self.__logger.error("Could not load kinectmemwriter module. Check modules.")
return False
depth_source = util.kinectmemwriter.KinectDepthSource()
rgb_source = util.kinectmemwriter.KinectRGBSource()
try:
depth_source.get_image()
except:
self.__logger.error("Kinect not found. Please check connection!")
return False
if self.__server_mode:
self.__register_video_source('kinect_depth', depth_source)
self.__register_video_source('kinect_rgb', rgb_source)
else:
self.__vidmemwriter.add_video_source(depth_source, "kinect_depth")
self.__vidmemwriter.add_video_source(rgb_source, "kinect_rgb")
self.__video_sources.append("kinect_depth")
self.__video_sources.append("kinect_rgb")
self.__video_sources.append("kinect")
self.__logger.info("Kinect started")
return True
elif camType == "naovideo":
self.__logger.debug("I'm starting naovideo")
try:
import util.naovideo as naovideo
except:
self.__logger.error("Could not load naovideo module. Check modules")
return False
#get ip of nao:
#TODO: fix this dirty hack (it should be read from the config file)
naoip = "129.125.178.232"
if ip:
naoip = ip
self.__logger.warn("Using input resolution %s and output resolution %s" % (inputres, outputres))
#use the naovideo module:
if self.__camera != 0 and self.__camera != 1:
self.__camera = 0
try:
naocamsource = naovideo.VideoModule(naoip, inputres, outputres, camera=self.__camera)
naocamsource.get_image()
except:
self.__logger.error("Something went wrong using the camera of the nao (check connection!)")
traceback.print_exc()
return False
if self.__server_mode:
self.__register_video_source('naovideo', naocamsource)
else:
self.__vidmemwriter.add_video_source(naocamsource, "naovideo")
self.__video_sources.append("naovideo")
self.__nao_camera = naocamsource
self.__logger.info("Naovideo started")
return True
else:
self.__logger.warning("Invalid video source specified: %s" % camType)
return False
def __register_video_source(self, name, source):
"""
Register a running video source, for use in server mode.
"""
self.__video_modules[name] = source
self.__last_images[name] = (time.time(), source.get_image())
self.__video_locks[name] = threading.Lock()
def _stop_video_sources(self):
"""
Stop all video sources.
"""
for source in self.__video_sources:
self._stop_video_source(source)
def _stop_video_source(self, video_source):
"""
Stop the specified video source
"""
if not video_source in self.__video_sources:
self.__logger.warning("Not stopping video source %s - not running" % video_source)
return True
if self.__server_mode:
if video_source == "kinect":
if "kinect_rgb" in self.__video_modules:
del self.__video_modules["kinect_rgb"]
del self.__last_images["kinect_rgb"]
del self.__video_locks["kinect_rgb"]
if "kinect_depth" in self.__video_modules:
del self.__video_modules["kinect_depth"]
del self.__last_images["kinect_depth"]
del self.__video_locks["kinect_depth"]
else:
if video_source in self.__video_modules:
if video_source == "naovideo":
self.__video_modules[video_source].unsubscribe()
del self.__video_modules[video_source]
del self.__last_images[video_source]
del self.__video_locks[video_source]
if video_source == "kinect":
import util.kinectvideo
if not self.__server_mode:
self.__vidmemwriter.stop_video_source("kinect_rgb")
self.__vidmemwriter.stop_video_source("kinect_depth")
for src in ['kinect', 'kinect_rgb', 'kinect_depth', 'kinect_openni']:
if src in self.__video_sources:
self.__video_sources.remove(src)
util.kinectvideo.StopKinect()
else:
self.__video_sources.remove(video_source)
if not self.__server_mode:
self.__vidmemwriter.stop_video_source(video_source)
if not self.__video_sources:
self.__logger.info("No video source active. Stopping vidmemwriter.")
self.__vidmemwriter = None
return True
def _check_sources(self):
"""
Check the available video sources. Use the list of active video sources
as a starting point. Then, for each known video source not currently
active, try to start it, and if this succeeds, add it to the list.
"""
avail_sources = []
for source in self.__video_sources:
avail_sources.append(source)
# Try to start each video source known. Naovideo is not in this list,
# because without an explicit request, no IP and port is known so no
# connection can be made.
for source in ["webcam", "kinect"]:
if source in avail_sources:
continue
# Status unknown, try to start and stop
self.__logger.info("Trying to see if source %s is available" % source)
try:
if self._start_video_source(source):
avail_sources.append(source)
self._stop_video_source(source)
except Exception, e:
self.__logger.info("Error starting source %s, (error: %s) must not be available" % (source, str(e)))
return avail_sources
class Brain(object):
"""
Main class that starts the sensors and the behaviors and connects them
togethers
"""
def __init__(self, param_dict):
"""
Initialize brain class: create sensorintegrator, behaviorcontroller and bodycontroller
"""
speed = int(param_dict.get_option('general', 'brain_speed', 10))
self.logger = logging.getLogger('Borg.Brain.Brain')
self.ticker = Ticker(frequency=speed, name="Brain", ignore_delay=True)
starting_behavior = param_dict.get_option('general', 'starting_behavior')
self.sensorintegrator = sensorintegrator.SensorIntegrator(param_dict)
self.logger.info("Sensor Integrator initialiazed")
self.behaviorcontroller = basebehavior.behaviorcontroller.BehaviorController(starting_behavior)
self.logger.info("Behavior Controller initialized")
self.set_brain_speed(speed)
self.bodycontroller = body.bodycontroller.BodyController()
self.bodycontroller.set_config(param_dict)
self.logger.info("Body controller initialized")
self.running = False
def set_brain_speed(self, Hertz = 10):
"""
The speed of the brain is measured in Hertz
"""
self.ticker.set_frequency(Hertz)
self.brain_speed = Hertz
def get_brain_speed(self):
"""
Return the speed of the brain measured in Hertz
"""
return self.brain_speed
def start(self):
"""
This starts the robot brain.
This method needs to be threaded otherwise we cannot stop the brain
"""
self.running = True
self.logger.info("Running brain")
sleep_func = True
if self.bodycontroller.get_nr_pioneers():
if body.bodycontroller.pioneer.ROS_ENABLED:
self.logger.info("ROS enabled in body controller. " \
"Using ros.sleep instead of time.sleep")
sleep_func = body.bodycontroller.pioneer.rospy.sleep
while self.running == True:
self.ticker.tick(sleep_func)
memory = self.sensorintegrator.update()
try:
self.behaviorcontroller.update()
except basebehavior.behaviorcontroller.BehaviorFinished:
self.logger.warn("All behaviors have finished or stopped. Stopping brain")
self.stop()
self.bodycontroller.update()
def stop(self, signum=None, frame=None):
"""
the stop function only works if start is threaded
"""
if self.running:
self.running = False
self.sensorintegrator.stop()
self.bodycontroller.stop()
class Brain(object):
"""
Main class that starts the sensors and the behaviors and connects them
togethers
"""
def __init__(self, param_dict):
"""
Initialize brain class: create sensorintegrator, behaviorcontroller and bodycontroller
"""
speed = int(param_dict.get_option('general', 'brain_speed', 10))
self.logger = logging.getLogger('Borg.Brain.Brain')
self.ticker = Ticker(frequency=speed, name="Brain", ignore_delay=True)
starting_behavior = param_dict.get_option('general', 'starting_behavior')
self.sensorintegrator = sensorintegrator.SensorIntegrator(param_dict)
self.logger.info("Sensor Integrator initialiazed")
self.behaviorcontroller = basebehavior.behaviorcontroller.BehaviorController(starting_behavior)
self.logger.info("Behavior Controller initialized")
self.set_brain_speed(speed)
self.bodycontroller = body.bodycontroller.BodyController()
self.bodycontroller.set_config(param_dict)
self.logger.info("Body controller initialized")
self.running = False
def set_brain_speed(self, Hertz = 10):
"""
The speed of the brain is measured in Hertz
"""
self.ticker.set_frequency(Hertz)
self.brain_speed = Hertz
def get_brain_speed(self):
"""
Return the speed of the brain measured in Hertz
"""
return self.brain_speed
def start(self):
"""
This starts the robot brain.
This method needs to be threaded otherwise we cannot stop the brain
"""
self.running = True
self.logger.info("Running brain")
sleep_func = True
if self.bodycontroller.get_nr_pioneers():
if body.bodycontroller.pioneer.ROS_ENABLED:
self.logger.info("ROS enabled in body controller. " \
"Using ros.sleep instead of time.sleep")
sleep_func = body.bodycontroller.pioneer.rospy.sleep
while self.running == True:
self.ticker.tick(sleep_func)
memory = self.sensorintegrator.update()
try:
self.behaviorcontroller.update()
except basebehavior.behaviorcontroller.BehaviorFinished:
self.logger.warn("All behaviors have finished or stopped. Stopping brain")
self.stop()
self.bodycontroller.update()
def stop(self, signum=None, frame=None):
"""
the stop function only works if start is threaded
"""
if self.running:
self.running = False
self.sensorintegrator.stop()
self.bodycontroller.stop()
