|
30 | 30 | from sensor_msgs.point_cloud2 import create_cloud_xyz32
|
31 | 31 | from std_msgs.msg import Header, String
|
32 | 32 | import tf
|
33 |
| -# pylint: disable=line-too-long |
34 |
| -from carla_msgs.msg import CarlaEgoVehicleStatus, CarlaEgoVehicleInfo, CarlaEgoVehicleInfoWheel, CarlaEgoVehicleControl, CarlaWorldInfo |
35 |
| -# pylint: enable=line-too-long |
| 33 | +from carla_msgs.msg import ( |
| 34 | + CarlaEgoVehicleStatus, |
| 35 | + CarlaEgoVehicleInfo, |
| 36 | + CarlaEgoVehicleInfoWheel, |
| 37 | + CarlaEgoVehicleControl, |
| 38 | + CarlaWorldInfo, |
| 39 | +) |
36 | 40 |
|
37 | 41 | from srunner.autoagents.autonomous_agent import AutonomousAgent
|
38 | 42 |
|
@@ -122,42 +126,73 @@ def setup(self, path_to_conf_file):
|
122 | 126 | self.cv_bridge = CvBridge()
|
123 | 127 |
|
124 | 128 | # setup ros publishers for sensors
|
125 |
| - # pylint: disable=line-too-long |
126 | 129 | for sensor in self.sensors():
|
127 |
| - self.id_to_sensor_type_map[sensor['id']] = sensor['type'] |
128 |
| - if sensor['type'] == 'sensor.camera.rgb': |
129 |
| - self.publisher_map[sensor['id']] = rospy.Publisher( |
130 |
| - '/carla/ego_vehicle/camera/rgb/' + sensor['id'] + "/image_color", Image, queue_size=1, latch=True) |
131 |
| - self.id_to_camera_info_map[sensor['id']] = self.build_camera_info(sensor) |
132 |
| - self.publisher_map[sensor['id'] + '_info'] = rospy.Publisher( |
133 |
| - '/carla/ego_vehicle/camera/rgb/' + sensor['id'] + "/camera_info", CameraInfo, queue_size=1, latch=True) |
134 |
| - elif sensor['type'] == 'sensor.lidar.ray_cast': |
135 |
| - self.publisher_map[sensor['id']] = rospy.Publisher( |
136 |
| - '/carla/ego_vehicle/lidar/' + sensor['id'] + "/point_cloud", PointCloud2, queue_size=1, latch=True) |
137 |
| - elif sensor['type'] == 'sensor.other.gnss': |
138 |
| - self.publisher_map[sensor['id']] = rospy.Publisher( |
139 |
| - '/carla/ego_vehicle/gnss/' + sensor['id'] + "/fix", NavSatFix, queue_size=1, latch=True) |
140 |
| - elif sensor['type'] == 'sensor.can_bus': |
| 130 | + self.id_to_sensor_type_map[sensor["id"]] = sensor["type"] |
| 131 | + if sensor["type"] == "sensor.camera.rgb": |
| 132 | + self.publisher_map[sensor["id"]] = rospy.Publisher( |
| 133 | + "/carla/ego_vehicle/camera/rgb/" + sensor["id"] + "/image_color", |
| 134 | + Image, |
| 135 | + queue_size=1, |
| 136 | + latch=True, |
| 137 | + ) |
| 138 | + self.id_to_camera_info_map[sensor["id"]] = self.build_camera_info( |
| 139 | + sensor |
| 140 | + ) |
| 141 | + self.publisher_map[sensor["id"] + "_info"] = rospy.Publisher( |
| 142 | + "/carla/ego_vehicle/camera/rgb/" + sensor["id"] + "/camera_info", |
| 143 | + CameraInfo, |
| 144 | + queue_size=1, |
| 145 | + latch=True, |
| 146 | + ) |
| 147 | + elif sensor["type"] == "sensor.lidar.ray_cast": |
| 148 | + self.publisher_map[sensor["id"]] = rospy.Publisher( |
| 149 | + "/carla/ego_vehicle/lidar/" + sensor["id"] + "/point_cloud", |
| 150 | + PointCloud2, |
| 151 | + queue_size=1, |
| 152 | + latch=True, |
| 153 | + ) |
| 154 | + elif sensor["type"] == "sensor.other.gnss": |
| 155 | + self.publisher_map[sensor["id"]] = rospy.Publisher( |
| 156 | + "/carla/ego_vehicle/gnss/" + sensor["id"] + "/fix", |
| 157 | + NavSatFix, |
| 158 | + queue_size=1, |
| 159 | + latch=True, |
| 160 | + ) |
| 161 | + elif sensor["type"] == "sensor.can_bus": |
141 | 162 | if not self.vehicle_info_publisher:
|
142 | 163 | self.vehicle_info_publisher = rospy.Publisher(
|
143 |
| - '/carla/ego_vehicle/vehicle_info', CarlaEgoVehicleInfo, queue_size=1, latch=True) |
| 164 | + "/carla/ego_vehicle/vehicle_info", |
| 165 | + CarlaEgoVehicleInfo, |
| 166 | + queue_size=1, |
| 167 | + latch=True, |
| 168 | + ) |
144 | 169 | if not self.vehicle_status_publisher:
|
145 | 170 | self.vehicle_status_publisher = rospy.Publisher(
|
146 |
| - '/carla/ego_vehicle/vehicle_status', CarlaEgoVehicleStatus, queue_size=1, latch=True) |
147 |
| - elif sensor['type'] == 'sensor.hd_map': |
| 171 | + "/carla/ego_vehicle/vehicle_status", |
| 172 | + CarlaEgoVehicleStatus, |
| 173 | + queue_size=1, |
| 174 | + latch=True, |
| 175 | + ) |
| 176 | + elif sensor["type"] == "sensor.hd_map": |
148 | 177 | if not self.odometry_publisher:
|
149 | 178 | self.odometry_publisher = rospy.Publisher(
|
150 |
| - '/carla/ego_vehicle/odometry', Odometry, queue_size=1, latch=True) |
| 179 | + "/carla/ego_vehicle/odometry", |
| 180 | + Odometry, |
| 181 | + queue_size=1, |
| 182 | + latch=True, |
| 183 | + ) |
151 | 184 | if not self.world_info_publisher:
|
152 | 185 | self.world_info_publisher = rospy.Publisher(
|
153 |
| - '/carla/world_info', CarlaWorldInfo, queue_size=1, latch=True) |
| 186 | + "/carla/world_info", CarlaWorldInfo, queue_size=1, latch=True |
| 187 | + ) |
154 | 188 | if not self.map_file_publisher:
|
155 |
| - self.map_file_publisher = rospy.Publisher('/carla/map_file', String, queue_size=1, latch=True) |
| 189 | + self.map_file_publisher = rospy.Publisher( |
| 190 | + "/carla/map_file", String, queue_size=1, latch=True |
| 191 | + ) |
156 | 192 | if not self.tf_broadcaster:
|
157 | 193 | self.tf_broadcaster = tf.TransformBroadcaster()
|
158 | 194 | else:
|
159 |
| - raise TypeError("Invalid sensor type: {}".format(sensor['type'])) |
160 |
| - # pylint: enable=line-too-long |
| 195 | + raise TypeError("Invalid sensor type: {}".format(sensor["type"])) |
161 | 196 |
|
162 | 197 | def destroy(self):
|
163 | 198 | """
|
@@ -293,9 +328,12 @@ def publish_gnss(self, sensor_id, data):
|
293 | 328 | msg.longitude = data[1]
|
294 | 329 | msg.altitude = data[2]
|
295 | 330 | msg.status.status = NavSatStatus.STATUS_SBAS_FIX
|
296 |
| - # pylint: disable=line-too-long |
297 |
| - msg.status.service = NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS | NavSatStatus.SERVICE_COMPASS | NavSatStatus.SERVICE_GALILEO |
298 |
| - # pylint: enable=line-too-long |
| 331 | + msg.status.service = ( |
| 332 | + NavSatStatus.SERVICE_GPS |
| 333 | + | NavSatStatus.SERVICE_GLONASS |
| 334 | + | NavSatStatus.SERVICE_COMPASS |
| 335 | + | NavSatStatus.SERVICE_GALILEO |
| 336 | + ) |
299 | 337 | self.publisher_map[sensor_id].publish(msg)
|
300 | 338 |
|
301 | 339 | def publish_camera(self, sensor_id, data):
|
|
0 commit comments