publish hd map data
(self, sensor_id, data)
| 355 | self.vehicle_status_publisher.publish(msg) |
| 356 | |
| 357 | def publish_hd_map(self, sensor_id, data): |
| 358 | """ |
| 359 | publish hd map data |
| 360 | """ |
| 361 | roll = -math.radians(data['transform']['roll']) |
| 362 | pitch = -math.radians(data['transform']['pitch']) |
| 363 | yaw = -math.radians(data['transform']['yaw']) |
| 364 | quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw) |
| 365 | x = data['transform']['x'] |
| 366 | y = -data['transform']['y'] |
| 367 | z = data['transform']['z'] |
| 368 | |
| 369 | if self.odometry_publisher: |
| 370 | |
| 371 | odometry = Odometry() |
| 372 | odometry.header.frame_id = 'map' |
| 373 | odometry.header.stamp = rospy.Time.from_sec(self.timestamp) |
| 374 | odometry.child_frame_id = 'base_link' |
| 375 | odometry.pose.pose.position.x = x |
| 376 | odometry.pose.pose.position.y = y |
| 377 | odometry.pose.pose.position.z = z |
| 378 | |
| 379 | odometry.pose.pose.orientation.x = quat[0] |
| 380 | odometry.pose.pose.orientation.y = quat[1] |
| 381 | odometry.pose.pose.orientation.z = quat[2] |
| 382 | odometry.pose.pose.orientation.w = quat[3] |
| 383 | |
| 384 | odometry.twist.twist.linear.x = self.speed |
| 385 | odometry.twist.twist.linear.y = 0 |
| 386 | odometry.twist.twist.linear.z = 0 |
| 387 | |
| 388 | self.odometry_publisher.publish(odometry) |
| 389 | |
| 390 | if self.world_info_publisher: |
| 391 | # extract map name |
| 392 | map_name = os.path.basename(data['map_file'])[:-4] |
| 393 | if self.current_map_name != map_name: |
| 394 | self.current_map_name = map_name |
| 395 | world_info = CarlaWorldInfo() |
| 396 | world_info.map_name = self.current_map_name |
| 397 | world_info.opendrive = data['opendrive'] |
| 398 | self.world_info_publisher.publish(world_info) |
| 399 | if self.map_file_publisher: |
| 400 | self.map_file_publisher.publish(data['map_file']) |
| 401 | |
| 402 | def use_stepping_mode(self): # pylint: disable=no-self-use |
| 403 | """ |