Skip to content
Snippets Groups Projects
Commit 72c1cc90 authored by Eloy Retamino's avatar Eloy Retamino
Browse files

[NUIT-212] replaced rospy with roslibpy for ROS communication
parent 2eb78f41
No related branches found
No related tags found
No related merge requests found
...@@ -44,6 +44,9 @@ from future import standard_library ...@@ -44,6 +44,9 @@ from future import standard_library
from hbp_nrp_virtual_coach.requests_client import RequestsClient from hbp_nrp_virtual_coach.requests_client import RequestsClient
from hbp_nrp_virtual_coach.config import Config from hbp_nrp_virtual_coach.config import Config
import roslibpy
try: try:
from urllib2 import HTTPError from urllib2 import HTTPError
except ImportError: except ImportError:
...@@ -79,6 +82,8 @@ class Simulation(object): ...@@ -79,6 +82,8 @@ class Simulation(object):
self.__experiment_id = None self.__experiment_id = None
self.__previous_subtask = None self.__previous_subtask = None
self.__ros_client = None
self.__status_sub = None self.__status_sub = None
self.__error_sub = None self.__error_sub = None
self.__status_callbacks = [] self.__status_callbacks = []
...@@ -164,22 +169,22 @@ class Simulation(object): ...@@ -164,22 +169,22 @@ class Simulation(object):
traceback.print_exc() traceback.print_exc()
return False return False
# pylint: disable=import-error # Connect to rosbridge and create subscribers
try: self.__ros_client = roslibpy.Ros(host=self._get_rosbridge_ws())
import rospy self.__ros_client.run()
from std_msgs.msg import String
from cle_ros_msgs.msg import CLEError if self.__ros_client.is_connected:
# register for status updates, currently only support localhost but in the future roslibpy.Topic(self.__ros_client, self.__config['ros']['status'], 'std_msgs/String')
# we may need to set ROS_MASTER_URI to the simulation backend self.__status_sub.subscribe(self.__on_status)
self.__status_sub = rospy.Subscriber(self.__config['ros']['status'],
String, self.__error_sub = roslibpy.Topic(self.__ros_client, self.__config['ros']['error'],
self.__on_status) 'cle_ros_msgs/CLEError')
self.__error_sub = rospy.Subscriber(self.__config['ros']['error'], self.__error_sub.subscribe(self.__on_error)
CLEError, else:
self.__on_error) self.__ros_client.terminate()
except ImportError: self.__ros_client = None
self.__logger.warn( self.__logger.info('Connection with rosbridge could not be stablished. \
'ROS is not installed, some functionality will be disabled.') Some functionalities will be disabled')
# store server information, experiment_id and the endpoint url for this server/simulation id # store server information, experiment_id and the endpoint url for this server/simulation id
self.__server = server self.__server = server
...@@ -190,6 +195,16 @@ class Simulation(object): ...@@ -190,6 +195,16 @@ class Simulation(object):
self.__logger.info('Ready.') self.__logger.info('Ready.')
return True return True
def _get_rosbridge_ws(self):
"""
Returns the rosbridge websocket address
:return:
"""
rbridge = self.__config['proxy-services']['rosbridge']
if rbridge.find('http://') == 0:
rbridge = rbridge.split('http://')[1]
return 'ws://' + rbridge
def start(self): def start(self):
""" """
Attempt to start the simulation by transitioning to the "started" state. Attempt to start the simulation by transitioning to the "started" state.
...@@ -207,11 +222,6 @@ class Simulation(object): ...@@ -207,11 +222,6 @@ class Simulation(object):
Attempt to stop the simulation by transitioning to the "stopped" state. Attempt to stop the simulation by transitioning to the "stopped" state.
""" """
# unregister (non-status) subscribers before shutting down
if self.__error_sub is not None:
self.__error_sub.unregister()
self.__error_sub = None
self.__set_state('stopped') self.__set_state('stopped')
def register_status_callback(self, callback): def register_status_callback(self, callback):
...@@ -231,7 +241,7 @@ class Simulation(object): ...@@ -231,7 +241,7 @@ class Simulation(object):
# make sure ROS is enabled (this is only a valid check after simulation is created) # make sure ROS is enabled (this is only a valid check after simulation is created)
if not self.__status_sub: if not self.__status_sub:
raise Exception( raise Exception(
"Unable to register status callback, ROS is not available.") "Unable to register status callback, rosbridge connection is not available.")
# make sure the callback is not registered already, warn the user if it is # make sure the callback is not registered already, warn the user if it is
if callback in self.__status_callbacks: if callback in self.__status_callbacks:
...@@ -319,25 +329,22 @@ class Simulation(object): ...@@ -319,25 +329,22 @@ class Simulation(object):
for callback in self.__status_callbacks: for callback in self.__status_callbacks:
callback(status) callback(status)
# if the simulation has stopped, unregister this listener - this mechanism relies on the # if the simulation has stopped, close rosbridge connection - this mechanism relies on the
# simulation state messages always being accurate for now, but this may not be the case # simulation state messages always being accurate for now, but this may not be the case
# we should have a state machine that will check if a simulation is done and shutdown # we should have a state machine that will check if a simulation is done and shutdown
# this simulation instance cleanly # this simulation instance cleanly
if status['state'] in ['stopped', 'halted']: if status['state'] in ['stopped', 'halted']:
# clear all callbacks, this simulation instance should not be reused
self.__status_callbacks[:] = []
self.__logger.info('Simulation has been stopped.')
# simulation complete, no more status events # close rosbridge connection
if self.__status_sub is not None: if self.__ros_client:
self.__status_sub.unregister() self.__ros_client.terminate()
self.__ros_client = None
self.__status_sub = None self.__status_sub = None
# if stop was called by the frontend or timeout, the error sub will still exist
if self.__error_sub is not None:
self.__error_sub.unregister()
self.__error_sub = None self.__error_sub = None
# clear all callbacks, this simulation instance should not be reused
self.__status_callbacks[:] = []
self.__logger.info('Simulation has been stopped.')
def get_state(self): def get_state(self):
""" """
......
...@@ -102,27 +102,6 @@ class VirtualCoach(object): ...@@ -102,27 +102,6 @@ class VirtualCoach(object):
assert isinstance(oidc_token, (string_types, type(None))) assert isinstance(oidc_token, (string_types, type(None)))
assert isinstance(storage_username, (string_types, type(None))) assert isinstance(storage_username, (string_types, type(None)))
# ROS node and logger configuration only if rospy is available
# pylint: disable=import-error
try:
# initialize the common ROS node for all simulations
import rospy
import sys
rospy.init_node('virtual_coach')
# initializing the ROS node overrides the root logger, restore it
console_handler = logging.StreamHandler(sys.stdout)
console_handler.setFormatter(logging.Formatter(logger_format))
logging.root.addHandler(console_handler)
logging.root.setLevel(logging.INFO)
# turn down the rospy logging verbosity to minimum
rospy_logger = logging.getLogger('rospy')
rospy_logger.setLevel(logging.CRITICAL)
except ImportError:
logger.warn('ROS is not installed, some functionality will be disabled.')
# parse and load the config file before any OIDC actions # parse and load the config file before any OIDC actions
self.__config = Config(environment) self.__config = Config(environment)
self.__oidc_username = oidc_username self.__oidc_username = oidc_username
......
...@@ -7,4 +7,5 @@ pandas==1.0.5 ...@@ -7,4 +7,5 @@ pandas==1.0.5
jupyter==1.0.0 jupyter==1.0.0
requests requests
ipykernel==5.3.2 ipykernel==5.3.2
mechanize mechanize
\ No newline at end of file roslibpy==1.1.0
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment