Merge pull request #125 from carla-simulator/feature/move_manual_over…

…ride_to_bridge

Feature/move manual override to bridge

README.md

@@ -145,10 +145,22 @@ Reports all vehicles, except the ego vehicle.
 |Topic                                 | Type |
 |--------------------------------------|------|
 | `/carla/<ROLE NAME>/vehicle_control_cmd` (subscriber) | [carla_msgs.CarlaEgoVehicleControl](carla_msgs/msg/CarlaEgoVehicleControl.msg) |
+| `/carla/<ROLE NAME>/vehicle_control_cmd_manual` (subscriber) | [carla_msgs.CarlaEgoVehicleControl](carla_msgs/msg/CarlaEgoVehicleControl.msg) |
+| `/carla/<ROLE NAME>/vehicle_control_manual_override` (subscriber) | [std_msgs.Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html) |
 | `/carla/<ROLE NAME>/vehicle_status` | [carla_msgs.CarlaEgoVehicleStatus](carla_msgs/msg/CarlaEgoVehicleStatus.msg) |
 | `/carla/<ROLE NAME>/vehicle_info` | [carla_msgs.CarlaEgoVehicleInfo](carla_msgs/msg/CarlaEgoVehicleInfo.msg) |
 
-You can stear the ego vehicle from the commandline by publishing to the topic `/carla/<ROLE NAME>/vehicle_control_cmd`.
+There are two modes to control the vehicle.
+
+1. Normal Mode (reading commands from `/carla/<ROLE NAME>/vehicle_control_cmd`)
+1. Manual Mode (reading commands from `/carla/<ROLE NAME>/vehicle_control_cmd_manual`)
+
+This allows to manually override a Vehicle Control Commands published by a software stack. You can toggle between the two modes by publishing to `/carla/<ROLE NAME>/vehicle_control_manual_override`.
+
+[carla_manual_control](carla_manual_control/) makes use of this feature.
+
+
+For testing purposes, you can stear the ego vehicle from the commandline by publishing to the topic `/carla/<ROLE NAME>/vehicle_control_cmd`.
 
 Examples for a ego vehicle with role_name 'ego_vehicle':
 

carla_manual_control/README.md

@@ -13,11 +13,5 @@ To be able to use `carla_manual_control`, some sensors need to be attached to th
 
 ## 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 `/carla/<ego vehicle role name>/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.
+In order to steer manually, press 'B'. This will toggle manual-driving mode within carla_ros_bridge.
 

carla_manual_control/src/carla_manual_control/carla_manual_control.py

@@ -162,7 +162,7 @@ def __init__(self, role_name, hud):
         self.auto_pilot_enable_publisher = rospy.Publisher(
             "/carla/{}/enable_autopilot".format(self.role_name), Bool, queue_size=1)
         self.vehicle_control_publisher = rospy.Publisher(
-            "/carla/{}/vehicle_control_cmd".format(self.role_name), CarlaEgoVehicleControl, queue_size=1)
+            "/carla/{}/vehicle_control_cmd_manual".format(self.role_name), CarlaEgoVehicleControl, queue_size=1)
         self._autopilot_enabled = False
         self._control = CarlaEgoVehicleControl()
         self.set_autopilot(self._autopilot_enabled)

carla_ros_bridge/src/carla_ros_bridge/ego_vehicle.py

@@ -61,10 +61,21 @@ def __init__(self, carla_actor, parent):
                                          append_role_name_topic_postfix=False)
 
         self.vehicle_info_published = False
+        self.vehicle_control_override = False
 
         self.control_subscriber = rospy.Subscriber(
             self.topic_name() + "/vehicle_control_cmd",
-            CarlaEgoVehicleControl, self.control_command_updated)
+            CarlaEgoVehicleControl,
+            lambda data: self.control_command_updated(data, manual_override=False))
+
+        self.manual_control_subscriber = rospy.Subscriber(
+            self.topic_name() + "/vehicle_control_cmd_manual",
+            CarlaEgoVehicleControl,
+            lambda data: self.control_command_updated(data, manual_override=True))
+
+        self.control_override_subscriber = rospy.Subscriber(
+            self.topic_name() + "/vehicle_control_manual_override",
+            Bool, self.control_command_override)
 
         self.enable_autopilot_subscriber = rospy.Subscriber(
             self.topic_name() + "/enable_autopilot",
@@ -181,28 +192,35 @@ def destroy(self):
         self.enable_autopilot_subscriber = None
         super(EgoVehicle, self).destroy()
 
-    def control_command_updated(self, ros_vehicle_control):
+    def control_command_override(self, enable):
+        """
+        Set the vehicle control mode according to ros topic
+        """
+        self.vehicle_control_override = enable.data
+
+    def control_command_updated(self, ros_vehicle_control, manual_override):
         """
         Receive a CarlaEgoVehicleControl msg and send to CARLA
 
-        This function gets called whenever a ROS message is received via
-        '/carla/ego_vehicle/vehicle_control_cmd' topic.
-        The received ROS message is converted into carla.VehicleControl command and
-        sent to CARLA.
+        This function gets called whenever a ROS CarlaEgoVehicleControl is received.
+        If the mode is valid (either normal or manual), the received ROS message is
+        converted into carla.VehicleControl command and sent to CARLA.
         This bridge is not responsible for any restrictions on velocity or steering.
         It's just forwarding the ROS input to CARLA
 
+        :param manual_override: manually override the vehicle control command
         :param ros_vehicle_control: current vehicle control input received via ROS
         :type ros_vehicle_control: carla_msgs.msg.CarlaEgoVehicleControl
         :return:
         """
-        vehicle_control = VehicleControl()
-        vehicle_control.hand_brake = ros_vehicle_control.hand_brake
-        vehicle_control.brake = ros_vehicle_control.brake
-        vehicle_control.steer = ros_vehicle_control.steer
-        vehicle_control.throttle = ros_vehicle_control.throttle
-        vehicle_control.reverse = ros_vehicle_control.reverse
-        self.carla_actor.apply_control(vehicle_control)
+        if manual_override == self.vehicle_control_override:
+            vehicle_control = VehicleControl()
+            vehicle_control.hand_brake = ros_vehicle_control.hand_brake
+            vehicle_control.brake = ros_vehicle_control.brake
+            vehicle_control.steer = ros_vehicle_control.steer
+            vehicle_control.throttle = ros_vehicle_control.throttle
+            vehicle_control.reverse = ros_vehicle_control.reverse
+            self.carla_actor.apply_control(vehicle_control)
 
     def enable_autopilot_updated(self, enable_auto_pilot):
         """