From a6ba639944c7c16802a0f2dcf238441185f71535 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Mon, 10 Dec 2018 11:46:15 +0100 Subject: [PATCH 1/3] Added Ackermann Drive control interface Besides the plain throttle/brake interface a basic ackermann drive control interface is added. The implemented controller is a cascaded PID controller. The first stage controller controls the speed: - desired input: target speed (from AckermannDrive) - measured input: current speed (from CARLA) - control output: accleration delta The accleration delta is integrated over time into a acceleration target value, which is bound by the provided AckermannDrive desired acceleration and the maximum acceleration values of the vehicle to be controlled. The second stage controller controls the acceleration: - desired input: target acceleration value (the output of the first stage) - measured input: current acceleration (derived from current speed to focus on longitudinal component, slightly average filtered) - control output: pedal delta The pedal delta is integrated over time into a pedal target value, which is bound by the provided maximum acceleration values of the vehicle to be controlled. Finally, the pedal target value is mapped from its interval [-max_decleration; max_acceleration] towards the actual pedal range. The mapping translates pedal target values [-max_decleration; brake_upper_border] to the brake pedal and pedal target values [throttle_lower_border; max_acceleration] to the throttle pedal. The throttle_lower_border is defined by the acceleration impedance of the driving vehicle. The brake_upper_border is defined by the deceleration of the vehicle on laying off the throttle pedal. As a result the pedal target values of the interval [brake_upper_border; throttle_lower_border] translate to a coasting of the vehicle. In addition, some special handling for stopping and reverse were added. To parametrize the controller a dynamic reconfigure interface EgoVehicleControlParameters.cfg was added, the control information is published via EgoVehicleControlInfo.msg and a rqt setup (EgoVehicleControlInfo.perspective, client_with_rqt.launch) is provided. Furthermore, the settings.yaml file is extended to allow the configuration of the vehicle. --- CMakeLists.txt | 9 +- client_with_rqt.launch | 5 + config/EgoVehicleControlInfo.perspective | 477 +++++++++++++++++++++++ config/EgoVehicleControlParameter.cfg | 16 + config/settings.yaml | 23 +- msg/EgoVehicleControlInfo.msg | 34 ++ src/carla_ros_bridge/bridge.py | 6 +- src/carla_ros_bridge/child.py | 4 +- src/carla_ros_bridge/ego_vehicle.py | 447 ++++++++++++++++++++- src/carla_ros_bridge/parent.py | 4 +- src/carla_ros_bridge/physics.py | 337 ++++++++++++++++ src/carla_ros_bridge/transforms.py | 118 +++++- src/carla_ros_bridge/vehicle.py | 6 +- 13 files changed, 1451 insertions(+), 35 deletions(-) create mode 100644 client_with_rqt.launch create mode 100644 config/EgoVehicleControlInfo.perspective create mode 100755 config/EgoVehicleControlParameter.cfg create mode 100644 msg/EgoVehicleControlInfo.msg create mode 100644 src/carla_ros_bridge/physics.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 2288edb9..8d6e5391 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS rospy sensor_msgs tf + dynamic_reconfigure ) catkin_python_setup() @@ -13,15 +14,19 @@ catkin_python_setup() add_message_files( FILES CarlaVehicleControl.msg + EgoVehicleControlInfo.msg ) generate_messages( DEPENDENCIES - sensor_msgs + std_msgs ) -catkin_package() +generate_dynamic_reconfigure_options( + config/EgoVehicleControlParameter.cfg +) +catkin_package() include_directories( diff --git a/client_with_rqt.launch b/client_with_rqt.launch new file mode 100644 index 00000000..f6774f9f --- /dev/null +++ b/client_with_rqt.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/config/EgoVehicleControlInfo.perspective b/config/EgoVehicleControlInfo.perspective new file mode 100644 index 00000000..d4edd8ad --- /dev/null +++ b/config/EgoVehicleControlInfo.perspective @@ -0,0 +1,477 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'rqt_reconfigure/Param': [1], u'rqt_plot/Plot': [3, 2, 1], u'rqt_topic/TopicPlugin': [1], u'rqt_publisher/Publisher': [1]}" + } + }, + "groups": { + "plugin__rqt_msg__Messages__1": { + "keys": {}, + "groups": { + "dock_widget__messages.ui": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Message Type Browser'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_reconfigure__Param__1": { + "keys": {}, + "groups": { + "dock_widget___plugincontainer_top_widget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Dynamic Reconfigure'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000100000002000000ae0000006401ffffffff010000000100')", + "pretty-print": " d " + }, + "_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000001000000020000012c000000640100000009010000000200')", + "pretty-print": " , d " + } + }, + "groups": {} + } + } + }, + "plugin__rqt_image_view__ImageView__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "max_range": { + "type": "repr", + "repr": "u'10'" + }, + "rotate": { + "type": "repr", + "repr": "u'0'" + }, + "smooth_image": { + "type": "repr", + "repr": "u'false'" + }, + "mouse_pub_topic": { + "type": "repr", + "repr": "u'/carla/ego_vehicle/sensor/camera/rgb/image_raw_mouse_left'" + }, + "num_gridlines": { + "type": "repr", + "repr": "u'0'" + }, + "toolbar_hidden": { + "type": "repr", + "repr": "u'false'" + }, + "zoom1": { + "type": "repr", + "repr": "u'false'" + }, + "dynamic_range": { + "type": "repr", + "repr": "u'false'" + }, + "topic": { + "type": "repr", + "repr": "u'/carla/ego_vehicle/sensor/camera/rgb/image_raw'" + }, + "publish_click_location": { + "type": "repr", + "repr": "u'false'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_plot__Plot__1": { + "keys": {}, + "groups": { + "dock_widget__DataPlotWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'MatPlot'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "autoscroll": { + "type": "repr", + "repr": "True" + }, + "plot_type": { + "type": "repr", + "repr": "1" + }, + "topics": { + "type": "repr", + "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/target_speed_abs', u'/carla/ego_vehicle/ego_vehicle_control_info/current_speed_abs']" + }, + "y_limits": { + "type": "repr", + "repr": "[0.0, 10.0]" + }, + "x_limits": { + "type": "repr", + "repr": "[96.72015870599716, 97.72015870599716]" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_plot__Plot__2": { + "keys": {}, + "groups": { + "dock_widget__DataPlotWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'MatPlot (2)'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "autoscroll": { + "type": "repr", + "repr": "True" + }, + "plot_type": { + "type": "repr", + "repr": "1" + }, + "topics": { + "type": "repr", + "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/speed_control_accel_delta', u'/carla/ego_vehicle/ego_vehicle_control_info/speed_control_accel_target', u'/carla/ego_vehicle/ego_vehicle_control_info/current_accel']" + }, + "y_limits": { + "type": "repr", + "repr": "[-3.895254899912703, 4.734989355153215]" + }, + "x_limits": { + "type": "repr", + "repr": "[96.83249355900625, 97.83249355900625]" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_plot__Plot__3": { + "keys": {}, + "groups": { + "dock_widget__DataPlotWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'MatPlot (3)'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "autoscroll": { + "type": "repr", + "repr": "True" + }, + "plot_type": { + "type": "repr", + "repr": "1" + }, + "topics": { + "type": "repr", + "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/accel_control_pedal_delta', u'/carla/ego_vehicle/ego_vehicle_control_info/throttle_lower_border', u'/carla/ego_vehicle/ego_vehicle_control_info/brake_upper_border', u'/carla/ego_vehicle/ego_vehicle_control_info/accel_control_pedal_target']" + }, + "y_limits": { + "type": "repr", + "repr": "[-4.976509219499873, 6.023490780500127]" + }, + "x_limits": { + "type": "repr", + "repr": "[97.05394369999703, 98.05394369999703]" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_graph__RosGraph__1": { + "keys": {}, + "groups": { + "dock_widget__RosGraphUi": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Node Graph'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "graph_type_combo_box_index": { + "type": "repr", + "repr": "u'0'" + }, + "topic_filter_line_edit_text": { + "type": "repr", + "repr": "u'/'" + }, + "filter_line_edit_text": { + "type": "repr", + "repr": "u'/'" + }, + "highlight_connections_check_box_state": { + "type": "repr", + "repr": "u'true'" + }, + "unreachable_check_box_state": { + "type": "repr", + "repr": "u'false'" + }, + "actionlib_check_box_state": { + "type": "repr", + "repr": "u'true'" + }, + "quiet_check_box_state": { + "type": "repr", + "repr": "u'false'" + }, + "dead_sinks_check_box_state": { + "type": "repr", + "repr": "u'false'" + }, + "leaf_topics_check_box_state": { + "type": "repr", + "repr": "u'false'" + }, + "namespace_cluster_check_box_state": { + "type": "repr", + "repr": "u'true'" + }, + "auto_fit_graph_check_box_state": { + "type": "repr", + "repr": "u'true'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_publisher__Publisher__1": { + "keys": {}, + "groups": { + "dock_widget__PublisherWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Message Publisher'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "publishers": { + "type": "repr", + "repr": "u\"[{'topic_name': '/carla/ego_vehicle/ackermann_cmd', 'type_name': 'ackermann_msgs/AckermannDrive', 'enabled': False, 'rate': 1.0, 'expressions': {u'/carla/ego_vehicle/ackermann_cmd/speed': '1'}, 'publisher_id': 0, 'counter': 0}]\"" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_topic__TopicPlugin__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "tree_widget_header_state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000010000000000000000010000000000000000000000000000000000000479000000050101000100000000000000000500000064ffffffff0000008100000003000000050000018800000001000000030000011700000001000000030000007200000001000000030000003d00000001000000030000012b0000000100000003000003e800')", + "pretty-print": " d r = + " + } + }, + "groups": {} + }, + "dock_widget__TopicWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Topic Monitor'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_tf_tree__RosTfTree__2": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "highlight_connections_check_box_state": { + "type": "repr", + "repr": "u'true'" + }, + "auto_fit_graph_check_box_state": { + "type": "repr", + "repr": "u'true'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_tf_tree__RosTfTree__1": { + "keys": {}, + "groups": { + "dock_widget__RosTfTreeUi": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'TF Tree'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "highlight_connections_check_box_state": { + "type": "repr", + "repr": "True" + }, + "auto_fit_graph_check_box_state": { + "type": "repr", + "repr": "True" + } + }, + "groups": {} + } + } + } + } + }, + "mainwindow": { + "keys": { + "geometry": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb0002000000000037000000180000077f000004af00000037000000180000077f000004af00000000000000000780')", + "pretty-print": " 7 7 " + }, + "state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('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')", + "pretty-print": " Drqt_graph__RosGraph__1__RosGraphUi Brqt_msg__Messages__1__messages.ui e r 4 G { x x x x I " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} \ No newline at end of file diff --git a/config/EgoVehicleControlParameter.cfg b/config/EgoVehicleControlParameter.cfg new file mode 100755 index 00000000..2ab6653e --- /dev/null +++ b/config/EgoVehicleControlParameter.cfg @@ -0,0 +1,16 @@ +#!/usr/bin/env python +PACKAGE = "carla_ros_bridge" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("speed_Kp", double_t, 0, "Kp of the speed PID", 0.05, 0, 1.) +gen.add("speed_Ki", double_t, 0, "Ki of the speed PID", 0., 0, 1.) +gen.add("speed_Kd", double_t, 0, "Kd of the speed PID", 0.5, 0, 10.) + +gen.add("accel_Kp", double_t, 0, "Kp of the accel PID", 0.02, 0, 10.) +gen.add("accel_Ki", double_t, 0, "Ki of the accel PID", 0., 0, 10.) +gen.add("accel_Kd", double_t, 0, "Kd of the accel PID", 0.05, 0, 10.) + +exit(gen.generate(PACKAGE, "carla_ros_bridge", "EgoVehicleControlParameter")) \ No newline at end of file diff --git a/config/settings.yaml b/config/settings.yaml index 3bcbc2b8..28a0ac56 100644 --- a/config/settings.yaml +++ b/config/settings.yaml @@ -3,5 +3,24 @@ carla: # the network connection for the python connection to CARLA host: localhost port: 2000 - # the vehicle that acts as ego vehicle for this ros bridge instance - ego_vehicle_role_name: hero + # configuration values for the ego vehicle + ego_vehicle: + # the role name of the vehicle that acts as ego vehicle for this ros bridge instance + # (the vehicle from CARLA is selected which has the attribute 'role_name' set to this value + role_name: hero + # select the ego vehicle control mode: + # pedal: CarlaVehicleControl ROS messages from /carla/ego_vehicle/vehicle_control_cmd are forwarded + # to CARLA (this is the default) + # ackermann: AckermannDrive ROS messages from /carla/ego_vehicle/ackermann_cmd provide the target values, a + # cascaded PID controller tries to achieve + control_mode: ackermann + # override the default values of the pid speed controller + # (only relevant for ackermann control mode) + speed_Kp: 0.05 + speed_Ki: 0.00 + speed_Kd: 0.50 + # override the default values of the pid acceleration controller + # (only relevant for ackermann control mode) + accel_Kp: 0.02 + accel_Ki: 0.00 + accel_Kd: 0.05 \ No newline at end of file diff --git a/msg/EgoVehicleControlInfo.msg b/msg/EgoVehicleControlInfo.msg new file mode 100644 index 00000000..7e4a205c --- /dev/null +++ b/msg/EgoVehicleControlInfo.msg @@ -0,0 +1,34 @@ +# This represents an info message of the ego vehicle + +Header header + +# vehicle maximum values +float32 max_steering_angle +float32 max_speed +float32 max_accel +float32 max_decel + +# the target speed/accel values of the vehicle +float32 target_steering_angle +float32 target_speed +float32 target_speed_abs +float32 target_accel_abs +float32 target_decel_abs + +# the current speed/accel values of the vehicle +float32 current_time_sec +float32 current_speed +float32 current_speed_abs +float32 current_accel + +# the current control state +string status +# speed controller +float32 speed_control_accel_delta +float32 speed_control_accel_target +# acceleration controller +float32 accel_control_pedal_delta +float32 accel_control_pedal_target +# borders for lay off pedal +float32 brake_upper_border +float32 throttle_lower_border diff --git a/src/carla_ros_bridge/bridge.py b/src/carla_ros_bridge/bridge.py index 214d7a0c..9d5d41c8 100755 --- a/src/carla_ros_bridge/bridge.py +++ b/src/carla_ros_bridge/bridge.py @@ -113,16 +113,18 @@ def publish_ros_message(self, topic, msg): topic, type(msg), queue_size=10) self.msgs_to_publish.append((self.publishers[topic], msg)) - def get_param(self, key): + def get_param(self, key, default=None): """ Function (override) to query global parameters passed from the outside. :param key: the key of the parameter :type key: string + :param default: the default value of the parameter to return if key is not found + :type default: string :return: the parameter string :rtype: string """ - return self.params[key] + return self.params.get(key, default) def topic_name(self): """ diff --git a/src/carla_ros_bridge/child.py b/src/carla_ros_bridge/child.py index eb1b4ead..4cc83d97 100644 --- a/src/carla_ros_bridge/child.py +++ b/src/carla_ros_bridge/child.py @@ -90,7 +90,7 @@ def publish_ros_message(self, topic, msg): """ self.parent.publish_ros_message(topic, msg) - def get_param(self, key): + def get_param(self, key, default=None): """ Function (override) to query global parameters passed from the outside. @@ -98,6 +98,8 @@ def get_param(self, key): :param key: the key of the parameter :type key: string + :param default: the default value of the parameter to return if key is not found + :type default: string :return: the parameter string :rtype: string """ diff --git a/src/carla_ros_bridge/ego_vehicle.py b/src/carla_ros_bridge/ego_vehicle.py index 21d64d5d..1d2c8a69 100644 --- a/src/carla_ros_bridge/ego_vehicle.py +++ b/src/carla_ros_bridge/ego_vehicle.py @@ -8,14 +8,25 @@ """ Classes to handle Carla vehicles """ +import sys +import numpy + +from simple_pid import PID + import rospy +from dynamic_reconfigure.server import Server from nav_msgs.msg import Odometry from std_msgs.msg import ColorRGBA +from ackermann_msgs.msg import AckermannDrive from carla import VehicleControl + from carla_ros_bridge.vehicle import Vehicle +import carla_ros_bridge.physics as phys from carla_ros_bridge.msg import CarlaVehicleControl # pylint: disable=no-name-in-module,import-error +from carla_ros_bridge.msg import EgoVehicleControlInfo # pylint: disable=no-name-in-module,import-error +from carla_ros_bridge.cfg import EgoVehicleControlParameterConfig # pylint: disable=no-name-in-module,import-error class EgoVehicle(Vehicle): @@ -24,6 +35,29 @@ class EgoVehicle(Vehicle): Vehicle implementation details for the ego vehicle """ + @staticmethod + def create_actor(carla_actor, parent): + """ + Static factory method to create ego vehicle actors + + :param carla_actor: carla vehicle actor object + :type carla_actor: carla.Vehicle + :param parent: the parent of the new traffic actor + :type parent: carla_ros_bridge.Parent + :return: the created vehicle actor + :rtype: carla_ros_bridge.Vehicle or derived type + """ + ego_vehicle_control_mode = parent.get_param('ego_vehicle').get('control_mode', 'pedal') + if ego_vehicle_control_mode == 'pedal': + return PedalControlVehicle(carla_actor=carla_actor, parent=parent) + elif ego_vehicle_control_mode == 'ackermann': + return AckermannControlVehicle(carla_actor=carla_actor, parent=parent) + else: + raise ValueError( + "Unsupported control mode of the ego vehicle '{}'" + " configured at '/carla/ego_vehicle/control_mode' parameter." + " Only 'pedal' or 'ackermann' allowed!".format(ego_vehicle_control_mode)) + def __init__(self, carla_actor, parent): """ Constructor @@ -33,29 +67,11 @@ def __init__(self, carla_actor, parent): :param parent: the parent of this :type parent: carla_ros_bridge.Parent """ - super(EgoVehicle, self).__init__(carla_actor=carla_actor, parent=parent, topic_prefix="ego_vehicle", append_role_name_topic_postfix=False) - self.control_subscriber = rospy.Subscriber( - self.topic_name() + "/vehicle_control_cmd", - CarlaVehicleControl, self.control_command_updated) - - def destroy(self): - """ - Function (override) to destroy this object. - - Terminate ROS subscription on AckermannDrive control commands. - Finally forward call to super class. - - :return: - """ - rospy.logdebug("Destroy EgoVehicle(id={})".format(self.get_id())) - self.control_subscriber = None - super(EgoVehicle, self).destroy() - def get_marker_color(self): """ Function (override) to return the color for marker messages. @@ -89,6 +105,42 @@ def send_object_msg(self): self.publish_ros_message(self.topic_name() + "/odometry", odometry) + +class PedalControlVehicle(EgoVehicle): + + """ + Vehicle implementation details for the ego vehicle in pedal control variant + """ + + def __init__(self, carla_actor, parent): + """ + Constructor + + :param carla_actor: carla actor object + :type carla_actor: carla.Actor + :param parent: the parent of this + :type parent: carla_ros_bridge.Parent + """ + super(PedalControlVehicle, self).__init__(carla_actor=carla_actor, + parent=parent) + + self.control_subscriber = rospy.Subscriber( + self.topic_name() + "/vehicle_control_cmd", + CarlaVehicleControl, self.control_command_updated) + + def destroy(self): + """ + Function (override) to destroy this object. + + Terminate ROS subscription on CarlaVehicleControl commands. + Finally forward call to super class. + + :return: + """ + rospy.logdebug("Destroy PedalControlVehicle(id={})".format(self.get_id())) + self.control_subscriber = None + super(PedalControlVehicle, self).destroy() + def control_command_updated(self, ros_vehicle_control): """ Receive a CarlaVehicleControl msg and send to CARLA @@ -110,5 +162,362 @@ def control_command_updated(self, ros_vehicle_control): vehicle_control.steer = ros_vehicle_control.steer vehicle_control.throttle = ros_vehicle_control.throttle vehicle_control.reverse = ros_vehicle_control.reverse - + # send control command out self.carla_actor.apply_control(vehicle_control) + + +class AckermannControlVehicle(EgoVehicle): + + """ + Vehicle implementation details for the ego vehicle in ackermann control variant + """ + + def __init__(self, carla_actor, parent): + """ + Constructor + + :param carla_actor: carla actor object + :type carla_actor: carla.Actor + :param parent: the parent of this + :type parent: carla_ros_bridge.Parent + """ + super(AckermannControlVehicle, self).__init__(carla_actor=carla_actor, + parent=parent) + + # control info + self.info = EgoVehicleControlInfo() + + # maximum values + self.info.max_steering_angle = phys.get_vehicle_max_steering_angle( + self.carla_actor) + self.info.max_speed = phys.get_vehicle_max_speed( + self.carla_actor) + self.info.max_accel = phys.get_vehicle_max_acceleration( + self.carla_actor) + self.info.max_decel = phys.get_vehicle_max_deceleration( + self.carla_actor) + + # target values + self.info.target_steering_angle = 0. + self.info.target_speed = 0. + self.info.target_speed_abs = 0. + self.info.target_accel_abs = 0. + self.info.target_decel_abs = 0. + + # current values + self.info.current_time_sec = self.get_current_ros_time().to_sec() + self.info.current_speed = 0. + self.info.current_speed_abs = 0. + self.info.current_accel = 0. + + # control values + self.info.status = '' + self.info.speed_control_accel_delta = 0. + self.info.speed_control_accel_target = 0. + self.info.accel_control_pedal_delta = 0. + self.info.accel_control_pedal_target = 0. + self.info.brake_upper_border = 0. + self.info.throttle_lower_border = 0. + + # the carla vehicle control message + self.vehicle_control = VehicleControl() + + # PID controller + # the controller has to run with the simulation time, not with real-time + sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access + lambda: AckermannControlVehicle.get_current_ros_time(self).to_sec()) + # we might want to use a PID controller to reach the final target speed + self.speed_controller = PID(Kp=0.0, + Ki=0.0, + Kd=0.0, + sample_time=0.05, + output_limits=(-1., 1.)) + self.accel_controller = PID(Kp=0.0, + Ki=0.0, + Kd=0.0, + sample_time=0.05, + output_limits=(-1, 1)) + + self.reconfigure_server = Server( + EgoVehicleControlParameterConfig, + namespace=self.topic_name(), + callback=(lambda config, level: AckermannControlVehicle.reconfigure_pid_parameters( + self, config, level))) + + # ROS subsriber + self.control_subscriber = rospy.Subscriber( + self.topic_name() + "/ackermann_cmd", + AckermannDrive, self.ackermann_command_updated) + + def destroy(self): + """ + Function (override) to destroy this object. + + Terminate ROS subscription on AckermannDrive commands. + Finish the PID controllers. + Destroy the reconfiguration server + Finally forward call to super class. + + :return: + """ + rospy.logdebug("Destroy EgoVehicleAckermannControl(id={})".format(self.get_id())) + self.control_subscriber = None + self.speed_controller = None + self.accel_controller = None + # first cleanup the server (otherwise leaves a mess behind ;-) + self.reconfigure_server.set_service.shutdown() + self.reconfigure_server = None + super(AckermannControlVehicle, self).destroy() + + def update(self): + """ + Function (override) to update this object. + + On update ego vehicle calculates and sends the new values for VehicleControl() + + :return: + """ + super(AckermannControlVehicle, self).update() + self.vehicle_control_cycle() + self.send_ego_vehicle_control_info_msg() + + def reconfigure_pid_parameters(self, ego_vehicle_control_parameter, dummy_level): + """ + Callback for dynamic reconfigure call to set the PID parameters + + :param ego_vehicle_control_parameter: + :type ego_vehicle_control_parameter: carla_ros_bridge.cfg.EgoVehicleControlParameterConfig + + """ + rospy.loginfo("Reconfigure Request: " + "speed ({speed_Kp}, {speed_Ki}, {speed_Kd})," + "accel ({accel_Kp}, {accel_Ki}, {accel_Kd})," + "".format(**ego_vehicle_control_parameter)) + self.speed_controller.tunings = ( + ego_vehicle_control_parameter['speed_Kp'], + ego_vehicle_control_parameter['speed_Ki'], + ego_vehicle_control_parameter['speed_Kd'] + ) + self.accel_controller.tunings = ( + ego_vehicle_control_parameter['accel_Kp'], + ego_vehicle_control_parameter['accel_Ki'], + ego_vehicle_control_parameter['accel_Kd'] + ) + return ego_vehicle_control_parameter + + def send_ego_vehicle_control_info_msg(self): + """ + Function to send carla_ros_bridge.msg.EgoVehicleControlInfo message. + + :return: + """ + self.info.header = self.get_msg_header() + self.publish_ros_message( + self.topic_name() + "/ego_vehicle_control_info", self.info) + + def ackermann_command_updated(self, ros_ackermann_drive): + """ + Convert a Ackerman drive msg into carla control msg + + 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 brigde is not responsible for any restrictions on velocity or steering. + It's just forwarding the ROS input to CARLA + + :param ros_vehicle_control: + :type ros_vehicle_control: carla_ros_bridge.msg.CarlaVehicleControl + :return: + """ + # set target values + self.set_target_steering_angle(ros_ackermann_drive.steering_angle) + self.set_target_speed(ros_ackermann_drive.speed) + self.set_target_accel(ros_ackermann_drive.acceleration) + + def set_target_steering_angle(self, target_steering_angle): + """ + set target sterring angle + """ + if abs(target_steering_angle) > self.info.max_steering_angle: + rospy.logerr("Max steering angle reached, clipping value") + self.info.target_steering_angle = numpy.clip( + target_steering_angle, -self.info.max_steering_angle, self.info.max_steering_angle) + else: + self.info.target_steering_angle = target_steering_angle + + def set_target_speed(self, target_speed): + """ + set target speed + """ + if abs(target_speed) > self.info.max_speed: + rospy.logerr("Max speed reached, clipping value") + self.info.target_speed = numpy.clip( + target_speed, -self.info.max_speed, self.info.max_speed) + else: + self.info.target_speed = target_speed + self.info.target_speed_abs = abs(self.info.target_speed) + + def set_target_accel(self, target_accel): + """ + set target accel + """ + epsilon = 0.00001 + # per definition of ackermann drive: if zero, then use max value + if abs(target_accel) < epsilon: + self.info.target_accel_abs = self.info.max_accel + self.info.target_decel_abs = self.info.max_decel + else: + self.info.target_accel_abs = numpy.clip( + abs(target_accel), 0, self.info.max_accel) + self.info.target_decel_abs = numpy.clip( + abs(target_accel), 0, self.info.max_decel) + + def vehicle_control_cycle(self): + """ + Perform a vehicle control cycle and sends out carla.VehicleControl message + """ + # update current values + # we calculate the acceleration on ourselves, because we are interested only in + # the acceleration regarding our driving direction + # in addition we just filter + current_time_sec = self.get_current_ros_time().to_sec() + delta_time = current_time_sec - self.info.current_time_sec + current_speed = phys.get_vehicle_speed(self.carla_actor) + if delta_time > 0: + delta_speed = current_speed - self.info.current_speed + current_accel = delta_speed / delta_time + # average filter + self.info.current_accel = (self.info.current_accel * 4 + current_accel) / 5 + self.info.current_time_sec = current_time_sec + self.info.current_speed = current_speed + self.info.current_speed_abs = abs(current_speed) + + # perform actual control + self.control_steering() + self.control_stop_and_reverse() + self.run_pid_control_loop() + if not self.vehicle_control.hand_brake: + self.update_drive_vehicle_control_command() + + # send control command out + self.carla_actor.apply_control(self.vehicle_control) + + def control_steering(self): + """ + Basic steering control + """ + self.vehicle_control.steer = self.info.target_steering_angle / \ + self.info.max_steering_angle + + def control_stop_and_reverse(self): + """ + Handle stop and switching to reverse gear + """ + # from this velocity on it is allowed to switch to reverse gear + standing_still_epsilon = 0.1 + # from this velocity on hand brake is turned on + full_stop_epsilon = 0.00001 + + # auto-control of hand-brake and reverse gear + self.vehicle_control.hand_brake = False + if self.info.current_speed_abs < standing_still_epsilon: + # standing still, change of driving direction allowed + self.info.status = "standing" + if self.info.target_speed < 0: + if not self.vehicle_control.reverse: + rospy.loginfo( + "VehicleControl: Change of driving direction to reverse") + self.vehicle_control.reverse = True + elif self.info.target_speed > 0: + if self.vehicle_control.reverse: + rospy.loginfo( + "VehicleControl: Change of driving direction to forward") + self.vehicle_control.reverse = False + if self.info.target_speed_abs < full_stop_epsilon: + self.info.status = "full stop" + self.vehicle_control.hand_brake = True + self.vehicle_control.brake = 1.0 + self.vehicle_control.throttle = 0.0 + self.set_target_speed(0.) + self.info.current_speed = 0. + self.info.current_speed_abs = 0. + self.info.current_accel = 0. + self.info.speed_control_accel_target = 0. + self.info.accel_control_pedal_target = 0. + + elif numpy.sign(self.info.current_speed) * numpy.sign(self.info.target_speed) == -1: + # requrest for change of driving direction + # first we have to come to full stop before changing driving + # direction + rospy.loginfo("VehicleControl: Request change of driving direction." + " v_current={} v_desired={}" + " Set desired speed to 0".format(self.info.current_speed, + self.info.target_speed)) + self.set_target_speed(0.) + + def run_pid_control_loop(self): + """ + Run the PID control loop for the speed + """ + self.speed_controller.setpoint = self.info.target_speed_abs + self.info.speed_control_accel_delta = self.speed_controller( + self.info.current_speed_abs) + self.info.speed_control_accel_target = numpy.clip( + self.info.speed_control_accel_target + self.info.speed_control_accel_delta, + -self.info.target_decel_abs, + self.info.target_accel_abs) + + self.accel_controller.setpoint = self.info.speed_control_accel_target + self.info.accel_control_pedal_delta = self.accel_controller( + self.info.current_accel) + self.info.accel_control_pedal_target = numpy.clip( + self.info.accel_control_pedal_target + self.info.accel_control_pedal_delta, + -self.info.max_decel, + self.info.max_accel) + + def update_drive_vehicle_control_command(self): + """ + Apply the current speed_control_target value to throttle/brake commands + """ + + # the driving impedance moves the 'zero' acceleration border + # Interpretation: To reach a zero acceleration the throttle has to pushed + # down for a certain amount + self.info.throttle_lower_border = phys.get_vehicle_driving_impedance_acceleration( + self.carla_actor, self.vehicle_control.reverse) + # the engine lay off acceleration defines the size of the coasting area + # Interpretation: The engine already prforms braking on its own; + # therefore pushing the brake is not required for small decelerations + self.info.brake_upper_border = self.info.throttle_lower_border + \ + phys.get_vehicle_lay_off_engine_acceleration(self.carla_actor) + + if self.info.accel_control_pedal_target > self.info.throttle_lower_border: + self.info.status = "accelerating" + self.vehicle_control.brake = 0.0 + # the value has to be normed to max_accel + # be aware: is not required to take throttle_lower_border into the scaling factor, + # because that border is in reality a shift of the coordinate system + # the global maximum acceleration can practically not be reached anymore because of + # driving impedance + self.vehicle_control.throttle = ( + (self.info.accel_control_pedal_target - self.info.throttle_lower_border) / + abs(self.info.max_accel)) + elif self.info.accel_control_pedal_target > self.info.brake_upper_border: + self.info.status = "coasting" + # no control required + self.vehicle_control.brake = 0.0 + self.vehicle_control.throttle = 0.0 + else: + self.info.status = "braking" + # braking required + self.vehicle_control.brake = ( + (self.info.brake_upper_border - self.info.accel_control_pedal_target) / + abs(self.info.max_decel)) + self.vehicle_control.throttle = 0.0 + + # finally clip the final control output (should actually never happen) + self.vehicle_control.brake = numpy.clip( + self.vehicle_control.brake, 0., 1.) + self.vehicle_control.throttle = numpy.clip( + self.vehicle_control.throttle, 0., 1.) diff --git a/src/carla_ros_bridge/parent.py b/src/carla_ros_bridge/parent.py index ca304f49..25129831 100644 --- a/src/carla_ros_bridge/parent.py +++ b/src/carla_ros_bridge/parent.py @@ -209,7 +209,7 @@ def publish_ros_message(self, topic, msg): "If this error becomes visible the class hierarchy is somehow broken") @abstractmethod - def get_param(self, key): + def get_param(self, key, default=None): """ Pure virtual function to query global parameters passed from the outside. @@ -219,6 +219,8 @@ def get_param(self, key): :param key: the key of the parameter :type key: string + :param default: the default value of the parameter to return if key is not found + :type default: string :return: the parameter string :rtype: string """ diff --git a/src/carla_ros_bridge/physics.py b/src/carla_ros_bridge/physics.py new file mode 100644 index 00000000..8dbd6022 --- /dev/null +++ b/src/carla_ros_bridge/physics.py @@ -0,0 +1,337 @@ +""" +Tool functions to calculate vehicle physics +""" + +import math +import numpy + +import carla_ros_bridge.transforms as trans + + +def get_vector_length_squared(carla_vector): + """ + Calculate the squared length of a carla_vector + + :param carla_vector: the carla vector + :type carla_vector: carla.Vector3D + :return: squared vector length + :rtype: float64 + """ + return carla_vector.x * carla_vector.x + \ + carla_vector.y * carla_vector.y + \ + carla_vector.z * carla_vector.z + + +def get_vehicle_speed_squared(carla_vehicle): + """ + Get the squared speed of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: squared speed of a carla vehicle [(m/s)^2] + :rtype: float64 + """ + return get_vector_length_squared(carla_vehicle.get_velocity()) + + +def get_vehicle_speed_abs(carla_vehicle): + """ + Get the absolute speed of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: speed of a carla vehicle [m/s >= 0] + :rtype: float64 + """ + speed = math.sqrt(get_vehicle_speed_squared(carla_vehicle)) + return speed + + +def get_vehicle_acceleration_abs(carla_vehicle): + """ + Get the absolute acceleration of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: vehicle acceleration value [m/s^2 >=0] + :rtype: float64 + """ + return math.sqrt(get_vector_length_squared(carla_vehicle.get_acceleration())) + + +def get_vehicle_driving_direction_sign(carla_vehicle): + """ + Get the driving direction of the carla vehicle as sign + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: sign of the driving direction of a carla vehicle {-1.0; 1.0} + :rtype: float64 + """ + vector_looking_forward = trans.carla_rotation_to_directional_numpy_vector( + carla_vehicle.get_transform().rotation) + velocity_vector = trans.carla_velocity_to_numpy_vector( + carla_vehicle.get_velocity()) + dot_product = numpy.dot(vector_looking_forward, velocity_vector) + if dot_product < 0: + driving_direction_sign = -1.0 + else: + driving_direction_sign = 1.0 + return driving_direction_sign + + +def get_vehicle_speed(carla_vehicle): + """ + Get the signed speed of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: speed of a carla vehicle [m/s >= 0] + :rtype: float64 + """ + speed = get_vehicle_speed_abs(carla_vehicle) + # below a certain epsilon around zero, the sign detection is not reliable + # @todo get the driving direction directly from CARLA + speed_sign_detection_epsilon = 0.01 + if speed > speed_sign_detection_epsilon: + speed *= get_vehicle_driving_direction_sign(carla_vehicle) + return speed + + +def get_vehicle_mass(carla_vehicle): + """ + Get the mass of a carla vehicle (defaults to 1500kg) + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: mass of a carla vehicle [kg] + :rtype: float64 + """ + mass = carla_vehicle.attributes.get( + 'mass', 1500.0) + + return mass + + +def get_acceleration_of_gravity(dummy_carla_vehicle): + """ + Get the acceleration of gravity for a carla vehicle + (for the moment constant at 9.81 m/s^2) + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: acceleration of gravity [m/s^2] + :rtype: float64 + """ + acceleration = 9.81 + + return acceleration + + +def get_weight_force(carla_vehicle): + """ + Get the weight of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: weight of the vehicle [N] + :rtype: float64 + """ + weight = get_vehicle_mass(carla_vehicle) * \ + get_acceleration_of_gravity(carla_vehicle) + + return weight + + +def get_vehicle_max_steering_angle(carla_vehicle): + """ + Get the maximum steering angle of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: maximum steering angle [radians] + :rtype: float64 + """ + # 80 degrees is the default max steering angle of a car + max_steering_angle = carla_vehicle.attributes.get( + 'max_steering_angle', math.radians(80)) + + return max_steering_angle + + +def get_vehicle_max_speed(carla_vehicle): + """ + Get the maximum speed of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: maximum speed [m/s] + :rtype: float64 + """ + # 180 km/h is the default max speed of a car + max_speed = carla_vehicle.attributes.get( + 'max_speed', 180.0 / 3.6) + + return max_speed + + +def get_vehicle_max_acceleration(carla_vehicle): + """ + Get the maximum acceleration of a carla vehicle + + default: 3.0 m/s^2: 0-100 km/h in 9.2 seconds + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: maximum acceleration [m/s^2 > 0] + :rtype: float64 + """ + max_acceleration = carla_vehicle.attributes.get( + 'max_acceleration', 3.0) + + return max_acceleration + + +def get_vehicle_max_deceleration(carla_vehicle): + """ + Get the maximum deceleration of a carla vehicle + + default: 8 m/s^2 + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: maximum deceleration [m/s^2 > 0] + :rtype: float64 + """ + max_deceleration = carla_vehicle.attributes.get( + 'max_deceleration', 8.0) + + return max_deceleration + + +def get_aerodynamic_drag_force(carla_vehicle): + """ + Calculate the aerodynamic drag force of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: aerodynamic drag force [N] + :rtype: float64 + """ + # see also https://en.wikipedia.org/wiki/Automobile_drag_coefficient + default_aerodynamic_drag_coefficient = 0.3 + default_drag_reference_area = 2.37 + drag_area = carla_vehicle.attributes.get( + 'drag_area', + default_aerodynamic_drag_coefficient * default_drag_reference_area) + rho_air_25 = 1.184 + speed_squared = get_vehicle_speed_squared(carla_vehicle) + + aerodynamic_drag_force = 0.5 * drag_area * rho_air_25 * speed_squared + return aerodynamic_drag_force + + +def get_rolling_resistance_force(carla_vehicle): + """ + Calculate the rolling resistance force of a carla vehicle + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: rolling resistance force [N] + :rtype: float64 + """ + # usually somewhere between 0.007 to 0.014 for car tyres + # and between 0.0025 to 0.005 for bycicle tyres + # see also https://en.wikipedia.org/wiki/Rolling_resistance + rolling_resistance_coefficient = carla_vehicle.attributes.get( + 'rolling_resistance_coefficient', 0.01) + normal_force = get_weight_force(carla_vehicle) + + rolling_resistance_force = rolling_resistance_coefficient * normal_force + + return rolling_resistance_force + + +def get_engine_brake_force(dummy_carla_vehicle): + """ + Calculate the engine brake force of a carla vehicle if the gas pedal would be layed off + + As this heavily depends on the engine, the current gear and velocity, this is not + trivial to calculate. Maybe one can get this from within Unreal to the outside, + to enable better vehicle control. + For the moment we just put a constant force. + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: engine braking force [N] + :rtype: float64 + """ + return 500.0 + + +def get_slope_force(carla_vehicle): + """ + Calculate the force of a carla vehicle faces when driving on a slope. + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: slope force [N, >0 uphill, <0 downhill] + :rtype: float64 + """ + dummy_roll, pitch, dummy_yaw = trans.carla_rotation_to_RPY( + carla_vehicle.get_transform().rotation) + slope_force = get_acceleration_of_gravity( + carla_vehicle) * get_vehicle_mass(carla_vehicle) * math.sin(pitch) + return slope_force + + +def get_vehicle_lay_off_engine_acceleration(carla_vehicle): + """ + Calculate the acceleration a carla vehicle faces by the engine on lay off + + This respects the following forces: + - engine brake force + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :return: acceleration the vehicle [m/s^2 < 0] + :rtype: float64 + """ + return -get_engine_brake_force(carla_vehicle) / get_vehicle_mass(carla_vehicle) + + +def get_vehicle_driving_impedance_acceleration(carla_vehicle, reverse): + """ + Calculate the acceleration a carla vehicle faces by the driving impedance + + This respects the following forces: + - rolling resistance force + - aerodynamic drag force + - slope force + + :param carla_vehicle: the carla vehicle + :type carla_vehicle: carla.Vehicle + :param reverse: `True' if the vehicle is driving in reverse direction + :type reverse: boolean + :return: acceleration the vehicle [m/s^2 <= 0 on flat surface] + :rtype: float64 + """ + # taking the following forumlar as basis + # + # mass * acceleration = rolling_resitance_force + aerodynamic_drag_force + # + # deceleration = -(rolling_resitance_force + aerodynamic_drag_force)/mass + # + # future enhancements: incorporate also the expected motor braking force + # + rolling_resistance_force = get_rolling_resistance_force(carla_vehicle) + aerodynamic_drag_force = get_aerodynamic_drag_force(carla_vehicle) + slope_force = get_slope_force(carla_vehicle) + if reverse: + slope_force = -slope_force + deceleration = -(rolling_resistance_force + + aerodynamic_drag_force + + slope_force) / \ + get_vehicle_mass(carla_vehicle) + + return deceleration diff --git a/src/carla_ros_bridge/transforms.py b/src/carla_ros_bridge/transforms.py index e65c23df..d73a5573 100644 --- a/src/carla_ros_bridge/transforms.py +++ b/src/carla_ros_bridge/transforms.py @@ -10,11 +10,31 @@ """ import math +import numpy import tf from geometry_msgs.msg import Vector3, Quaternion, Transform, Pose, Point, Twist, Accel +def carla_location_to_numpy_vector(carla_location): + """ + Convert a carla location to a ROS vector3 + + Considers the conversion from left-handed system (unreal) to right-handed + system (ROS) + + :param carla_location: the carla location + :type carla_location: carla.Location + :return: a numpy.array with 3 elements + :rtype: numpy.array + """ + return numpy.array([ + carla_location.x, + -carla_location.y, + carla_location.z + ]) + + def carla_location_to_ros_vector3(carla_location): """ Convert a carla location to a ROS vector3 @@ -72,9 +92,9 @@ def numpy_quaternion_to_ros_quaternion(numpy_quaternion): return ros_quaternion -def carla_rotation_to_ros_quaternion(carla_rotation): +def carla_rotation_to_RPY(carla_rotation): """ - Convert a carla rotation to a ROS quaternion + Convert a carla rotation to a roll, pitch, yaw tuple Considers the conversion from left-handed system (unreal) to right-handed system (ROS). @@ -82,21 +102,90 @@ def carla_rotation_to_ros_quaternion(carla_rotation): :param carla_rotation: the carla rotation :type carla_rotation: carla.Rotation - :return: a ROS quaternion - :rtype: geometry_msgs.msg.Quaternion + :return: a tuple with 3 elements (roll, pitch, yaw) + :rtype: tuple """ - roll = -math.radians(carla_rotation.roll) pitch = math.radians(carla_rotation.pitch) yaw = -math.radians(carla_rotation.yaw) + return (roll, pitch, yaw) + + +def carla_rotation_to_numpy_quaternion(carla_rotation): + """ + Convert a carla rotation to a numpy quaternion + + Considers the conversion from left-handed system (unreal) to right-handed + system (ROS). + Considers the conversion from degrees (carla) to radians (ROS). + + :param carla_rotation: the carla rotation + :type carla_rotation: carla.Rotation + :return: a numpy.array with 4 elements (quaternion) + :rtype: numpy.array + """ + roll, pitch, yaw = carla_rotation_to_RPY(carla_rotation) quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + return quat + + +def carla_rotation_to_ros_quaternion(carla_rotation): + """ + Convert a carla rotation to a ROS quaternion + + Considers the conversion from left-handed system (unreal) to right-handed + system (ROS). + Considers the conversion from degrees (carla) to radians (ROS). + + :param carla_rotation: the carla rotation + :type carla_rotation: carla.Rotation + :return: a ROS quaternion + :rtype: geometry_msgs.msg.Quaternion + """ + quat = carla_rotation_to_numpy_quaternion(carla_rotation) ros_quaternion = numpy_quaternion_to_ros_quaternion(quat) return ros_quaternion +def carla_rotation_to_numpy_rotation_matrix(carla_rotation): + """ + Convert a carla rotation to a ROS quaternion + + Considers the conversion from left-handed system (unreal) to right-handed + system (ROS). + Considers the conversion from degrees (carla) to radians (ROS). + + :param carla_rotation: the carla rotation + :type carla_rotation: carla.Rotation + :return: a numpy.array with 3x3 elements + :rtype: numpy.array + """ + roll, pitch, yaw = carla_rotation_to_RPY(carla_rotation) + numpy_array = tf.transformations.euler_matrix(roll, pitch, yaw) + rotation_matrix = numpy_array[:3, :3] + return rotation_matrix + + +def carla_rotation_to_directional_numpy_vector(carla_rotation): + """ + Convert a carla rotation (as orientation) into a numpy directional vector + + ros_quaternion = np_quaternion_to_ros_quaternion(quat) + :param carla_rotation: the carla rotation + :type carla_rotation: carla.Rotation + :return: a numpy.array with 3 elements as directional vector + representation of the orientation + :rtype: numpy.array + """ + rotation_matrix = carla_rotation_to_numpy_rotation_matrix(carla_rotation) + directional_vector = numpy.array([1, 0, 0]) + rotated_directional_vector = rotation_matrix.dot(directional_vector) + return rotated_directional_vector + + def carla_velocity_to_ros_twist(carla_velocity): """ Convert a carla velocity to a ROS twist @@ -118,6 +207,25 @@ def carla_velocity_to_ros_twist(carla_velocity): return ros_twist +def carla_velocity_to_numpy_vector(carla_velocity): + """ + Convert a carla velocity to a numpy array + + Considers the conversion from left-handed system (unreal) to right-handed + system (ROS) + + :param carla_velocity: the carla velocity + :type carla_velocity: carla.Vector3D + :return: a numpy.array with 3 elements + :rtype: numpy.array + """ + return numpy.array([ + carla_velocity.x, + -carla_velocity.y, + carla_velocity.z + ]) + + def carla_acceleration_to_ros_accel(carla_acceleration): """ Convert a carla acceleration to a ROS accel diff --git a/src/carla_ros_bridge/vehicle.py b/src/carla_ros_bridge/vehicle.py index 1120e67a..7e2e0bb6 100644 --- a/src/carla_ros_bridge/vehicle.py +++ b/src/carla_ros_bridge/vehicle.py @@ -37,9 +37,9 @@ def create_actor(carla_actor, parent): :return: the created vehicle actor :rtype: carla_ros_bridge.Vehicle or derived type """ - if (carla_actor.attributes.has_key('role_name') and - carla_actor.attributes['role_name'] == parent.get_param('ego_vehicle_role_name')): - return EgoVehicle(carla_actor=carla_actor, parent=parent) + if (carla_actor.attributes.get('role_name') == + parent.get_param('ego_vehicle').get('role_name')): + return EgoVehicle.create_actor(carla_actor=carla_actor, parent=parent) else: return Vehicle(carla_actor=carla_actor, parent=parent) From 3fdb84274d3bc05e0529595f6802f784a89a9bd5 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Wed, 12 Dec 2018 17:51:19 +0100 Subject: [PATCH 2/3] Added copyright header --- src/carla_ros_bridge/physics.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/carla_ros_bridge/physics.py b/src/carla_ros_bridge/physics.py index 8dbd6022..2e24b9de 100644 --- a/src/carla_ros_bridge/physics.py +++ b/src/carla_ros_bridge/physics.py @@ -1,3 +1,10 @@ +#!/usr/bin/env python + +# +# Copyright (c) 2018 Intel Labs. +# +# authors: Bernd Gassmann (bernd.gassmann@intel.com) +# """ Tool functions to calculate vehicle physics """ From 8b74f0e2d7c05f1f1429186264e96e8a061d2803 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Fri, 14 Dec 2018 10:35:53 +0100 Subject: [PATCH 3/3] Make AD stack drive EgoVehicleControlInfo.msg - Split data into junks - Add control output - Ego vehicle always sends out this message Control behavior: - Fix steering angle sign - On speed target of zero, brake maximal (support fast emergency brake) - Introduce max_pedal value to make range of acceleration and deceleration pedal area more comparable as the pid control parameters have to work for both - Default PID values for pedal: increased proportional part to allow faster adaptions - Introduce min_accel border to disable (slow adatping) speed control if high accelerations are required (support more exact stopping) Others: - Split rqt perspectives into AckermannControl and AckermannPlot - Cleanup of launch files - bridge.py is more verbose on publisher exceptions - child.py get_param() forward default parameter to parent (fix) - fix pitch angle sign (pitch has to be negative if the vehicle is driving uphils) -> the above results in ad stack able to drive including appropiate stopping/starting uphils and downhils --- CMakeLists.txt | 4 + client_with_rqt.launch | 1 - ...rspective => AckermannControl.perspective} | 18 +- config/AckermannPlot.perspective | 516 ++++++++++++++++++ config/EgoVehicleControlParameter.cfg | 2 +- config/settings.yaml | 8 +- msg/EgoVehicleControlCurrent.msg | 11 + msg/EgoVehicleControlInfo.msg | 41 +- msg/EgoVehicleControlMaxima.msg | 14 + msg/EgoVehicleControlState.msg | 22 + msg/EgoVehicleControlTarget.msg | 12 + src/carla_ros_bridge/bridge.py | 10 +- src/carla_ros_bridge/child.py | 2 +- src/carla_ros_bridge/ego_vehicle.py | 439 +++++++++------ src/carla_ros_bridge/physics.py | 2 +- src/carla_ros_bridge/transforms.py | 2 +- 16 files changed, 886 insertions(+), 218 deletions(-) rename config/{EgoVehicleControlInfo.perspective => AckermannControl.perspective} (89%) create mode 100644 config/AckermannPlot.perspective create mode 100644 msg/EgoVehicleControlCurrent.msg create mode 100644 msg/EgoVehicleControlMaxima.msg create mode 100644 msg/EgoVehicleControlState.msg create mode 100644 msg/EgoVehicleControlTarget.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 8d6e5391..a5a6623e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,11 @@ catkin_python_setup() add_message_files( FILES CarlaVehicleControl.msg + EgoVehicleControlCurrent.msg EgoVehicleControlInfo.msg + EgoVehicleControlMaxima.msg + EgoVehicleControlState.msg + EgoVehicleControlTarget.msg ) generate_messages( diff --git a/client_with_rqt.launch b/client_with_rqt.launch index f6774f9f..96c455da 100644 --- a/client_with_rqt.launch +++ b/client_with_rqt.launch @@ -1,5 +1,4 @@ - diff --git a/config/EgoVehicleControlInfo.perspective b/config/AckermannControl.perspective similarity index 89% rename from config/EgoVehicleControlInfo.perspective rename to config/AckermannControl.perspective index d4edd8ad..a9608058 100644 --- a/config/EgoVehicleControlInfo.perspective +++ b/config/AckermannControl.perspective @@ -5,7 +5,7 @@ "keys": { "running-plugins": { "type": "repr", - "repr": "{u'rqt_reconfigure/Param': [1], u'rqt_plot/Plot': [3, 2, 1], u'rqt_topic/TopicPlugin': [1], u'rqt_publisher/Publisher': [1]}" + "repr": "{u'rqt_reconfigure/Param': [1], u'rqt_topic/TopicPlugin': [1], u'rqt_publisher/Publisher': [1]}" } }, "groups": { @@ -150,7 +150,7 @@ }, "topics": { "type": "repr", - "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/target_speed_abs', u'/carla/ego_vehicle/ego_vehicle_control_info/current_speed_abs']" + "repr": "u''" }, "y_limits": { "type": "repr", @@ -197,7 +197,7 @@ }, "topics": { "type": "repr", - "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/speed_control_accel_delta', u'/carla/ego_vehicle/ego_vehicle_control_info/speed_control_accel_target', u'/carla/ego_vehicle/ego_vehicle_control_info/current_accel']" + "repr": "u''" }, "y_limits": { "type": "repr", @@ -244,7 +244,7 @@ }, "topics": { "type": "repr", - "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/accel_control_pedal_delta', u'/carla/ego_vehicle/ego_vehicle_control_info/throttle_lower_border', u'/carla/ego_vehicle/ego_vehicle_control_info/brake_upper_border', u'/carla/ego_vehicle/ego_vehicle_control_info/accel_control_pedal_target']" + "repr": "u''" }, "y_limits": { "type": "repr", @@ -354,7 +354,7 @@ "keys": { "publishers": { "type": "repr", - "repr": "u\"[{'topic_name': '/carla/ego_vehicle/ackermann_cmd', 'type_name': 'ackermann_msgs/AckermannDrive', 'enabled': False, 'rate': 1.0, 'expressions': {u'/carla/ego_vehicle/ackermann_cmd/speed': '1'}, 'publisher_id': 0, 'counter': 0}]\"" + "repr": "u\"[{'type_name': 'ackermann_msgs/AckermannDrive', 'topic_name': '/carla/ego_vehicle/ackermann_cmd', 'enabled': False, 'rate': 1.0, 'expressions': {u'/carla/ego_vehicle/ackermann_cmd/speed': '1'}, 'publisher_id': 0, 'counter': 0}]\"" } }, "groups": {} @@ -368,8 +368,8 @@ "keys": { "tree_widget_header_state": { "type": "repr(QByteArray.hex)", - "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000010000000000000000010000000000000000000000000000000000000479000000050101000100000000000000000500000064ffffffff0000008100000003000000050000018800000001000000030000011700000001000000030000007200000001000000030000003d00000001000000030000012b0000000100000003000003e800')", - "pretty-print": " d r = + " + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000010000000000000000010000000000000000000000000000000000000728000000050101000100000000000000000500000064ffffffff000000810000000300000005000002c100000001000000030000022d00000001000000030000007200000001000000030000003d00000001000000030000018b0000000100000003000003e800')", + "pretty-print": " d - r = " } }, "groups": {} @@ -457,8 +457,8 @@ }, "state": { "type": "repr(QByteArray.hex)", - "repr(QByteArray.hex)": "QtCore.QByteArray('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')", - "pretty-print": " Drqt_graph__RosGraph__1__RosGraphUi Brqt_msg__Messages__1__messages.ui e r 4 G { x x x x I " + "repr(QByteArray.hex)": "QtCore.QByteArray('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')", + "pretty-print": " Drqt_graph__RosGraph__1__RosGraphUi Brqt_msg__Messages__1__messages.ui I e ) r / j 4 G { 0 8 I " } }, "groups": { diff --git a/config/AckermannPlot.perspective b/config/AckermannPlot.perspective new file mode 100644 index 00000000..5db0d105 --- /dev/null +++ b/config/AckermannPlot.perspective @@ -0,0 +1,516 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'rqt_plot/Plot': [3, 2, 1], u'rqt_graph/RosGraph': [1]}" + } + }, + "groups": { + "plugin__mo2ive_status_monitor__Mo2IVe Status Monitor__1": { + "keys": {}, + "groups": { + "dock_widget__StatusMonitorUI": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Mo2IVe Status Monitor'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_msg__Messages__1": { + "keys": {}, + "groups": { + "dock_widget__messages.ui": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Message Type Browser'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_reconfigure__Param__1": { + "keys": {}, + "groups": { + "dock_widget___plugincontainer_top_widget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Dynamic Reconfigure'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000100000002000000ae0000006401ffffffff010000000100')", + "pretty-print": " d " + }, + "_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000001000000020000012c000000640100000009010000000200')", + "pretty-print": " , d " + } + }, + "groups": {} + } + } + }, + "plugin__rqt_image_view__ImageView__1": { + "keys": {}, + "groups": { + "dock_widget__ImageViewWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Image View'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "max_range": { + "type": "repr", + "repr": "u'10'" + }, + "rotate": { + "type": "repr", + "repr": "u'0'" + }, + "smooth_image": { + "type": "repr", + "repr": "u'false'" + }, + "mouse_pub_topic": { + "type": "repr", + "repr": "u'/carla/ego_vehicle/camera/depth/front/image_depth'" + }, + "num_gridlines": { + "type": "repr", + "repr": "u'0'" + }, + "toolbar_hidden": { + "type": "repr", + "repr": "u'false'" + }, + "zoom1": { + "type": "repr", + "repr": "u'false'" + }, + "dynamic_range": { + "type": "repr", + "repr": "u'false'" + }, + "topic": { + "type": "repr", + "repr": "u'/carla/ego_vehicle/camera/depth/front/image_depth'" + }, + "publish_click_location": { + "type": "repr", + "repr": "u'false'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_plot__Plot__1": { + "keys": {}, + "groups": { + "dock_widget__DataPlotWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'MatPlot'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "autoscroll": { + "type": "repr", + "repr": "True" + }, + "plot_type": { + "type": "repr", + "repr": "1" + }, + "topics": { + "type": "repr", + "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/state/speed_control_activation_count', u'/carla/ego_vehicle/ego_vehicle_control_info/target/speed_abs', u'/carla/ego_vehicle/ego_vehicle_control_info/current/speed_abs']" + }, + "y_limits": { + "type": "repr", + "repr": "[0.0, 11.121379852294922]" + }, + "x_limits": { + "type": "repr", + "repr": "[789.135796021, 790.135796021]" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_plot__Plot__2": { + "keys": {}, + "groups": { + "dock_widget__DataPlotWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'MatPlot (2)'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "autoscroll": { + "type": "repr", + "repr": "True" + }, + "plot_type": { + "type": "repr", + "repr": "1" + }, + "topics": { + "type": "repr", + "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/target/jerk', u'/carla/ego_vehicle/ego_vehicle_control_info/target/accel', u'/carla/ego_vehicle/ego_vehicle_control_info/current/accel', u'/carla/ego_vehicle/ego_vehicle_control_info/state/speed_control_accel_target']" + }, + "y_limits": { + "type": "repr", + "repr": "[-8.235246658325195, 5.247461795806885]" + }, + "x_limits": { + "type": "repr", + "repr": "[780.918297879, 781.918297879]" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_plot__Plot__3": { + "keys": {}, + "groups": { + "dock_widget__DataPlotWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'MatPlot (3)'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "autoscroll": { + "type": "repr", + "repr": "True" + }, + "plot_type": { + "type": "repr", + "repr": "1" + }, + "topics": { + "type": "repr", + "repr": "[u'/carla/ego_vehicle/ego_vehicle_control_info/state/throttle_lower_border', u'/carla/ego_vehicle/ego_vehicle_control_info/state/brake_upper_border', u'/carla/ego_vehicle/ego_vehicle_control_info/output/throttle', u'/carla/ego_vehicle/ego_vehicle_control_info/output/brake', u'/carla/ego_vehicle/ego_vehicle_control_info/state/accel_control_pedal_target']" + }, + "y_limits": { + "type": "repr", + "repr": "[-0.3841038942337036, 1.0]" + }, + "x_limits": { + "type": "repr", + "repr": "[125.35065189599982, 126.35065189599982]" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_graph__RosGraph__1": { + "keys": {}, + "groups": { + "dock_widget__RosGraphUi": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Node Graph'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "graph_type_combo_box_index": { + "type": "repr", + "repr": "0" + }, + "topic_filter_line_edit_text": { + "type": "repr", + "repr": "u'/'" + }, + "filter_line_edit_text": { + "type": "repr", + "repr": "u'/'" + }, + "highlight_connections_check_box_state": { + "type": "repr", + "repr": "True" + }, + "unreachable_check_box_state": { + "type": "repr", + "repr": "False" + }, + "actionlib_check_box_state": { + "type": "repr", + "repr": "True" + }, + "quiet_check_box_state": { + "type": "repr", + "repr": "True" + }, + "dead_sinks_check_box_state": { + "type": "repr", + "repr": "False" + }, + "leaf_topics_check_box_state": { + "type": "repr", + "repr": "False" + }, + "namespace_cluster_check_box_state": { + "type": "repr", + "repr": "True" + }, + "auto_fit_graph_check_box_state": { + "type": "repr", + "repr": "True" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_publisher__Publisher__1": { + "keys": {}, + "groups": { + "dock_widget__PublisherWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Message Publisher'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "publishers": { + "type": "repr", + "repr": "u\"[{'topic_name': '/carla/ego_vehicle/vehicle_control_cmd', 'type_name': 'carla_ros_bridge/CarlaVehicleControl', 'enabled': False, 'rate': 1.0, 'expressions': {u'/carla/ego_vehicle/vehicle_control_cmd/throttle': '0.5', u'/carla/ego_vehicle/vehicle_control_cmd/hand_brake': 'False', u'/carla/ego_vehicle/vehicle_control_cmd/steer': '0', u'/carla/ego_vehicle/vehicle_control_cmd/reverse': 'False', u'/carla/ego_vehicle/vehicle_control_cmd/brake': '0'}, 'publisher_id': 0, 'counter': 0}]\"" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_topic__TopicPlugin__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "tree_widget_header_state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000001000000000000000001000000000000000000000000000000000000070e000000050101000100000000000000000500000064ffffffff000000810000000300000005000002c100000001000000030000022d00000001000000030000007200000001000000030000003d0000000100000003000001710000000100000003000003e800')", + "pretty-print": " d - r = q " + } + }, + "groups": {} + }, + "dock_widget__TopicWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Topic Monitor'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_tf_tree__RosTfTree__2": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "highlight_connections_check_box_state": { + "type": "repr", + "repr": "u'true'" + }, + "auto_fit_graph_check_box_state": { + "type": "repr", + "repr": "u'true'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_tf_tree__RosTfTree__1": { + "keys": {}, + "groups": { + "dock_widget__RosTfTreeUi": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'TF Tree'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "highlight_connections_check_box_state": { + "type": "repr", + "repr": "u'true'" + }, + "auto_fit_graph_check_box_state": { + "type": "repr", + "repr": "u'true'" + } + }, + "groups": {} + } + } + } + } + }, + "mainwindow": { + "keys": { + "geometry": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb0002000000000037000000180000077f000002a600000037000000340000077f000002a600000000000000000780')", + "pretty-print": " 7 7 4 " + }, + "state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('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')", + "pretty-print": " Drqt_graph__RosGraph__1__RosGraphUi Brqt_msg__Messages__1__messages.ui = 1 L t G Brqt_plot__Plot__3__DataPlotWidget Brqt_plot__Plot__1__DataPlotWidget mo2ive_status_monitor__Mo2IVe Status Monitor__1__StatusMonitorUI Brqt_plot__Plot__2__DataPlotWidget 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} \ No newline at end of file diff --git a/config/EgoVehicleControlParameter.cfg b/config/EgoVehicleControlParameter.cfg index 2ab6653e..8e80c53f 100755 --- a/config/EgoVehicleControlParameter.cfg +++ b/config/EgoVehicleControlParameter.cfg @@ -9,7 +9,7 @@ gen.add("speed_Kp", double_t, 0, "Kp of the speed PID", 0.05, 0, 1.) gen.add("speed_Ki", double_t, 0, "Ki of the speed PID", 0., 0, 1.) gen.add("speed_Kd", double_t, 0, "Kd of the speed PID", 0.5, 0, 10.) -gen.add("accel_Kp", double_t, 0, "Kp of the accel PID", 0.02, 0, 10.) +gen.add("accel_Kp", double_t, 0, "Kp of the accel PID", 0.05, 0, 10.) gen.add("accel_Ki", double_t, 0, "Ki of the accel PID", 0., 0, 10.) gen.add("accel_Kd", double_t, 0, "Kd of the accel PID", 0.05, 0, 10.) diff --git a/config/settings.yaml b/config/settings.yaml index 28a0ac56..aa014dda 100644 --- a/config/settings.yaml +++ b/config/settings.yaml @@ -23,4 +23,10 @@ carla: # (only relevant for ackermann control mode) accel_Kp: 0.02 accel_Ki: 0.00 - accel_Kd: 0.05 \ No newline at end of file + accel_Kd: 0.05 + # set the minimum acceleration in (m/s^2) + # This border value is used to enable the speed controller which is used to control driving + # at more or less constant speed. + # If the absolute value of the ackermann drive target acceleration exceeds this value, + # directly the input acceleration is controlled + min_accel: 1.0 \ No newline at end of file diff --git a/msg/EgoVehicleControlCurrent.msg b/msg/EgoVehicleControlCurrent.msg new file mode 100644 index 00000000..8375ed21 --- /dev/null +++ b/msg/EgoVehicleControlCurrent.msg @@ -0,0 +1,11 @@ +#  +# Copyright (c) 2018 Intel Labs. +# +# authors: Bernd Gassmann (bernd.gassmann@intel.com) +# +# This represents the current time/speed/accel values of the vehicle used by the controller + +float32 time_sec +float32 speed +float32 speed_abs +float32 accel \ No newline at end of file diff --git a/msg/EgoVehicleControlInfo.msg b/msg/EgoVehicleControlInfo.msg index 7e4a205c..ebb2f1ad 100644 --- a/msg/EgoVehicleControlInfo.msg +++ b/msg/EgoVehicleControlInfo.msg @@ -1,34 +1,23 @@ +#  +# Copyright (c) 2018 Intel Labs. +# +# authors: Bernd Gassmann (bernd.gassmann@intel.com) +# # This represents an info message of the ego vehicle Header header -# vehicle maximum values -float32 max_steering_angle -float32 max_speed -float32 max_accel -float32 max_decel +# the restrictions +EgoVehicleControlMaxima restrictions -# the target speed/accel values of the vehicle -float32 target_steering_angle -float32 target_speed -float32 target_speed_abs -float32 target_accel_abs -float32 target_decel_abs +# the target values +EgoVehicleControlTarget target -# the current speed/accel values of the vehicle -float32 current_time_sec -float32 current_speed -float32 current_speed_abs -float32 current_accel +# the currently measured values +EgoVehicleControlCurrent current # the current control state -string status -# speed controller -float32 speed_control_accel_delta -float32 speed_control_accel_target -# acceleration controller -float32 accel_control_pedal_delta -float32 accel_control_pedal_target -# borders for lay off pedal -float32 brake_upper_border -float32 throttle_lower_border +EgoVehicleControlState state + +# the current control output to CARLA +CarlaVehicleControl output diff --git a/msg/EgoVehicleControlMaxima.msg b/msg/EgoVehicleControlMaxima.msg new file mode 100644 index 00000000..9e9ec965 --- /dev/null +++ b/msg/EgoVehicleControlMaxima.msg @@ -0,0 +1,14 @@ +#  +# Copyright (c) 2018 Intel Labs. +# +# authors: Bernd Gassmann (bernd.gassmann@intel.com) +# +# This represents some ego vehicle control maximal values + +# vehicle maximum values +float32 max_steering_angle +float32 max_speed +float32 max_accel +float32 max_decel +float32 min_accel +float32 max_pedal diff --git a/msg/EgoVehicleControlState.msg b/msg/EgoVehicleControlState.msg new file mode 100644 index 00000000..013b1837 --- /dev/null +++ b/msg/EgoVehicleControlState.msg @@ -0,0 +1,22 @@ +#  +# Copyright (c) 2018 Intel Labs. +# +# authors: Bernd Gassmann (bernd.gassmann@intel.com) +# +# This represents the control state variables of the ego vehicle control + +# the current control state +string status + +# speed controller +uint8 speed_control_activation_count +float32 speed_control_accel_delta +float32 speed_control_accel_target + +# acceleration controller +float32 accel_control_pedal_delta +float32 accel_control_pedal_target + +# borders for lay off pedal +float32 brake_upper_border +float32 throttle_lower_border diff --git a/msg/EgoVehicleControlTarget.msg b/msg/EgoVehicleControlTarget.msg new file mode 100644 index 00000000..64814ad7 --- /dev/null +++ b/msg/EgoVehicleControlTarget.msg @@ -0,0 +1,12 @@ +#  +# Copyright (c) 2018 Intel Labs. +# +# authors: Bernd Gassmann (bernd.gassmann@intel.com) +# +# This represents the target speed/accel values of the ego vehicle + +float32 steering_angle +float32 speed +float32 speed_abs +float32 accel +float32 jerk diff --git a/src/carla_ros_bridge/bridge.py b/src/carla_ros_bridge/bridge.py index 9d5d41c8..bd8d9038 100755 --- a/src/carla_ros_bridge/bridge.py +++ b/src/carla_ros_bridge/bridge.py @@ -192,11 +192,13 @@ def send_msgs(self): :return: """ - try: - for publisher, msg in self.msgs_to_publish: + for publisher, msg in self.msgs_to_publish: + try: publisher.publish(msg) - except: # pylint: disable=bare-except - rospy.logwarn("Failed to publish messages") + except rospy.ROSSerializationException as error: + rospy.logwarn("Failed to serialize message on publishing: {}".format(error)) + except: # pylint: disable=bare-except + rospy.logwarn("Failed to publish message") self.msgs_to_publish = [] def run(self): diff --git a/src/carla_ros_bridge/child.py b/src/carla_ros_bridge/child.py index 4cc83d97..a54ea7d9 100644 --- a/src/carla_ros_bridge/child.py +++ b/src/carla_ros_bridge/child.py @@ -103,7 +103,7 @@ def get_param(self, key, default=None): :return: the parameter string :rtype: string """ - return self.parent.get_param(key) + return self.parent.get_param(key, default) def topic_name(self): """ diff --git a/src/carla_ros_bridge/ego_vehicle.py b/src/carla_ros_bridge/ego_vehicle.py index 1d2c8a69..dcde660c 100644 --- a/src/carla_ros_bridge/ego_vehicle.py +++ b/src/carla_ros_bridge/ego_vehicle.py @@ -71,6 +71,50 @@ def __init__(self, carla_actor, parent): parent=parent, topic_prefix="ego_vehicle", append_role_name_topic_postfix=False) + # control info + self.info = EgoVehicleControlInfo() + + # maximum values + self.info.restrictions.max_steering_angle = phys.get_vehicle_max_steering_angle( + self.carla_actor) + self.info.restrictions.max_speed = phys.get_vehicle_max_speed( + self.carla_actor) + self.info.restrictions.max_accel = phys.get_vehicle_max_acceleration( + self.carla_actor) + self.info.restrictions.max_decel = phys.get_vehicle_max_deceleration( + self.carla_actor) + self.info.restrictions.min_accel = 0. + self.info.restrictions.max_pedal = 1.0 + + # target values + self.info.target.steering_angle = 0. + self.info.target.speed = 0. + self.info.target.speed_abs = 0. + self.info.target.accel = 0. + self.info.target.jerk = 0. + + # current values + self.info.current.time_sec = self.get_current_ros_time().to_sec() + self.info.current.speed = 0. + self.info.current.speed_abs = 0. + self.info.current.accel = 0. + + # control values + self.info.state.status = 'n/a' + self.info.state.speed_control_activation_count = 0 + self.info.state.speed_control_accel_delta = 0. + self.info.state.speed_control_accel_target = 0. + self.info.state.accel_control_pedal_delta = 0. + self.info.state.accel_control_pedal_target = 0. + self.info.state.brake_upper_border = 0. + self.info.state.throttle_lower_border = 0. + + # control output + self.info.output.throttle = 0. + self.info.output.brake = 1.0 + self.info.output.steer = 0. + self.info.output.reverse = False + self.info.output.hand_brake = True def get_marker_color(self): """ @@ -105,6 +149,77 @@ def send_object_msg(self): self.publish_ros_message(self.topic_name() + "/odometry", odometry) + def apply_control(self): + """ + Apply current control output to CARLA + + :return: + """ + vehicle_control = VehicleControl() + vehicle_control.hand_brake = self.info.output.hand_brake + vehicle_control.brake = self.info.output.brake + vehicle_control.steer = self.info.output.steer + vehicle_control.throttle = self.info.output.throttle + vehicle_control.reverse = self.info.output.reverse + # send control command out + self.carla_actor.apply_control(vehicle_control) + + def update_current_values(self): + """ + Function to update vehicle control current values. + + we calculate the acceleration on ourselves, because we are interested only in + the acceleration in respect to the driving direction + In addition a small average filter is applied + + :return: + """ + current_time_sec = self.get_current_ros_time().to_sec() + delta_time = current_time_sec - self.info.current.time_sec + current_speed = phys.get_vehicle_speed(self.carla_actor) + if delta_time > 0: + delta_speed = current_speed - self.info.current.speed + current_accel = delta_speed / delta_time + # average filter + self.info.current.accel = (self.info.current.accel * 4 + current_accel) / 5 + self.info.current.time_sec = current_time_sec + self.info.current.speed = current_speed + self.info.current.speed_abs = abs(current_speed) + + def vehicle_control_cycle(self): + """ + Function to perform a control cycle. + + To be overridden by derived control classes + + :return: + """ + pass + + def update(self): + """ + Function (override) to update this object. + + On update ego vehicle calculates and sends the new values for VehicleControl() + + :return: + """ + super(EgoVehicle, self).update() + self.update_current_values() + self.vehicle_control_cycle() + self.send_ego_vehicle_control_info_msg() + + def send_ego_vehicle_control_info_msg(self): + """ + Function to send carla_ros_bridge.msg.EgoVehicleControlInfo message. + + :return: + """ + self.info.header = self.get_msg_header() + self.info.output.header = self.info.header + self.publish_ros_message( + self.topic_name() + "/ego_vehicle_control_info", self.info) + class PedalControlVehicle(EgoVehicle): @@ -152,18 +267,12 @@ def control_command_updated(self, ros_vehicle_control): This brigde is not responsible for any restrictions on velocity or steering. It's just forwarding the ROS input to CARLA - :param ros_vehicle_control: - :type ros_vehicle_control: carla_ros_bridge.msg.CarlaVehicleControl + :param ros_vehicle_control: current vehicle control input received via ROS + :type self.info.output: carla_ros_bridge.msg.CarlaVehicleControl :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 - # send control command out - self.carla_actor.apply_control(vehicle_control) + self.info.output = ros_vehicle_control + self.apply_control() class AckermannControlVehicle(EgoVehicle): @@ -184,43 +293,12 @@ def __init__(self, carla_actor, parent): super(AckermannControlVehicle, self).__init__(carla_actor=carla_actor, parent=parent) - # control info - self.info = EgoVehicleControlInfo() - - # maximum values - self.info.max_steering_angle = phys.get_vehicle_max_steering_angle( - self.carla_actor) - self.info.max_speed = phys.get_vehicle_max_speed( - self.carla_actor) - self.info.max_accel = phys.get_vehicle_max_acceleration( - self.carla_actor) - self.info.max_decel = phys.get_vehicle_max_deceleration( - self.carla_actor) - - # target values - self.info.target_steering_angle = 0. - self.info.target_speed = 0. - self.info.target_speed_abs = 0. - self.info.target_accel_abs = 0. - self.info.target_decel_abs = 0. - - # current values - self.info.current_time_sec = self.get_current_ros_time().to_sec() - self.info.current_speed = 0. - self.info.current_speed_abs = 0. - self.info.current_accel = 0. - - # control values - self.info.status = '' - self.info.speed_control_accel_delta = 0. - self.info.speed_control_accel_target = 0. - self.info.accel_control_pedal_delta = 0. - self.info.accel_control_pedal_target = 0. - self.info.brake_upper_border = 0. - self.info.throttle_lower_border = 0. - - # the carla vehicle control message - self.vehicle_control = VehicleControl() + # adapt initial values + self.info.restrictions.min_accel = parent.get_param('ego_vehicle').get('min_accel', 1.) + # clipping the pedal in both directions to the same range using the usual lower + # border: the max_accel to ensure the the pedal target is in symmetry to zero + self.info.restrictions.max_pedal = min( + self.info.restrictions.max_accel, self.info.restrictions.max_decel) # PID controller # the controller has to run with the simulation time, not with real-time @@ -269,18 +347,6 @@ def destroy(self): self.reconfigure_server = None super(AckermannControlVehicle, self).destroy() - def update(self): - """ - Function (override) to update this object. - - On update ego vehicle calculates and sends the new values for VehicleControl() - - :return: - """ - super(AckermannControlVehicle, self).update() - self.vehicle_control_cycle() - self.send_ego_vehicle_control_info_msg() - def reconfigure_pid_parameters(self, ego_vehicle_control_parameter, dummy_level): """ Callback for dynamic reconfigure call to set the PID parameters @@ -305,16 +371,6 @@ def reconfigure_pid_parameters(self, ego_vehicle_control_parameter, dummy_level) ) return ego_vehicle_control_parameter - def send_ego_vehicle_control_info_msg(self): - """ - Function to send carla_ros_bridge.msg.EgoVehicleControlInfo message. - - :return: - """ - self.info.header = self.get_msg_header() - self.publish_ros_message( - self.topic_name() + "/ego_vehicle_control_info", self.info) - def ackermann_command_updated(self, ros_ackermann_drive): """ Convert a Ackerman drive msg into carla control msg @@ -326,89 +382,79 @@ def ackermann_command_updated(self, ros_ackermann_drive): This brigde is not responsible for any restrictions on velocity or steering. It's just forwarding the ROS input to CARLA - :param ros_vehicle_control: - :type ros_vehicle_control: carla_ros_bridge.msg.CarlaVehicleControl + :param ros_ackermann_drive: the current ackermann control input + :type ros_ackermann_drive: ackermann_msgs.AckermannDrive :return: """ # set target values self.set_target_steering_angle(ros_ackermann_drive.steering_angle) self.set_target_speed(ros_ackermann_drive.speed) self.set_target_accel(ros_ackermann_drive.acceleration) + self.set_target_jerk(ros_ackermann_drive.jerk) def set_target_steering_angle(self, target_steering_angle): """ set target sterring angle """ - if abs(target_steering_angle) > self.info.max_steering_angle: + self.info.target.steering_angle = -target_steering_angle + if abs(self.info.target.steering_angle) > self.info.restrictions.max_steering_angle: rospy.logerr("Max steering angle reached, clipping value") - self.info.target_steering_angle = numpy.clip( - target_steering_angle, -self.info.max_steering_angle, self.info.max_steering_angle) - else: - self.info.target_steering_angle = target_steering_angle + self.info.target.steering_angle = numpy.clip( + self.info.target.steering_angle, + -self.info.restrictions.max_steering_angle, + self.info.restrictions.max_steering_angle) def set_target_speed(self, target_speed): """ set target speed """ - if abs(target_speed) > self.info.max_speed: + if abs(target_speed) > self.info.restrictions.max_speed: rospy.logerr("Max speed reached, clipping value") - self.info.target_speed = numpy.clip( - target_speed, -self.info.max_speed, self.info.max_speed) + self.info.target.speed = numpy.clip( + target_speed, -self.info.restrictions.max_speed, self.info.restrictions.max_speed) else: - self.info.target_speed = target_speed - self.info.target_speed_abs = abs(self.info.target_speed) + self.info.target.speed = target_speed + self.info.target.speed_abs = abs(self.info.target.speed) def set_target_accel(self, target_accel): """ set target accel """ epsilon = 0.00001 - # per definition of ackermann drive: if zero, then use max value - if abs(target_accel) < epsilon: - self.info.target_accel_abs = self.info.max_accel - self.info.target_decel_abs = self.info.max_decel + # if speed is set to zero, then use max decel value + if self.info.target.speed_abs < epsilon: + self.info.target.accel = -self.info.restrictions.max_decel else: - self.info.target_accel_abs = numpy.clip( - abs(target_accel), 0, self.info.max_accel) - self.info.target_decel_abs = numpy.clip( - abs(target_accel), 0, self.info.max_decel) + self.info.target.accel = numpy.clip( + target_accel, -self.info.restrictions.max_decel, self.info.restrictions.max_accel) + + def set_target_jerk(self, target_jerk): + """ + set target accel + """ + self.info.target.jerk = target_jerk def vehicle_control_cycle(self): """ Perform a vehicle control cycle and sends out carla.VehicleControl message """ - # update current values - # we calculate the acceleration on ourselves, because we are interested only in - # the acceleration regarding our driving direction - # in addition we just filter - current_time_sec = self.get_current_ros_time().to_sec() - delta_time = current_time_sec - self.info.current_time_sec - current_speed = phys.get_vehicle_speed(self.carla_actor) - if delta_time > 0: - delta_speed = current_speed - self.info.current_speed - current_accel = delta_speed / delta_time - # average filter - self.info.current_accel = (self.info.current_accel * 4 + current_accel) / 5 - self.info.current_time_sec = current_time_sec - self.info.current_speed = current_speed - self.info.current_speed_abs = abs(current_speed) - # perform actual control self.control_steering() self.control_stop_and_reverse() - self.run_pid_control_loop() - if not self.vehicle_control.hand_brake: + self.run_speed_control_loop() + self.run_accel_control_loop() + if not self.info.output.hand_brake: self.update_drive_vehicle_control_command() - # send control command out - self.carla_actor.apply_control(self.vehicle_control) + # apply control command to CARLA + self.apply_control() def control_steering(self): """ Basic steering control """ - self.vehicle_control.steer = self.info.target_steering_angle / \ - self.info.max_steering_angle + self.info.output.steer = self.info.target.steering_angle / \ + self.info.restrictions.max_steering_angle def control_stop_and_reverse(self): """ @@ -420,61 +466,106 @@ def control_stop_and_reverse(self): full_stop_epsilon = 0.00001 # auto-control of hand-brake and reverse gear - self.vehicle_control.hand_brake = False - if self.info.current_speed_abs < standing_still_epsilon: + self.info.output.hand_brake = False + if self.info.current.speed_abs < standing_still_epsilon: # standing still, change of driving direction allowed - self.info.status = "standing" - if self.info.target_speed < 0: - if not self.vehicle_control.reverse: + self.info.state.status = "standing" + if self.info.target.speed < 0: + if not self.info.output.reverse: rospy.loginfo( "VehicleControl: Change of driving direction to reverse") - self.vehicle_control.reverse = True - elif self.info.target_speed > 0: - if self.vehicle_control.reverse: + self.info.output.reverse = True + elif self.info.target.speed > 0: + if self.info.output.reverse: rospy.loginfo( "VehicleControl: Change of driving direction to forward") - self.vehicle_control.reverse = False - if self.info.target_speed_abs < full_stop_epsilon: - self.info.status = "full stop" - self.vehicle_control.hand_brake = True - self.vehicle_control.brake = 1.0 - self.vehicle_control.throttle = 0.0 + self.info.output.reverse = False + if self.info.target.speed_abs < full_stop_epsilon: + self.info.state.status = "full stop" + self.info.state.speed_control_accel_target = 0. + self.info.state.accel_control_pedal_target = 0. self.set_target_speed(0.) - self.info.current_speed = 0. - self.info.current_speed_abs = 0. - self.info.current_accel = 0. - self.info.speed_control_accel_target = 0. - self.info.accel_control_pedal_target = 0. - - elif numpy.sign(self.info.current_speed) * numpy.sign(self.info.target_speed) == -1: + self.info.current.speed = 0. + self.info.current.speed_abs = 0. + self.info.current.accel = 0. + self.info.output.hand_brake = True + self.info.output.brake = 1.0 + self.info.output.throttle = 0.0 + + elif numpy.sign(self.info.current.speed) * numpy.sign(self.info.target.speed) == -1: # requrest for change of driving direction # first we have to come to full stop before changing driving # direction rospy.loginfo("VehicleControl: Request change of driving direction." " v_current={} v_desired={}" - " Set desired speed to 0".format(self.info.current_speed, - self.info.target_speed)) + " Set desired speed to 0".format(self.info.current.speed, + self.info.target.speed)) self.set_target_speed(0.) - def run_pid_control_loop(self): + def run_speed_control_loop(self): """ Run the PID control loop for the speed + + The speed control is only activated if desired acceleration is moderate + otherwhise we try to follow the desired acceleration values + + Reasoning behind: + + An autonomous vehicle calculates a trajectory including position and velocities. + The ackermann drive is derived directly from that trajectory. + The acceleration and jerk values provided by the ackermann drive command + reflect already the speed profile of the trajectory. + It makes no sense to try to mimick this a-priori knowledge by the speed PID + controller. + => + The speed controller is mainly responsible to keep the speed. + On expected speed changes, the speed control loop is disabled + """ + epsilon = 0.00001 + target_accel_abs = abs(self.info.target.accel) + if target_accel_abs < self.info.restrictions.min_accel: + if self.info.state.speed_control_activation_count < 5: + self.info.state.speed_control_activation_count += 1 + else: + if self.info.state.speed_control_activation_count > 0: + self.info.state.speed_control_activation_count -= 1 + # set the auto_mode of the controller accordingly + self.speed_controller.auto_mode = self.info.state.speed_control_activation_count >= 5 + + if self.speed_controller.auto_mode: + self.speed_controller.setpoint = self.info.target.speed_abs + self.info.state.speed_control_accel_delta = self.speed_controller( + self.info.current.speed_abs) + + # clipping borders + clipping_lower_border = -target_accel_abs + clipping_upper_border = target_accel_abs + # per definition of ackermann drive: if zero, then use max value + if target_accel_abs < epsilon: + clipping_lower_border = -self.info.restrictions.max_decel + clipping_upper_border = self.info.restrictions.max_accel + self.info.state.speed_control_accel_target = numpy.clip( + self.info.state.speed_control_accel_target + + self.info.state.speed_control_accel_delta, + clipping_lower_border, clipping_upper_border) + else: + self.info.state.speed_control_accel_delta = 0. + self.info.state.speed_control_accel_target = self.info.target.accel + + def run_accel_control_loop(self): + """ + Run the PID control loop for the acceleration """ - self.speed_controller.setpoint = self.info.target_speed_abs - self.info.speed_control_accel_delta = self.speed_controller( - self.info.current_speed_abs) - self.info.speed_control_accel_target = numpy.clip( - self.info.speed_control_accel_target + self.info.speed_control_accel_delta, - -self.info.target_decel_abs, - self.info.target_accel_abs) - - self.accel_controller.setpoint = self.info.speed_control_accel_target - self.info.accel_control_pedal_delta = self.accel_controller( - self.info.current_accel) - self.info.accel_control_pedal_target = numpy.clip( - self.info.accel_control_pedal_target + self.info.accel_control_pedal_delta, - -self.info.max_decel, - self.info.max_accel) + # setpoint of the acceleration controller is the output of the speed controller + self.accel_controller.setpoint = self.info.state.speed_control_accel_target + self.info.state.accel_control_pedal_delta = self.accel_controller( + self.info.current.accel) + # @todo: we might want to scale by making use of the the abs-jerk value + # If the jerk input is big, then the trajectory input expects already quick changes + # in the acceleration; to respect this we put an additional proportional factor on top + self.info.state.accel_control_pedal_target = numpy.clip( + self.info.state.accel_control_pedal_target + self.info.state.accel_control_pedal_delta, + -self.info.restrictions.max_pedal, self.info.restrictions.max_pedal) def update_drive_vehicle_control_command(self): """ @@ -484,40 +575,42 @@ def update_drive_vehicle_control_command(self): # the driving impedance moves the 'zero' acceleration border # Interpretation: To reach a zero acceleration the throttle has to pushed # down for a certain amount - self.info.throttle_lower_border = phys.get_vehicle_driving_impedance_acceleration( - self.carla_actor, self.vehicle_control.reverse) + self.info.state.throttle_lower_border = phys.get_vehicle_driving_impedance_acceleration( + self.carla_actor, self.info.output.reverse) # the engine lay off acceleration defines the size of the coasting area # Interpretation: The engine already prforms braking on its own; # therefore pushing the brake is not required for small decelerations - self.info.brake_upper_border = self.info.throttle_lower_border + \ + self.info.state.brake_upper_border = self.info.state.throttle_lower_border + \ phys.get_vehicle_lay_off_engine_acceleration(self.carla_actor) - if self.info.accel_control_pedal_target > self.info.throttle_lower_border: - self.info.status = "accelerating" - self.vehicle_control.brake = 0.0 - # the value has to be normed to max_accel + if self.info.state.accel_control_pedal_target > self.info.state.throttle_lower_border: + self.info.state.status = "accelerating" + self.info.output.brake = 0.0 + # the value has to be normed to max_pedal # be aware: is not required to take throttle_lower_border into the scaling factor, # because that border is in reality a shift of the coordinate system # the global maximum acceleration can practically not be reached anymore because of # driving impedance - self.vehicle_control.throttle = ( - (self.info.accel_control_pedal_target - self.info.throttle_lower_border) / - abs(self.info.max_accel)) - elif self.info.accel_control_pedal_target > self.info.brake_upper_border: - self.info.status = "coasting" + self.info.output.throttle = ( + (self.info.state.accel_control_pedal_target - + self.info.state.throttle_lower_border) / + abs(self.info.restrictions.max_pedal)) + elif self.info.state.accel_control_pedal_target > self.info.state.brake_upper_border: + self.info.state.status = "coasting" # no control required - self.vehicle_control.brake = 0.0 - self.vehicle_control.throttle = 0.0 + self.info.output.brake = 0.0 + self.info.output.throttle = 0.0 else: - self.info.status = "braking" + self.info.state.status = "braking" # braking required - self.vehicle_control.brake = ( - (self.info.brake_upper_border - self.info.accel_control_pedal_target) / - abs(self.info.max_decel)) - self.vehicle_control.throttle = 0.0 + self.info.output.brake = ( + (self.info.state.brake_upper_border - + self.info.state.accel_control_pedal_target) / + abs(self.info.restrictions.max_pedal)) + self.info.output.throttle = 0.0 # finally clip the final control output (should actually never happen) - self.vehicle_control.brake = numpy.clip( - self.vehicle_control.brake, 0., 1.) - self.vehicle_control.throttle = numpy.clip( - self.vehicle_control.throttle, 0., 1.) + self.info.output.brake = numpy.clip( + self.info.output.brake, 0., 1.) + self.info.output.throttle = numpy.clip( + self.info.output.throttle, 0., 1.) diff --git a/src/carla_ros_bridge/physics.py b/src/carla_ros_bridge/physics.py index 2e24b9de..cb11f82e 100644 --- a/src/carla_ros_bridge/physics.py +++ b/src/carla_ros_bridge/physics.py @@ -288,7 +288,7 @@ def get_slope_force(carla_vehicle): dummy_roll, pitch, dummy_yaw = trans.carla_rotation_to_RPY( carla_vehicle.get_transform().rotation) slope_force = get_acceleration_of_gravity( - carla_vehicle) * get_vehicle_mass(carla_vehicle) * math.sin(pitch) + carla_vehicle) * get_vehicle_mass(carla_vehicle) * math.sin(-pitch) return slope_force diff --git a/src/carla_ros_bridge/transforms.py b/src/carla_ros_bridge/transforms.py index d73a5573..34f7ead3 100644 --- a/src/carla_ros_bridge/transforms.py +++ b/src/carla_ros_bridge/transforms.py @@ -106,7 +106,7 @@ def carla_rotation_to_RPY(carla_rotation): :rtype: tuple """ roll = -math.radians(carla_rotation.roll) - pitch = math.radians(carla_rotation.pitch) + pitch = -math.radians(carla_rotation.pitch) yaw = -math.radians(carla_rotation.yaw) return (roll, pitch, yaw)