123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450 |
- #!/usr/bin/env python
- #
- # Copyright (c) 2019 Intel Corporation
- #
- # This work is licensed under the terms of the MIT license.
- # For a copy, see <https://opensource.org/licenses/MIT>.
- #
- """
- This module provides a ROS autonomous agent interface to control the ego vehicle via a ROS stack
- """
- import math
- import os
- import subprocess
- import signal
- import threading
- import time
- import numpy
- import carla
- import rospy
- from cv_bridge import CvBridge
- from geometry_msgs.msg import PoseStamped
- from nav_msgs.msg import Odometry, Path
- from rosgraph_msgs.msg import Clock
- from sensor_msgs.msg import Image, PointCloud2, NavSatFix, NavSatStatus, CameraInfo
- from sensor_msgs.point_cloud2 import create_cloud_xyz32
- from std_msgs.msg import Header, String
- import tf
- # pylint: disable=line-too-long
- from carla_msgs.msg import CarlaEgoVehicleStatus, CarlaEgoVehicleInfo, CarlaEgoVehicleInfoWheel, CarlaEgoVehicleControl, CarlaWorldInfo
- # pylint: enable=line-too-long
- from srunner.autoagents.autonomous_agent import AutonomousAgent
- class RosAgent(AutonomousAgent):
- """
- Base class for ROS-based stacks.
- Derive from it and implement the sensors() method.
- Please define TEAM_CODE_ROOT in your environment.
- The stack is started by executing $TEAM_CODE_ROOT/start.sh
- The sensor data is published on similar topics as with the carla-ros-bridge. You can find details about
- the utilized datatypes there.
- This agent expects a roscore to be running.
- """
- speed = None
- current_control = None
- stack_process = None
- timestamp = None
- current_map_name = None
- step_mode_possible = None
- vehicle_info_publisher = None
- global_plan_published = None
- def setup(self, path_to_conf_file):
- """
- setup agent
- """
- self.stack_thread = None
- # get start_script from environment
- team_code_path = os.environ['TEAM_CODE_ROOT']
- if not team_code_path or not os.path.exists(team_code_path):
- raise IOError("Path '{}' defined by TEAM_CODE_ROOT invalid".format(team_code_path))
- start_script = "{}/start.sh".format(team_code_path)
- if not os.path.exists(start_script):
- raise IOError("File '{}' defined by TEAM_CODE_ROOT invalid".format(start_script))
- # set use_sim_time via commandline before init-node
- process = subprocess.Popen(
- "rosparam set use_sim_time true", shell=True, stderr=subprocess.STDOUT, stdout=subprocess.PIPE)
- process.wait()
- if process.returncode:
- raise RuntimeError("Could not set use_sim_time")
- # initialize ros node
- rospy.init_node('ros_agent', anonymous=True)
- # publish first clock value '0'
- self.clock_publisher = rospy.Publisher('clock', Clock, queue_size=10, latch=True)
- self.clock_publisher.publish(Clock(rospy.Time.from_sec(0)))
- # execute script that starts the ad stack (remains running)
- rospy.loginfo("Executing stack...")
- self.stack_process = subprocess.Popen(start_script, shell=True, preexec_fn=os.setpgrp)
- self.vehicle_control_event = threading.Event()
- self.timestamp = None
- self.speed = 0
- self.global_plan_published = False
- self.vehicle_info_publisher = None
- self.vehicle_status_publisher = None
- self.odometry_publisher = None
- self.world_info_publisher = None
- self.map_file_publisher = None
- self.current_map_name = None
- self.tf_broadcaster = None
- self.step_mode_possible = False
- self.vehicle_control_subscriber = rospy.Subscriber(
- '/carla/ego_vehicle/vehicle_control_cmd', CarlaEgoVehicleControl, self.on_vehicle_control)
- self.current_control = carla.VehicleControl()
- self.waypoint_publisher = rospy.Publisher(
- '/carla/ego_vehicle/waypoints', Path, queue_size=1, latch=True)
- self.publisher_map = {}
- self.id_to_sensor_type_map = {}
- self.id_to_camera_info_map = {}
- self.cv_bridge = CvBridge()
- # setup ros publishers for sensors
- # pylint: disable=line-too-long
- for sensor in self.sensors():
- self.id_to_sensor_type_map[sensor['id']] = sensor['type']
- if sensor['type'] == 'sensor.camera.rgb':
- self.publisher_map[sensor['id']] = rospy.Publisher(
- '/carla/ego_vehicle/camera/rgb/' + sensor['id'] + "/image_color", Image, queue_size=1, latch=True)
- self.id_to_camera_info_map[sensor['id']] = self.build_camera_info(sensor)
- self.publisher_map[sensor['id'] + '_info'] = rospy.Publisher(
- '/carla/ego_vehicle/camera/rgb/' + sensor['id'] + "/camera_info", CameraInfo, queue_size=1, latch=True)
- elif sensor['type'] == 'sensor.lidar.ray_cast':
- self.publisher_map[sensor['id']] = rospy.Publisher(
- '/carla/ego_vehicle/lidar/' + sensor['id'] + "/point_cloud", PointCloud2, queue_size=1, latch=True)
- elif sensor['type'] == 'sensor.other.gnss':
- self.publisher_map[sensor['id']] = rospy.Publisher(
- '/carla/ego_vehicle/gnss/' + sensor['id'] + "/fix", NavSatFix, queue_size=1, latch=True)
- elif sensor['type'] == 'sensor.can_bus':
- if not self.vehicle_info_publisher:
- self.vehicle_info_publisher = rospy.Publisher(
- '/carla/ego_vehicle/vehicle_info', CarlaEgoVehicleInfo, queue_size=1, latch=True)
- if not self.vehicle_status_publisher:
- self.vehicle_status_publisher = rospy.Publisher(
- '/carla/ego_vehicle/vehicle_status', CarlaEgoVehicleStatus, queue_size=1, latch=True)
- elif sensor['type'] == 'sensor.hd_map':
- if not self.odometry_publisher:
- self.odometry_publisher = rospy.Publisher(
- '/carla/ego_vehicle/odometry', Odometry, queue_size=1, latch=True)
- if not self.world_info_publisher:
- self.world_info_publisher = rospy.Publisher(
- '/carla/world_info', CarlaWorldInfo, queue_size=1, latch=True)
- if not self.map_file_publisher:
- self.map_file_publisher = rospy.Publisher('/carla/map_file', String, queue_size=1, latch=True)
- if not self.tf_broadcaster:
- self.tf_broadcaster = tf.TransformBroadcaster()
- else:
- raise TypeError("Invalid sensor type: {}".format(sensor['type']))
- # pylint: enable=line-too-long
- def destroy(self):
- """
- Cleanup of all ROS publishers
- """
- if self.stack_process and self.stack_process.poll() is None:
- rospy.loginfo("Sending SIGTERM to stack...")
- os.killpg(os.getpgid(self.stack_process.pid), signal.SIGTERM)
- rospy.loginfo("Waiting for termination of stack...")
- self.stack_process.wait()
- time.sleep(5)
- rospy.loginfo("Terminated stack.")
- rospy.loginfo("Stack is no longer running")
- self.world_info_publisher.unregister()
- self.map_file_publisher.unregister()
- self.vehicle_status_publisher.unregister()
- self.vehicle_info_publisher.unregister()
- self.waypoint_publisher.unregister()
- self.stack_process = None
- rospy.loginfo("Cleanup finished")
- def on_vehicle_control(self, data):
- """
- callback if a new vehicle control command is received
- """
- cmd = carla.VehicleControl()
- cmd.throttle = data.throttle
- cmd.steer = data.steer
- cmd.brake = data.brake
- cmd.hand_brake = data.hand_brake
- cmd.reverse = data.reverse
- cmd.gear = data.gear
- cmd.manual_gear_shift = data.manual_gear_shift
- self.current_control = cmd
- if not self.vehicle_control_event.is_set():
- self.vehicle_control_event.set()
- # After the first vehicle control is sent out, it is possible to use the stepping mode
- self.step_mode_possible = True
- def build_camera_info(self, attributes): # pylint: disable=no-self-use
- """
- Private function to compute camera info
- camera info doesn't change over time
- """
- camera_info = CameraInfo()
- # store info without header
- camera_info.header = None
- camera_info.width = int(attributes['width'])
- camera_info.height = int(attributes['height'])
- camera_info.distortion_model = 'plumb_bob'
- cx = camera_info.width / 2.0
- cy = camera_info.height / 2.0
- fx = camera_info.width / (
- 2.0 * math.tan(float(attributes['fov']) * math.pi / 360.0))
- fy = fx
- camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
- camera_info.D = [0, 0, 0, 0, 0]
- camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
- camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0]
- return camera_info
- def publish_plan(self):
- """
- publish the global plan
- """
- msg = Path()
- msg.header.frame_id = "/map"
- msg.header.stamp = rospy.Time.now()
- for wp in self._global_plan_world_coord:
- pose = PoseStamped()
- pose.pose.position.x = wp[0].location.x
- pose.pose.position.y = -wp[0].location.y
- pose.pose.position.z = wp[0].location.z
- quaternion = tf.transformations.quaternion_from_euler(
- 0, 0, -math.radians(wp[0].rotation.yaw))
- pose.pose.orientation.x = quaternion[0]
- pose.pose.orientation.y = quaternion[1]
- pose.pose.orientation.z = quaternion[2]
- pose.pose.orientation.w = quaternion[3]
- msg.poses.append(pose)
- rospy.loginfo("Publishing Plan...")
- self.waypoint_publisher.publish(msg)
- def sensors(self):
- """
- Define the sensor suite required by the agent
- :return: a list containing the required sensors
- """
- raise NotImplementedError(
- "This function has to be implemented by the derived classes")
- def get_header(self):
- """
- Returns ROS message header
- """
- header = Header()
- header.stamp = rospy.Time.from_sec(self.timestamp)
- return header
- def publish_lidar(self, sensor_id, data):
- """
- Function to publish lidar data
- """
- header = self.get_header()
- header.frame_id = 'ego_vehicle/lidar/{}'.format(sensor_id)
- lidar_data = numpy.frombuffer(
- data, dtype=numpy.float32)
- lidar_data = numpy.reshape(
- lidar_data, (int(lidar_data.shape[0] / 3), 3))
- # we take the oposite of y axis
- # (as lidar point are express in left handed coordinate system, and ros need right handed)
- # we need a copy here, because the data are read only in carla numpy
- # array
- lidar_data = -1.0 * lidar_data
- # we also need to permute x and y
- lidar_data = lidar_data[..., [1, 0, 2]]
- msg = create_cloud_xyz32(header, lidar_data)
- self.publisher_map[sensor_id].publish(msg)
- def publish_gnss(self, sensor_id, data):
- """
- Function to publish gnss data
- """
- msg = NavSatFix()
- msg.header = self.get_header()
- msg.header.frame_id = 'gps'
- msg.latitude = data[0]
- msg.longitude = data[1]
- msg.altitude = data[2]
- msg.status.status = NavSatStatus.STATUS_SBAS_FIX
- # pylint: disable=line-too-long
- msg.status.service = NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS | NavSatStatus.SERVICE_COMPASS | NavSatStatus.SERVICE_GALILEO
- # pylint: enable=line-too-long
- self.publisher_map[sensor_id].publish(msg)
- def publish_camera(self, sensor_id, data):
- """
- Function to publish camera data
- """
- msg = self.cv_bridge.cv2_to_imgmsg(data, encoding='bgra8')
- # the camera data is in respect to the camera's own frame
- msg.header = self.get_header()
- msg.header.frame_id = 'ego_vehicle/camera/rgb/{}'.format(sensor_id)
- cam_info = self.id_to_camera_info_map[sensor_id]
- cam_info.header = msg.header
- self.publisher_map[sensor_id + '_info'].publish(cam_info)
- self.publisher_map[sensor_id].publish(msg)
- def publish_can(self, sensor_id, data):
- """
- publish can data
- """
- if not self.vehicle_info_publisher:
- self.vehicle_info_publisher = rospy.Publisher(
- '/carla/ego_vehicle/vehicle_info', CarlaEgoVehicleInfo, queue_size=1, latch=True)
- info_msg = CarlaEgoVehicleInfo()
- for wheel in data['wheels']:
- wheel_info = CarlaEgoVehicleInfoWheel()
- wheel_info.tire_friction = wheel['tire_friction']
- wheel_info.damping_rate = wheel['damping_rate']
- wheel_info.steer_angle = wheel['steer_angle']
- wheel_info.disable_steering = wheel['disable_steering']
- info_msg.wheels.append(wheel_info)
- info_msg.max_rpm = data['max_rpm']
- info_msg.moi = data['moi']
- info_msg.damping_rate_full_throttle = data['damping_rate_full_throttle']
- info_msg.damping_rate_zero_throttle_clutch_disengaged = data['damping_rate_zero_throttle_clutch_disengaged']
- info_msg.use_gear_autobox = data['use_gear_autobox']
- info_msg.clutch_strength = data['clutch_strength']
- info_msg.mass = data['mass']
- info_msg.drag_coefficient = data['drag_coefficient']
- info_msg.center_of_mass.x = data['center_of_mass']['x']
- info_msg.center_of_mass.y = data['center_of_mass']['y']
- info_msg.center_of_mass.z = data['center_of_mass']['z']
- self.vehicle_info_publisher.publish(info_msg)
- msg = CarlaEgoVehicleStatus()
- msg.header = self.get_header()
- msg.velocity = data['speed']
- self.speed = data['speed']
- # todo msg.acceleration
- msg.control.throttle = self.current_control.throttle
- msg.control.steer = self.current_control.steer
- msg.control.brake = self.current_control.brake
- msg.control.hand_brake = self.current_control.hand_brake
- msg.control.reverse = self.current_control.reverse
- msg.control.gear = self.current_control.gear
- msg.control.manual_gear_shift = self.current_control.manual_gear_shift
- self.vehicle_status_publisher.publish(msg)
- def publish_hd_map(self, sensor_id, data):
- """
- publish hd map data
- """
- roll = -math.radians(data['transform']['roll'])
- pitch = -math.radians(data['transform']['pitch'])
- yaw = -math.radians(data['transform']['yaw'])
- quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
- x = data['transform']['x']
- y = -data['transform']['y']
- z = data['transform']['z']
- if self.odometry_publisher:
- odometry = Odometry()
- odometry.header.frame_id = 'map'
- odometry.header.stamp = rospy.Time.from_sec(self.timestamp)
- odometry.child_frame_id = 'base_link'
- odometry.pose.pose.position.x = x
- odometry.pose.pose.position.y = y
- odometry.pose.pose.position.z = z
- odometry.pose.pose.orientation.x = quat[0]
- odometry.pose.pose.orientation.y = quat[1]
- odometry.pose.pose.orientation.z = quat[2]
- odometry.pose.pose.orientation.w = quat[3]
- odometry.twist.twist.linear.x = self.speed
- odometry.twist.twist.linear.y = 0
- odometry.twist.twist.linear.z = 0
- self.odometry_publisher.publish(odometry)
- if self.world_info_publisher:
- # extract map name
- map_name = os.path.basename(data['map_file'])[:-4]
- if self.current_map_name != map_name:
- self.current_map_name = map_name
- world_info = CarlaWorldInfo()
- world_info.map_name = self.current_map_name
- world_info.opendrive = data['opendrive']
- self.world_info_publisher.publish(world_info)
- if self.map_file_publisher:
- self.map_file_publisher.publish(data['map_file'])
- def use_stepping_mode(self): # pylint: disable=no-self-use
- """
- Overload this function to use stepping mode!
- """
- return False
- def run_step(self, input_data, timestamp):
- """
- Execute one step of navigation.
- """
- self.vehicle_control_event.clear()
- self.timestamp = timestamp
- self.clock_publisher.publish(Clock(rospy.Time.from_sec(timestamp)))
- # check if stack is still running
- if self.stack_process and self.stack_process.poll() is not None:
- raise RuntimeError("Stack exited with: {} {}".format(
- self.stack_process.returncode, self.stack_process.communicate()[0]))
- # publish global plan to ROS once
- if self._global_plan_world_coord and not self.global_plan_published:
- self.global_plan_published = True
- self.publish_plan()
- new_data_available = False
- # publish data of all sensors
- for key, val in input_data.items():
- new_data_available = True
- sensor_type = self.id_to_sensor_type_map[key]
- if sensor_type == 'sensor.camera.rgb':
- self.publish_camera(key, val[1])
- elif sensor_type == 'sensor.lidar.ray_cast':
- self.publish_lidar(key, val[1])
- elif sensor_type == 'sensor.other.gnss':
- self.publish_gnss(key, val[1])
- elif sensor_type == 'sensor.can_bus':
- self.publish_can(key, val[1])
- elif sensor_type == 'sensor.hd_map':
- self.publish_hd_map(key, val[1])
- else:
- raise TypeError("Invalid sensor type: {}".format(sensor_type))
- if self.use_stepping_mode():
- if self.step_mode_possible and new_data_available:
- self.vehicle_control_event.wait()
- # if the stepping mode is not used or active, there is no need to wait here
- return self.current_control
|