def __init__(self):
"""Initialization."""
# initialize the node
rospy.init_node('thymio_controller' # name of the node
)
self.queue = [0 for i in range(3)]
#Dictionary with the instructions and other parameters
self.name = rospy.get_param('~robot_name')
self.working_path = rospy.get_param('~model_path')
self.debug = bool(int(rospy.get_param('~debug')))
path = rospy.get_param('~debug_path') + 'cnn_control_sensor.log'
if self.debug:
self.logger = CNNController.setup_logger('sensor_controller',
path,
level=logging.DEBUG)
else:
self.logger = CNNController.setup_logger('sensor_controller',
path,
level=logging.NOTSET)
self.logger.info('Robot start moving ' + self.name)
#self.model = self.initializeNetwork()
self.model = tf.keras.models.load_model(self.working_path +
rospy.get_param('~model'))
self.init_publisher_subscribers_camera()
self.init_publisher_subscribers_odometry()
self.init_publisher_subscribers_sensors()
self.sensor = proximity_sensor()
rospy.on_shutdown(self.stop)
# set node update frequency in Hz
self.rate = rospy.Rate(10)
def init_publisher_subscribers_sensors(self):
# create pose subscriber
self.sensor = proximity_sensor()
self.pose_subscriber = rospy.Subscriber(
self.name + '/odom', # name of the topic
Odometry, # message type
self.log_odometry # function that hanldes incoming messages
)
self.proximity_center_subscriber = rospy.Subscriber(
self.name + '/proximity/center', # name of the topic
Range, # message type
self.log_center_range # function that hanldes incoming messages
)
self.proximity_right_subscriber = rospy.Subscriber(
self.name + '/proximity/right', # name of the topic
Range, # message type
self.log_right_range # function that hanldes incoming messages
)
self.proximity_left_subscriber = rospy.Subscriber(
self.name + '/proximity/left', # name of the topic
Range, # message type
self.log_left_range # function that hanldes incoming messages
)
self.proximity_central_right_subscriber = rospy.Subscriber(
self.name + '/proximity/center_right', # name of the topic
Range, # message type
self.
log_central_right_range # function that hanldes incoming messages
)
self.proximity_central_left_subscriber = rospy.Subscriber(
self.name + '/proximity/center_left', # name of the topic
Range, # message type
self.
log_central_left_range # function that hanldes incoming messages
)
self.proximity_rear_right_subscriber = rospy.Subscriber(
self.name + '/proximity/rear_right', # name of the topic
Range, # message type
self.
log_rear_right_range # function that hanldes incoming messages
)
self.proximity_rear_left_subscriber = rospy.Subscriber(
self.name + '/proximity/rear_left', # name of the topic
Range, # message type
self.log_rear_left_range # function that hanldes incoming messages
)
def __init__(self):
rospy.init_node('visual')
self.name = rospy.get_param('~robot_name')
self.working_path = rospy.get_param('~data_path')
self.save_data = bool(int(rospy.get_param('~save_data')))
self.dataset_size = int(rospy.get_param('~dataset_size'))
self.debug = bool(int(rospy.get_param('~debug')))
self.frames_collected = 0
self.previous_metadata = None
self.offset = 0 #number of pictures already saved
#Logss
path = rospy.get_param('~debug_path') + 'interface.log'
if self.debug:
self.logger = interface.setup_logger('sensor_controller',
path,
level=logging.DEBUG)
else:
self.logger = interface.setup_logger('sensor_controller',
path,
level=logging.NOTSET)
self.logger.info('Interface start ' + self.name)
if (os.path.exists('{}/metadata.csv'.format(self.working_path))):
self.previous_metadata = np.loadtxt('{}/metadata.csv'.format(
self.working_path),
delimiter=',')
self.offset = self.previous_metadata[-1][0] + 1
self.pose_x = []
self.pose_y = []
self.init_publisher_subscribers_camera()
self.init_publisher_subscribers_odometry()
self.sensor = proximity_sensor()
self.init_publisher_subscribers_sensors()
self.rate = rospy.Rate(10)
self.subscriber_names = [
"/proximity/left", "/proximity/center_left", "/proximity/center",
"/proximity/center_right", "/proximity/right"
]
self.sensors_names = [
'left', 'center_left', 'center', 'center_right', 'right'
]
self.y = np.empty((self.dataset_size, 1))
self.ang = np.empty((self.dataset_size, 1))
self.id = np.empty((self.dataset_size, 1))
self.metadata = np.empty((self.dataset_size, 3))
self.sensors_data = np.empty((self.dataset_size, 5))
self.frame_count = 0
self.sensors_weight = np.array([-1, -2, 0, 2, 1])
def __init__(self):
"""Initialization."""
# initialize the node
rospy.init_node('thymio_controller' # name of the node
)
#Dictionary with the instructions and other parameters
with open(rospy.get_param(
'~instruction_path')) as f: #Open recorded data
self.actions = json.load(f)
self.random_noise = bool(int(rospy.get_param('~random_noise')))
self.use_sensors = bool(int(rospy.get_param('~use_sensors')))
self.debug = bool(int(rospy.get_param('~debug')))
self.name = rospy.get_param('~robot_name')
#Logss
path = rospy.get_param('~debug_path') + 'control_sensor.log'
if self.debug:
self.logger = ThymioController.setup_logger('sensor_controller',
path,
level=logging.DEBUG)
else:
self.logger = ThymioController.setup_logger('sensor_controller',
path,
level=logging.NOTSET)
self.logger.info('Robot start moving ' + self.name)
#Initialize publisher and subscribers
self.init_publisher_subscribers_sensors()
self.init_publisher_subscribers_odometry()
#Initialize sensors
self.sensor = proximity_sensor()
# log robot name to console
rospy.loginfo('Controlling %s' % self.name)
# tell ros to call stop when the program is terminated
rospy.on_shutdown(self.stop)
self.states = [
"Following instruction", "Colision detected", "Turning away",
"Avoiding colision"
]
self.actual_state = self.states[0]
# set node update frequency in Hz
self.rate = rospy.Rate(10)
def __init__(self):
"""Initialization."""
# initialize the node
rospy.init_node('thymio_controller' # name of the node
)
#Dictionary with the instructions and other parameters
with open(rospy.get_param(
'~instruction_path')) as f: #Open recorded data
self.actions = json.load(f)
self.random_noise = bool(int(rospy.get_param('~random_noise')))
self.use_sensors = bool(int(rospy.get_param('~use_sensors')))
self.debug = bool(int(rospy.get_param('~debug')))
self.name = rospy.get_param('~robot_name')
self.working_path = '/home/usi/catkin_ws/src/robotics/data/' #rospy.get_param('~data_path')
self.save_data = True #bool(int(rospy.get_param('~save_data')))
self.dataset_size = int(rospy.get_param('~dataset_size'))
self.frames_collected = 0
self.offset = 0
#Logss
path = rospy.get_param('~debug_path') + 'control_sensor.log'
if self.debug:
self.logger = ThymioController.setup_logger('sensor_controller',
path,
level=logging.DEBUG)
else:
self.logger = ThymioController.setup_logger('sensor_controller',
path,
level=logging.NOTSET)
self.logger.info('Robot start moving ' + self.name)
#Initialize publisher and subscribers
self.init_publisher_subscribers_sensors()
self.init_publisher_subscribers_odometry()
self.init_publisher_subscribers_camera()
#Initialize sensors
self.sensor = proximity_sensor()
self.sensors_weight = np.array([-1, -2, 0, 2, 1])
self.frame_count = 0
self.y = np.empty((self.dataset_size, 1))
self.ang = np.empty((self.dataset_size, 1))
self.id = np.empty((self.dataset_size, 1))
self.metadata = np.empty((self.dataset_size, 3))
self.sensors_data = np.empty((self.dataset_size, 5))
# log robot name to console
rospy.loginfo('Controlling %s' % self.name)
# tell ros to call stop when the program is terminated
rospy.on_shutdown(self.stop)
self.states = [
"Following instruction", "Colision detected", "Turning away",
"Avoiding colision"
]
self.actual_state = self.states[0]
# set node update frequency in Hz
self.rate = rospy.Rate(10)