diff --git a/CMakeLists.txt b/CMakeLists.txt index 2288edb9..a5a6623e 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,23 @@ catkin_python_setup() add_message_files( FILES CarlaVehicleControl.msg + EgoVehicleControlCurrent.msg + EgoVehicleControlInfo.msg + EgoVehicleControlMaxima.msg + EgoVehicleControlState.msg + EgoVehicleControlTarget.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..96c455da --- /dev/null +++ b/client_with_rqt.launch @@ -0,0 +1,4 @@ + + + + diff --git a/config/AckermannControl.perspective b/config/AckermannControl.perspective new file mode 100644 index 00000000..a9608058 --- /dev/null +++ b/config/AckermannControl.perspective @@ -0,0 +1,477 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'rqt_reconfigure/Param': [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''" + }, + "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''" + }, + "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''" + }, + "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\"[{'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": {} + } + } + }, + "plugin__rqt_topic__TopicPlugin__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "tree_widget_header_state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000010000000000000000010000000000000000000000000000000000000728000000050101000100000000000000000500000064ffffffff000000810000000300000005000002c100000001000000030000022d00000001000000030000007200000001000000030000003d00000001000000030000018b0000000100000003000003e800')", + "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 I e ) r / j 4 G { 0 8 I " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} \ No newline at end of file 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 new file mode 100755 index 00000000..8e80c53f --- /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.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.) + +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..aa014dda 100644 --- a/config/settings.yaml +++ b/config/settings.yaml @@ -3,5 +3,30 @@ 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 + # 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 new file mode 100644 index 00000000..ebb2f1ad --- /dev/null +++ b/msg/EgoVehicleControlInfo.msg @@ -0,0 +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 + +# the restrictions +EgoVehicleControlMaxima restrictions + +# the target values +EgoVehicleControlTarget target + +# the currently measured values +EgoVehicleControlCurrent current + +# the current control state +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 214d7a0c..bd8d9038 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): """ @@ -190,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 eb1b4ead..a54ea7d9 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,10 +98,12 @@ 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 """ - 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 21d64d5d..dcde660c 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,28 +67,54 @@ 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) + # control info + self.info = EgoVehicleControlInfo() - self.control_subscriber = rospy.Subscriber( - self.topic_name() + "/vehicle_control_cmd", - CarlaVehicleControl, self.control_command_updated) + # 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 - def destroy(self): - """ - Function (override) to destroy this object. + # 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. - Terminate ROS subscription on AckermannDrive control commands. - Finally forward call to super class. + # 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. - :return: - """ - rospy.logdebug("Destroy EgoVehicle(id={})".format(self.get_id())) - self.control_subscriber = None - super(EgoVehicle, self).destroy() + # 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): """ @@ -89,6 +149,113 @@ 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): + + """ + 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 @@ -100,15 +267,350 @@ 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 + self.info.output = ros_vehicle_control + self.apply_control() - 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) + + # 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 + 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 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 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_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 + """ + 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( + 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.restrictions.max_speed: + rospy.logerr("Max speed reached, clipping value") + 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) + + def set_target_accel(self, target_accel): + """ + set target accel + """ + epsilon = 0.00001 + # 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 = 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 + """ + # perform actual control + self.control_steering() + self.control_stop_and_reverse() + self.run_speed_control_loop() + self.run_accel_control_loop() + if not self.info.output.hand_brake: + self.update_drive_vehicle_control_command() + + # apply control command to CARLA + self.apply_control() + + def control_steering(self): + """ + Basic steering control + """ + self.info.output.steer = self.info.target.steering_angle / \ + self.info.restrictions.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.info.output.hand_brake = False + if self.info.current.speed_abs < standing_still_epsilon: + # standing still, change of driving direction allowed + 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.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.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.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)) + self.set_target_speed(0.) + + 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 + """ + # 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): + """ + 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.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.state.brake_upper_border = self.info.state.throttle_lower_border + \ + phys.get_vehicle_lay_off_engine_acceleration(self.carla_actor) + + 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.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.info.output.brake = 0.0 + self.info.output.throttle = 0.0 + else: + self.info.state.status = "braking" + # braking required + 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.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/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..cb11f82e --- /dev/null +++ b/src/carla_ros_bridge/physics.py @@ -0,0 +1,344 @@ +#!/usr/bin/env python + +# +# Copyright (c) 2018 Intel Labs. +# +# authors: Bernd Gassmann (bernd.gassmann@intel.com) +# +""" +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..34f7ead3 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) + 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)