Record speedometer

Author: sergiopaniego

behavior_metrics/configs/default_carla.yml

@@ -22,6 +22,10 @@ Behaviors:
                 BirdEyeView_0:
                     Name: 'bird_eye_view_0'
                     Topic: ''
+            Speedometer:
+                Speedometer_0:
+                    Name: 'speedometer_0'
+                    Topic: '/carla/ego_vehicle/speedometer'
         Actuators:
             CARLA_Motors:
                 Motors_0:
@@ -43,7 +47,7 @@ Behaviors:
             ImageTranform: ''
         Type: 'CARLA'
     Simulation:
-        World: /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_04_anticlockwise.launch
+        World: /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_01_anticlockwise.launch
     Dataset:
         In: '/tmp/my_bag.bag'
         Out: ''

behavior_metrics/configs/default_carla_multiple.yml

@@ -22,6 +22,10 @@ Behaviors:
                 BirdEyeView_0:
                     Name: 'bird_eye_view_0'
                     Topic: ''
+            Speedometer:
+                Speedometer_0:
+                    Name: 'speedometer_0'
+                    Topic: '/carla/ego_vehicle/speedometer'
         Actuators:
             CARLA_Motors:
                 Motors_0:
@@ -34,8 +38,8 @@ Behaviors:
             'brains/CARLA/brain_carla_bird_eye_deep_learning.py',
             'brains/CARLA/brain_carla_bird_eye_deep_learning.py',
             'brains/CARLA/brain_carla_bird_eye_deep_learning_x3.py',
-            'brain_carla_bird_eye_deep_learning_previous_v.py',
-            'brain_carla_bird_eye_deep_learning_previous_v.py'
+            'brains/CARLA/brain_carla_bird_eye_deep_learning_previous_v.py',
+            'brains/CARLA/brain_carla_bird_eye_deep_learning_previous_v.py'
             ]
         PilotTimeCycle: 100
         Parameters:

behavior_metrics/robot/interfaces/speedometer.py

@@ -0,0 +1,87 @@
+import rospy
+from std_msgs.msg import Float32
+import threading
+
+
+def speedometer2Speedometer(speedometer):
+
+    speed = Speedometer()
+
+    speed.data = speedometer.data
+    now = rospy.get_rostime()
+    speed.timeStamp = now.secs + (now.nsecs * 1e-9)
+
+    return speed
+
+
+class Speedometer():
+
+    def __init__(self):
+
+        self.data = 0  # X coord [meters]
+        self.timeStamp = 0  # Time stamp [s]
+
+    def __str__(self):
+        s = "Speedometer: {\n   x: " + str(self.x) + "\n   timeStamp: " + str(self.timeStamp) + "\n}"
+        return s
+
+
+class ListenerSpeedometer:
+    '''
+        ROS Speedometer Subscriber. Speedometer Client to Receive speedometer from ROS nodes.
+    '''
+    def __init__(self, topic):
+        '''
+        ListenerSpeedometer Constructor.
+
+        @param topic: ROS topic to subscribe
+        @type topic: String
+
+        '''
+        self.topic = topic
+        self.data = Speedometer()
+        self.sub = None
+        self.lock = threading.Lock()
+        self.start()
+
+    def __callback(self, speedometer):
+        '''
+        Callback function to receive and save Speedometer.
+
+        @param odom: ROS Odometry received
+
+        @type odom: Odometry
+
+        '''
+        speedometer = speedometer2Speedometer(speedometer)
+
+        self.lock.acquire()
+        self.data = speedometer
+        self.lock.release()
+
+    def stop(self):
+        '''
+        Stops (Unregisters) the client.
+
+        '''
+        self.sub.unregister()
+
+    def start(self):
+        '''
+        Starts (Subscribes) the client.
+
+        '''
+        self.sub = rospy.Subscriber(self.topic, Float32, self.__callback)
+
+    def getSpeedometer(self):
+        '''
+        Returns last Speedometer.
+
+        @return last Speedometer saved
+
+        '''
+        self.lock.acquire()
+        speedometer = self.data
+        self.lock.release()
+
+        return speedometer

behavior_metrics/robot/sensors.py

@@ -16,6 +16,7 @@
 from robot.interfaces.laser import ListenerLaser
 from robot.interfaces.pose3d import ListenerPose3d
 from robot.interfaces.birdeyeview import BirdEyeView
+from robot.interfaces.speedometer import ListenerSpeedometer
 
 __author__ = 'fqez'
 __contributors__ = []
@@ -61,6 +62,12 @@ def __init__(self, sensors_config):
         if bird_eye_view_conf:
             self.bird_eye_view = self.__create_sensor(bird_eye_view_conf, 'bird_eye_view')
 
+        # Load speedometer
+        speedometer_conf = sensors_config.get('Speedometer', None)
+        self.speedometer = None
+        if speedometer_conf:
+            self.speedometer = self.__create_sensor(speedometer_conf, 'speedometer')
+
     def __create_sensor(self, sensor_config, sensor_type):
         """Fill the sensor dictionary with instances of the sensor_type and sensor_config"""
         sensor_dict = {}
@@ -75,6 +82,8 @@ def __create_sensor(self, sensor_config, sensor_type):
                 sensor_dict[name] = ListenerPose3d(topic)
             elif sensor_type == 'bird_eye_view':
                 sensor_dict[name] = BirdEyeView()
+            elif sensor_type == 'speedometer':
+                sensor_dict[name] = ListenerSpeedometer(topic)
 
         return sensor_dict
 
@@ -91,6 +100,8 @@ def __get_sensor(self, sensor_name, sensor_type):
                 sensor = self.pose3d[sensor_name]
             elif sensor_type == 'bird_eye_view':
                 sensor = self.bird_eye_view[sensor_name]
+            elif sensor_type == 'speedometer':
+                sensor = self.speedometer[sensor_name]
         except KeyError:
             return "[ERROR] No existing camera with {} name.".format(sensor_name)
 

behavior_metrics/utils/controller_carla.py

@@ -267,7 +267,7 @@ def record_metrics(self, metrics_record_dir_path, world_counter=None, brain_coun
         self.metrics_record_dir_path = metrics_record_dir_path
         os.mkdir(self.metrics_record_dir_path + self.time_str)
         self.experiment_metrics_bag_filename = self.metrics_record_dir_path + self.time_str + '/' + self.time_str + '.bag'
-        topics = ['/carla/ego_vehicle/odometry', '/carla/ego_vehicle/collision', '/carla/ego_vehicle/lane_invasion', '/clock']
+        topics = ['/carla/ego_vehicle/odometry', '/carla/ego_vehicle/collision', '/carla/ego_vehicle/lane_invasion', '/carla/ego_vehicle/speedometer', '/clock']
         command = "rosbag record -O " + self.experiment_metrics_bag_filename + " " + " ".join(topics) + " __name:=behav_metrics_bag"
         command = shlex.split(command)
         with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: