diff --git a/README.md b/README.md index 982c5424..c0b98969 100644 --- a/README.md +++ b/README.md @@ -145,10 +145,22 @@ Reports all vehicles, except the ego vehicle. |Topic | Type | |--------------------------------------|------| | `/carla//vehicle_control_cmd` (subscriber) | [carla_msgs.CarlaEgoVehicleControl](carla_msgs/msg/CarlaEgoVehicleControl.msg) | +| `/carla//vehicle_control_cmd_manual` (subscriber) | [carla_msgs.CarlaEgoVehicleControl](carla_msgs/msg/CarlaEgoVehicleControl.msg) | +| `/carla//vehicle_control_manual_override` (subscriber) | [std_msgs.Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html) | | `/carla//vehicle_status` | [carla_msgs.CarlaEgoVehicleStatus](carla_msgs/msg/CarlaEgoVehicleStatus.msg) | | `/carla//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//vehicle_control_cmd`. +There are two modes to control the vehicle. + +1. Normal Mode (reading commands from `/carla//vehicle_control_cmd`) +1. Manual Mode (reading commands from `/carla//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//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//vehicle_control_cmd`. Examples for a ego vehicle with role_name 'ego_vehicle': diff --git a/carla_manual_control/README.md b/carla_manual_control/README.md index 0c44175b..466eff14 100644 --- a/carla_manual_control/README.md +++ b/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//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. diff --git a/carla_manual_control/src/carla_manual_control/carla_manual_control.py b/carla_manual_control/src/carla_manual_control/carla_manual_control.py index 38630b30..07d5045e 100755 --- a/carla_manual_control/src/carla_manual_control/carla_manual_control.py +++ b/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) diff --git a/carla_ros_bridge/src/carla_ros_bridge/ego_vehicle.py b/carla_ros_bridge/src/carla_ros_bridge/ego_vehicle.py index 26ae3831..55acef07 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/ego_vehicle.py +++ b/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): """