Merge pull request carla-simulator#87 from carla-simulator/feature/ad…

…d_client

Add ego vehicle and waypoint publisher

.pylintrc

@@ -1,3 +1,3 @@
 [TYPECHECK]
-ignored-modules=numpy,carla
+ignored-modules=numpy,carla,pygame
 disable=logging-format-interpolation,locally-disabled,cyclic-import,too-many-instance-attributes,invalid-name

README.md

@@ -27,13 +27,20 @@ This documentation is for CARLA versions *newer* than 0.9.4.
 
 ## Create a catkin workspace and install carla_ros_bridge package
 
+    #setup folder structure
     mkdir -p ~/carla-ros-bridge/catkin_ws/src
     cd ~/carla-ros-bridge
     git clone https://github.com/carla-simulator/ros-bridge.git
     cd catkin_ws/src
     ln -s ../../ros-bridge
     source /opt/ros/kinetic/setup.bash
     cd ..
+
+    #install required ros-dependencies
+    rosdep update
+    rosdep install --from-path .
+
+    #build
     catkin_make
 
 For more information about configuring a ROS environment see
@@ -50,17 +57,20 @@ Wait for the message:
 
     Waiting for the client to connect...
 
-Then start the ros bridge:
+Then start the ros bridge (choose one option):
 
     export PYTHONPATH=$PYTHONPATH:<path/to/carla/>/PythonAPI/<your_egg_file>
     source ~/carla-ros-bridge/catkin_ws/devel/setup.bash
 
-    #start the ros bridge
+    # Option 1: start the ros bridge
     roslaunch carla_ros_bridge carla_ros_bridge.launch
 
-    #alternatively, you might want to start rviz together with the ros-bridge
+    # Option 2: start the ros bridge together with RVIZ
     roslaunch carla_ros_bridge carla_ros_bridge_with_rviz.launch
 
+    # Option 3: start the ros bridge together with an example ego vehicle
+    roslaunch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch
+
 You can setup the ros bridge configuration [carla_ros_bridge/config/settings.yaml](carla_ros_bridge/config/settings.yaml).
 
 As we have not spawned any vehicle and have not added any sensors in our carla world there would not be any stream of data yet.
@@ -173,6 +183,16 @@ Object information of all vehicles, except the ego-vehicle(s) is published.
 The OPEN Drive map description is published.
 
 
+# Example Ego Vehicle
+
+An example Carla Client to spawn and control an ego vehicle is available. You can find further documentation [here](carla_ego_vehicle/README.md).
+
+
+# Waypoint calculation
+
+To make use of the Carla waypoint calculation a ROS Node is available to get waypoints. You can find further documentation [here](carla_waypoint_publisher/README.md).
+
+
 # ROSBAG recording (not yet tested)
 
 The carla_ros_bridge could also be used to record all published topics into a rosbag:

carla_ego_vehicle/README.md

@@ -0,0 +1,67 @@
+# ROS Ego Vehicle
+
+This package provides two ROS nodes:
+
+- Carla Example Ego Vehicle: A reference client for spawning an ego vehicle
+- Carla ROS Manual Control: a ROS-only manual control
+
+
+## Carla Example Ego Vehicle
+
+The reference Carla client `carla_example_ego_vehicle` can be used to spawn an ego vehicle (role-name: "ego_vehicle") with the following sensors attached to it.
+
+- GNSS
+- LIDAR
+- Cameras (one front-camera + one camera for visualization in carla_ros_manual_control)
+- Collision Sensor
+- Lane Invasion Sensor
+
+Info: To be able to use carla_ros_manual_control a camera with role-name 'view' is required.
+
+If no specific position is set, the ego vehicle is spawned at a random position.
+
+
+### Spawning at specific position
+
+It is possible to (re)spawn the ego vehicle at the specific location by publishing to `/initialpose`.
+
+The preferred way of doing that is using RVIZ:
+
+![Autoware Runtime Manager Settings](../docs/images/rviz_set_start_goal.png)
+
+Selecting a Pose with '2D Pose Estimate' will delete the current ego_vehicle and respawn it at the specified position.
+
+
+### Create your own sensor setup
+
+To setup your own ego vehicle with sensors, follow a similar approach as in `carla_example_ego_vehicle` by subclassing from `CarlaEgoVehicleBase`.
+
+Define sensors with their attributes as described in the Carla Documentation about [Cameras and Sensors](https://github.com/carla-simulator/carla/blob/master/Docs/cameras_and_sensors.md).
+
+The format is a list of dictionaries. One dictionary has the values as follows:
+
+    {
+        'type': '<SENSOR-TYPE>',
+        'role_name': '<NAME>',
+        'x': 0.0, 'y': 0.0, 'z': 0.0, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, # pose of the sensor, relative to the vehicle
+        <ADDITIONAL-SENSOR-ATTRIBUTES>
+    }
+
+## Carla ROS Manual Control
+
+The node `carla_ros_manual_control` is a ROS-only version of the Carla `manual_control.py`. All data is received
+via ROS topics.
+
+Note: To be able to use carla_ros_manual_control a camera with role-name 'view' needs to be spawned by `carla_ego_vehicle`.
+
+
+### Manual steering
+
+In order to steer manually, you might need to disable sending vehicle control commands within another ROS node.
+
+Therefore the manual control is able to publish to `/vehicle_control_manual_override` ([std_msgs/Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html)).
+
+Press `B` to toggle the value.
+
+Note: As sending the vehicle control commands is highly dependent on your setup, you need to implement the subscriber to that topic yourself.
+

carla_ego_vehicle/launch/carla_example_ego_vehicle.launch

@@ -0,0 +1,13 @@
+<!-- -->
+<launch>
+  <arg name='host' default='localhost'/>
+  <arg name='port' default='2000'/>
+  <arg name="vehicle_filter" default="vehicle.*" />
+
+  <param name="/carla/host" value="$(arg host)" />
+  <param name="/carla/port" value="$(arg port)" />
+  <param name="/carla/client/vehicle_filter" value="$(arg vehicle_filter)" />
+
+  <node pkg="carla_ego_vehicle" type="carla_example_ego_vehicle.py" name="carla_example_ego_vehicle" output="screen"/>
+</launch>
+

carla_ego_vehicle/launch/carla_ros_manual_control.launch

@@ -0,0 +1,7 @@
+<!-- -->
+<launch>
+
+  <node pkg="carla_ego_vehicle" type="carla_ros_manual_control.py" name="carla_ros_manual_control" output="screen"/>
+
+</launch>
+

carla_ego_vehicle/package.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>carla_ego_vehicle</name>
+  <version>0.0.0</version>
+  <description>The carla_ego_vehicle package</description>
+  <maintainer email="[email protected]">CARLA Simulator Team</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>rospy</exec_depend>
+  <export>
+  </export>
+</package>

carla_ego_vehicle/setup.py

@@ -0,0 +1,10 @@
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['carla_ego_vehicle'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)
+

carla_ego_vehicle/src/carla_ego_vehicle/carla_ego_vehicle_base.py

@@ -0,0 +1,213 @@
+#!/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>.
+"""
+base class for spawning a Ego Vehicle in ROS
+
+Two modes are available:
+- spawn at random Carla Spawnpoint
+- spawn at the pose read from ROS topic /initialpose
+
+Whenever a pose is received via /initialpose, the vehicle gets respawned at that
+position. If no /initialpose is set at startup, a random spawnpoint is used.
+
+/initialpose might be published via RVIZ '2D Pose Estimate" button.
+"""
+
+from abc import abstractmethod
+
+import sys
+import glob
+import os
+import random
+import math
+import rospy
+from tf.transformations import euler_from_quaternion
+from geometry_msgs.msg import PoseWithCovarianceStamped
+
+# ==============================================================================
+# -- find carla module ---------------------------------------------------------
+# ==============================================================================
+try:
+    sys.path.append(glob.glob('**/carla-*%d.%d-%s.egg' % (
+        sys.version_info.major,
+        sys.version_info.minor,
+        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+    pass
+
+import carla  # pylint: disable=wrong-import-position
+
+# ==============================================================================
+# -- CarlaEgoVehicleBase ------------------------------------------------------------
+# ==============================================================================
+
+
+class CarlaEgoVehicleBase(object):
+    """
+    Handles the spawning of the ego vehicle and its sensors
+
+    Derive from this class and implement method sensors()
+    """
+
+    def __init__(self):
+        rospy.init_node('ego_vehicle')
+        self.host = rospy.get_param('/carla/host', '127.0.0.1')
+        self.port = rospy.get_param('/carla/port', '2000')
+        self.world = None
+        self.player = None
+        self.sensor_actors = []
+        self.actor_filter = rospy.get_param('/carla/client/vehicle_filter', 'vehicle.*')
+        self.actor_spawnpoint = None
+        self.initialpose_subscriber = rospy.Subscriber(
+            "/initialpose", PoseWithCovarianceStamped, self.on_initialpose)
+        rospy.loginfo('listening to server %s:%s', self.host, self.port)
+        rospy.loginfo('using vehicle filter: %s', self.actor_filter)
+
+    def on_initialpose(self, initial_pose):
+        """
+        Callback for /initialpose
+
+        Receiving an initial pose (e.g. from RVIZ '2D Pose estimate') triggers a respawn.
+
+        :return:
+        """
+        self.actor_spawnpoint = initial_pose.pose.pose
+        self.restart()
+
+    def restart(self):
+        """
+        (Re)spawns the vehicle
+
+        Either at a given actor_spawnpoint or at a random Carla spawnpoint
+
+        :return:
+        """
+        # Get vehicle blueprint.
+        blueprint = random.choice(self.world.get_blueprint_library().filter(self.actor_filter))
+        blueprint.set_attribute('role_name', 'ego_vehicle')
+        if blueprint.has_attribute('color'):
+            color = random.choice(blueprint.get_attribute('color').recommended_values)
+            blueprint.set_attribute('color', color)
+        # Spawn the vehicle.
+        if self.actor_spawnpoint:
+            spawn_point = carla.Transform()
+            spawn_point.location.x = self.actor_spawnpoint.position.x
+            spawn_point.location.y = -self.actor_spawnpoint.position.y
+            spawn_point.location.z = self.actor_spawnpoint.position.z + 2  # spawn 2m above ground
+            quaternion = (
+                self.actor_spawnpoint.orientation.x,
+                self.actor_spawnpoint.orientation.y,
+                self.actor_spawnpoint.orientation.z,
+                self.actor_spawnpoint.orientation.w
+            )
+            _, _, yaw = euler_from_quaternion(quaternion)
+            spawn_point.rotation.yaw = -math.degrees(yaw)
+            rospy.loginfo("Spawn at x={} y={} z={} yaw={}".format(spawn_point.location.x,
+                                                                  spawn_point.location.y,
+                                                                  spawn_point.location.z,
+                                                                  spawn_point.rotation.yaw))
+            if self.player is not None:
+                self.destroy()
+            while self.player is None:
+                self.player = self.world.try_spawn_actor(blueprint, spawn_point)
+        else:
+            if self.player is not None:
+                spawn_point = self.player.get_transform()
+                spawn_point.location.z += 2.0
+                spawn_point.rotation.roll = 0.0
+                spawn_point.rotation.pitch = 0.0
+                self.destroy()
+                self.player = self.world.try_spawn_actor(blueprint, spawn_point)
+            while self.player is None:
+                spawn_points = self.world.get_map().get_spawn_points()
+                spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
+                self.player = self.world.try_spawn_actor(blueprint, spawn_point)
+        # Set up the sensors
+        self.sensor_actors = self.setup_sensors(self.sensors())
+
+    def setup_sensors(self, sensors):
+        """
+        Create the sensors defined by the user and attach them to the ego-vehicle
+        :param sensors: list of sensors
+        :return:
+        """
+        actors = []
+        bp_library = self.world.get_blueprint_library()
+        for sensor_spec in sensors:
+            try:
+                bp = bp_library.find(sensor_spec['type'])
+                bp.set_attribute('role_name', str(sensor_spec['role_name']))
+                if sensor_spec['type'].startswith('sensor.camera'):
+                    bp.set_attribute('image_size_x', str(sensor_spec['width']))
+                    bp.set_attribute('image_size_y', str(sensor_spec['height']))
+                    bp.set_attribute('fov', str(sensor_spec['fov']))
+                    sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
+                                                     z=sensor_spec['z'])
+                    sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
+                                                     roll=sensor_spec['roll'],
+                                                     yaw=sensor_spec['yaw'])
+                elif sensor_spec['type'].startswith('sensor.lidar'):
+                    bp.set_attribute('range', '200')
+                    bp.set_attribute('rotation_frequency', '10')
+                    bp.set_attribute('channels', '32')
+                    bp.set_attribute('upper_fov', '15')
+                    bp.set_attribute('lower_fov', '-30')
+                    bp.set_attribute('points_per_second', '500000')
+                    sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
+                                                     z=sensor_spec['z'])
+                    sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'],
+                                                     roll=sensor_spec['roll'],
+                                                     yaw=sensor_spec['yaw'])
+                elif sensor_spec['type'].startswith('sensor.other.gnss'):
+                    sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'],
+                                                     z=sensor_spec['z'])
+                    sensor_rotation = carla.Rotation()
+            except KeyError as e:
+                rospy.logfatal(
+                    "Sensor will not be spawned, because sensor spec is invalid: '{}'".format(e))
+                continue
+
+            # create sensor
+            sensor_transform = carla.Transform(sensor_location, sensor_rotation)
+            sensor = self.world.spawn_actor(bp, sensor_transform,
+                                            attach_to=self.player)
+            actors.append(sensor)
+        return actors
+
+    @abstractmethod
+    def sensors(self):
+        """
+        return a list of sensors attached
+        """
+        return []
+
+    def destroy(self):
+        """
+        destroy the current ego vehicle and its sensors
+        """
+        for i, _ in enumerate(self.sensor_actors):
+            if self.sensor_actors[i] is not None:
+                self.sensor_actors[i].destroy()
+                self.sensor_actors[i] = None
+        self.sensor_actors = []
+
+        if self.player and self.player.is_alive:
+            self.player.destroy()
+        self.player = None
+
+    def run(self):
+        """
+        main loop
+        """
+        client = carla.Client(self.host, self.port)
+        client.set_timeout(2.0)
+        self.world = client.get_world()
+        self.restart()
+        try:
+            rospy.spin()
+        except rospy.ROSInterruptException:
+            pass