diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a03662df..63a53df7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -86,10 +86,10 @@ jobs: strategy: matrix: include: - - docker_image: melodic-robot - ros_distro: melodic - ros_python_version: 2 - ros_version: 1 + #- docker_image: melodic-robot + # ros_distro: melodic + # ros_python_version: 2 + # ros_version: 1 - docker_image: noetic-robot ros_distro: noetic diff --git a/CHANGELOG.md b/CHANGELOG.md index 145615f7..c52e6613 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,12 @@ ## Latest +## CARLA-ROS-Bridge 0.9.13 + +* Disable vegetation on MarkerSensor to prevent from millions of boxes in maps with larger vegetation +* carla_ad_agent: Fill CarlaEgoVehicleControl message header +* Added workaround for vehicle classification +* Fixed use of duplicated topic names for different message types in ROS2 + ## CARLA-ROS-Bridge 0.9.12 * Fixed scenario runner node shutdown for foxy diff --git a/carla_ackermann_control/CMakeLists.txt b/carla_ackermann_control/CMakeLists.txt deleted file mode 100644 index 650bb301..00000000 --- a/carla_ackermann_control/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(carla_ackermann_control) - -# Find catkin macros and libraries -find_package( - catkin REQUIRED COMPONENTS rospy std_msgs ackermann_msgs dynamic_reconfigure - carla_msgs roslaunch) - -catkin_python_setup() - -generate_dynamic_reconfigure_options(config/EgoVehicleControlParameter.cfg) - -if(CATKIN_ENABLE_TESTING) - roslaunch_add_file_check(launch) -endif() - -catkin_package() - -install(PROGRAMS src/carla_ackermann_control/carla_ackermann_control_node.py - src/carla_ackermann_control/carla_control_physics.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - -install(DIRECTORY config/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) diff --git a/carla_ackermann_control/README.md b/carla_ackermann_control/README.md deleted file mode 100644 index 0928ab91..00000000 --- a/carla_ackermann_control/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# CARLA Ackermann Control - -Find documentation about the CARLA Ackermann Control package [__here__](https://carla.readthedocs.io/projects/ros-bridge/en/latest/carla_ackermann_control/). diff --git a/carla_ackermann_control/config/AckermannControl.perspective b/carla_ackermann_control/config/AckermannControl.perspective deleted file mode 100644 index a9608058..00000000 --- a/carla_ackermann_control/config/AckermannControl.perspective +++ /dev/null @@ -1,477 +0,0 @@ -{ - "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/carla_ackermann_control/config/AckermannPlot.perspective b/carla_ackermann_control/config/AckermannPlot.perspective deleted file mode 100644 index 5db0d105..00000000 --- a/carla_ackermann_control/config/AckermannPlot.perspective +++ /dev/null @@ -1,516 +0,0 @@ -{ - "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/carla_ackermann_control/config/EgoVehicleControlParameter.cfg b/carla_ackermann_control/config/EgoVehicleControlParameter.cfg deleted file mode 100755 index a3790326..00000000 --- a/carla_ackermann_control/config/EgoVehicleControlParameter.cfg +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "carla_ackermann_control" - -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_ackermann_control", "EgoVehicleControlParameter")) diff --git a/carla_ackermann_control/config/settings.ros1.yaml b/carla_ackermann_control/config/settings.ros1.yaml deleted file mode 100644 index 036568db..00000000 --- a/carla_ackermann_control/config/settings.ros1.yaml +++ /dev/null @@ -1,16 +0,0 @@ -# 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 diff --git a/carla_ackermann_control/config/settings.yaml b/carla_ackermann_control/config/settings.yaml deleted file mode 100644 index c49e3f72..00000000 --- a/carla_ackermann_control/config/settings.yaml +++ /dev/null @@ -1,18 +0,0 @@ -carla_ackermann_control: - ros__parameters: - # override the default values of the pid speed controller - # (only relevant for ackermann control mode) - speed_Kp: 0.05 # min: 0, max: 1. - speed_Ki: 0.00 # min: 0, max: 1. - speed_Kd: 0.50 # min: 0, max: 10. - # override the default values of the pid acceleration controller - # (only relevant for ackermann control mode) - accel_Kp: 0.05 # min: 0, max: 10. - accel_Ki: 0.00 # min: 0, max: 10. - accel_Kd: 0.05 # min: 0, max: 10. - # 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 diff --git a/carla_ackermann_control/launch/carla_ackermann_control.launch b/carla_ackermann_control/launch/carla_ackermann_control.launch deleted file mode 100644 index de92a4b9..00000000 --- a/carla_ackermann_control/launch/carla_ackermann_control.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/carla_ackermann_control/launch/carla_ackermann_control.launch.py b/carla_ackermann_control/launch/carla_ackermann_control.launch.py deleted file mode 100644 index 6fda877a..00000000 --- a/carla_ackermann_control/launch/carla_ackermann_control.launch.py +++ /dev/null @@ -1,36 +0,0 @@ -from pathlib import Path - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='role_name', - default_value='ego_vehicle' - ), - launch.actions.DeclareLaunchArgument( - name='control_loop_rate', - default_value='0.05' - ), - launch_ros.actions.Node( - package='carla_ackermann_control', - executable='carla_ackermann_control_node', - name='carla_ackermann_control', - output='screen', - parameters=[ - Path(get_package_share_directory('carla_ackermann_control'), "settings.yaml"), - { - 'role_name': launch.substitutions.LaunchConfiguration('role_name'), - 'control_loop_rate': launch.substitutions.LaunchConfiguration('control_loop_rate') - } - ] - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/carla_ackermann_control/launch/carla_ros_bridge_with_ackermann_control.launch b/carla_ackermann_control/launch/carla_ros_bridge_with_ackermann_control.launch deleted file mode 100644 index c53090d9..00000000 --- a/carla_ackermann_control/launch/carla_ros_bridge_with_ackermann_control.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/carla_ackermann_control/launch/carla_ros_bridge_with_ackermann_control.launch.py b/carla_ackermann_control/launch/carla_ros_bridge_with_ackermann_control.launch.py deleted file mode 100644 index ab402129..00000000 --- a/carla_ackermann_control/launch/carla_ros_bridge_with_ackermann_control.launch.py +++ /dev/null @@ -1,26 +0,0 @@ -import os - -import launch -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'carla_ros_bridge'), 'carla_ros_bridge.launch.py') - ) - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'carla_ackermann_control'), 'carla_ackermann_control.launch.py') - ) - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/carla_ackermann_control/package.xml b/carla_ackermann_control/package.xml deleted file mode 100644 index 35cc1cbd..00000000 --- a/carla_ackermann_control/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - carla_ackermann_control - 0.0.0 - The carla_ackermann_control package - CARLA Simulator Team - MIT - - ackermann_msgs - carla_ackermann_msgs - carla_msgs - carla_ros_bridge - ros_compatibility - std_msgs - - - rclpy - tf2_ros - - - catkin - rospy - roslaunch - rospy - dynamic_reconfigure - rospy - - rosidl_interface_packages - - - catkin - ament_python - - diff --git a/carla_ackermann_control/setup.cfg b/carla_ackermann_control/setup.cfg deleted file mode 100644 index c68e601a..00000000 --- a/carla_ackermann_control/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/carla_ackermann_control -[install] -install_scripts=$base/lib/carla_ackermann_control diff --git a/carla_ackermann_control/setup.py b/carla_ackermann_control/setup.py deleted file mode 100644 index e7a840f1..00000000 --- a/carla_ackermann_control/setup.py +++ /dev/null @@ -1,46 +0,0 @@ -""" -Setup for carla_ackermann_control -""" -import os -from glob import glob -ROS_VERSION = int(os.environ['ROS_VERSION']) - -if ROS_VERSION == 1: - from distutils.core import setup - from catkin_pkg.python_setup import generate_distutils_setup - - d = generate_distutils_setup( - packages=['carla_ackermann_control'], - package_dir={'': 'src'} - ) - - setup(**d) - -elif ROS_VERSION == 2: - from setuptools import setup - - package_name = 'carla_ackermann_control' - setup( - name=package_name, - version='0.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), - (os.path.join('share', package_name), ['package.xml']), - (os.path.join('share', package_name), glob('launch/*.launch.py')), - (os.path.join('share', package_name), ['config/settings.yaml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='CARLA Simulator Team', - maintainer_email='carla.simulator@gmail.com', - description='The carla_ackermann_control package', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - '{}_node = {}.{}_node:main'.format(package_name, package_name, package_name) - ], - }, - package_dir={'': 'src'}, - ) diff --git a/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py b/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py deleted file mode 100755 index 806b7c96..00000000 --- a/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py +++ /dev/null @@ -1,598 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Control Carla ego vehicle by using AckermannDrive messages -""" - -import sys - -import numpy -from simple_pid import PID # pylint: disable=import-error,wrong-import-order - -import ros_compatibility as roscomp -from ros_compatibility.node import CompatibleNode - -from carla_ackermann_control import carla_control_physics as phys - -from ackermann_msgs.msg import AckermannDrive # pylint: disable=import-error,wrong-import-order -from std_msgs.msg import Header # pylint: disable=wrong-import-order -from carla_msgs.msg import CarlaEgoVehicleStatus # pylint: disable=no-name-in-module,import-error -from carla_msgs.msg import CarlaEgoVehicleControl # pylint: disable=no-name-in-module,import-error -from carla_msgs.msg import CarlaEgoVehicleInfo # pylint: disable=no-name-in-module,import-error -from carla_ackermann_msgs.msg import EgoVehicleControlInfo # pylint: disable=no-name-in-module,import-error - -ROS_VERSION = roscomp.get_ros_version() - -if ROS_VERSION == 1: - from carla_ackermann_control.cfg import EgoVehicleControlParameterConfig # pylint: disable=no-name-in-module,import-error,ungrouped-imports - from dynamic_reconfigure.server import Server # pylint: disable=no-name-in-module,import-error -if ROS_VERSION == 2: - from rcl_interfaces.msg import SetParametersResult - - -class CarlaAckermannControl(CompatibleNode): - - """ - Convert ackermann_drive messages to carla VehicleCommand with a PID controller - """ - - def __init__(self): - """ - Constructor - - """ - super(CarlaAckermannControl, self).__init__("carla_ackermann_control") - - # PID controller - # the controller has to run with the simulation time, not with real-time - # - # To prevent "float division by zero" within PID controller initialize it with - # a previous point in time (the error happens because the time doesn't - # change between initialization and first call, therefore dt is 0) - sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access - lambda: self.get_time() - 0.1) - - # we might want to use a PID controller to reach the final target speed - self.speed_controller = PID(Kp=self.get_param("speed_Kp", alternative_value=0.05), - Ki=self.get_param("speed_Ki", alternative_value=0.), - Kd=self.get_param("speed_Kd", alternative_value=0.5), - sample_time=0.05, - output_limits=(-1., 1.)) - self.accel_controller = PID(Kp=self.get_param("accel_Kp", alternative_value=0.05), - Ki=self.get_param("accel_Ki", alternative_value=0.), - Kd=self.get_param("accel_Kd", alternative_value=0.05), - sample_time=0.05, - output_limits=(-1, 1)) - - # use the correct time for further calculations - sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access - lambda: self.get_time()) - - if ROS_VERSION == 1: - self.reconfigure_server = Server( - EgoVehicleControlParameterConfig, - namespace="", - callback=self.reconfigure_pid_parameters, - ) - if ROS_VERSION == 2: - self. add_on_set_parameters_callback(self.reconfigure_pid_parameters) - - self.control_loop_rate = self.get_param("control_loop_rate", 0.05) - self.last_ackermann_msg_received_sec = self.get_time() - self.vehicle_status = CarlaEgoVehicleStatus() - self.vehicle_info = CarlaEgoVehicleInfo() - self.role_name = self.get_param('role_name', 'ego_vehicle') - # control info - self.info = EgoVehicleControlInfo() - - # set initial maximum values - self.vehicle_info_updated(self.vehicle_info) - - # 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_time() - self.info.current.speed = 0. - self.info.current.speed_abs = 0. - self.info.current.accel = 0. - - # control values - self.info.status.status = 'n/a' - self.info.status.speed_control_activation_count = 0 - self.info.status.speed_control_accel_delta = 0. - self.info.status.speed_control_accel_target = 0. - self.info.status.accel_control_pedal_delta = 0. - self.info.status.accel_control_pedal_target = 0. - self.info.status.brake_upper_border = 0. - self.info.status.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 - - # ackermann drive commands - self.control_subscriber = self.new_subscription( - AckermannDrive, - "/carla/" + self.role_name + "/ackermann_cmd", - self.ackermann_command_updated, - qos_profile=10 - ) - - # current status of the vehicle - self.vehicle_status_subscriber = self.new_subscription( - CarlaEgoVehicleStatus, - "/carla/" + self.role_name + "/vehicle_status", - self.vehicle_status_updated, - qos_profile=10 - ) - - # vehicle info - self.vehicle_info_subscriber = self.new_subscription( - CarlaEgoVehicleInfo, - "/carla/" + self.role_name + "/vehicle_info", - self.vehicle_info_updated, - qos_profile=10 - ) - - # to send command to carla - self.carla_control_publisher = self.new_publisher( - CarlaEgoVehicleControl, - "/carla/" + self.role_name + "/vehicle_control_cmd", - qos_profile=1) - - # report controller info - self.control_info_publisher = self.new_publisher( - EgoVehicleControlInfo, - "/carla/" + self.role_name + "/ackermann_control/control_info", - qos_profile=1) - - if ROS_VERSION == 1: - - def reconfigure_pid_parameters(self, ego_vehicle_control_parameter, _level): - """ - Callback for dynamic reconfigure call to set the PID parameters - - :param ego_vehicle_control_parameter: - :type ego_vehicle_control_parameter: \ - carla_ackermann_control.cfg.EgoVehicleControlParameterConfig - - """ - self.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 - - if ROS_VERSION == 2: - - def reconfigure_pid_parameters(self, params): # pylint: disable=function-redefined - """Check and update the node's parameters.""" - param_values = {p.name: p.value for p in params} - - pid_param_names = { - "speed_Kp", - "speed_Ki", - "speed_Kd", - "accel_Kp", - "accel_Ki", - "accel_Kd", - } - common_names = pid_param_names.intersection(param_values.keys()) - if not common_names: - # No work do to - return SetParametersResult(successful=True) - - if any(p.value is None for p in params): - return SetParametersResult( - successful=False, reason="Parameter must have a value assigned" - ) - - self.speed_controller.tunings = ( - param_values.get("speed_Kp", self.speed_controller.Kp), - param_values.get("speed_Ki", self.speed_controller.Ki), - param_values.get("speed_Kd", self.speed_controller.Kd), - ) - self.accel_controller.tunings = ( - param_values.get("accel_Kp", self.accel_controller.Kp), - param_values.get("accel_Ki", self.accel_controller.Ki), - param_values.get("accel_Kd", self.accel_controller.Kd), - ) - - self.loginfo( - "Reconfigure Request: speed ({}, {}, {}), accel ({}, {}, {})".format( - self.speed_controller.tunings[0], - self.speed_controller.tunings[1], - self.speed_controller.tunings[2], - self.accel_controller.tunings[0], - self.accel_controller.tunings[1], - self.accel_controller.tunings[2] - ) - ) - - return SetParametersResult(successful=True) - - def get_msg_header(self): - """ - Get a filled ROS message header - :return: ROS message header - :rtype: std_msgs.msg.Header - """ - header = Header() - header.frame_id = "map" - header.stamp = roscomp.ros_timestamp(sec=self.get_time(), from_sec=True) - return header - - def vehicle_status_updated(self, vehicle_status): - """ - Stores the ackermann drive message for the next controller calculation - - :param ros_ackermann_drive: the current ackermann control input - :type ros_ackermann_drive: ackermann_msgs.AckermannDrive - :return: - """ - - # set target values - self.vehicle_status = vehicle_status - - def vehicle_info_updated(self, vehicle_info): - """ - Stores the ackermann drive message for the next controller calculation - - :param ros_ackermann_drive: the current ackermann control input - :type ros_ackermann_drive: ackermann_msgs.AckermannDrive - :return: - """ - # set target values - self.vehicle_info = vehicle_info - - # calculate restrictions - self.info.restrictions.max_steering_angle = phys.get_vehicle_max_steering_angle( - self.vehicle_info) - self.info.restrictions.max_speed = phys.get_vehicle_max_speed( - self.vehicle_info) - self.info.restrictions.max_accel = phys.get_vehicle_max_acceleration( - self.vehicle_info) - self.info.restrictions.max_decel = phys.get_vehicle_max_deceleration( - self.vehicle_info) - self.info.restrictions.min_accel = self.get_param('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) - - def ackermann_command_updated(self, ros_ackermann_drive): - """ - Stores the ackermann drive message for the next controller calculation - - :param ros_ackermann_drive: the current ackermann control input - :type ros_ackermann_drive: ackermann_msgs.AckermannDrive - :return: - """ - self.last_ackermann_msg_received_sec = self.get_time() - # 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: - self.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: - self.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 CarlaEgoVehicleControl 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() - - # only send out the Carla Control Command if AckermannDrive messages are - # received in the last second (e.g. to allows manually controlling the vehicle) - if (self.last_ackermann_msg_received_sec + 1.0) > \ - self.get_time(): - self.info.output.header = self.get_msg_header() - self.carla_control_publisher.publish(self.info.output) - - 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.status.status = "standing" - if self.info.target.speed < 0: - if not self.info.output.reverse: - self.loginfo( - "VehicleControl: Change of driving direction to reverse") - self.info.output.reverse = True - elif self.info.target.speed > 0: - if self.info.output.reverse: - self.loginfo( - "VehicleControl: Change of driving direction to forward") - self.info.output.reverse = False - if self.info.target.speed_abs < full_stop_epsilon: - self.info.status.status = "full stop" - self.info.status.speed_control_accel_target = 0. - self.info.status.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 - self.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.status.speed_control_activation_count < 5: - self.info.status.speed_control_activation_count += 1 - else: - if self.info.status.speed_control_activation_count > 0: - self.info.status.speed_control_activation_count -= 1 - # set the auto_mode of the controller accordingly - self.speed_controller.auto_mode = self.info.status.speed_control_activation_count >= 5 - - if self.speed_controller.auto_mode: - self.speed_controller.setpoint = self.info.target.speed_abs - self.info.status.speed_control_accel_delta = float(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.status.speed_control_accel_target = numpy.clip( - self.info.status.speed_control_accel_target + - self.info.status.speed_control_accel_delta, - clipping_lower_border, clipping_upper_border) - else: - self.info.status.speed_control_accel_delta = 0. - self.info.status.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.status.speed_control_accel_target - self.info.status.accel_control_pedal_delta = float(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.status.accel_control_pedal_target = numpy.clip( - self.info.status.accel_control_pedal_target + - self.info.status.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.status.throttle_lower_border = phys.get_vehicle_driving_impedance_acceleration( - self.vehicle_info, self.vehicle_status, 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.status.brake_upper_border = self.info.status.throttle_lower_border + \ - phys.get_vehicle_lay_off_engine_acceleration(self.vehicle_info) - - if self.info.status.accel_control_pedal_target > self.info.status.throttle_lower_border: - self.info.status.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.status.accel_control_pedal_target - - self.info.status.throttle_lower_border) / - abs(self.info.restrictions.max_pedal)) - elif self.info.status.accel_control_pedal_target > self.info.status.brake_upper_border: - self.info.status.status = "coasting" - # no control required - self.info.output.brake = 0.0 - self.info.output.throttle = 0.0 - else: - self.info.status.status = "braking" - # braking required - self.info.output.brake = ( - (self.info.status.brake_upper_border - - self.info.status.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.) - - # from ego vehicle - def send_ego_vehicle_control_info_msg(self): - """ - Function to send carla_ackermann_control.msg.EgoVehicleControlInfo message. - - :return: - """ - self.info.header = self.get_msg_header() - self.control_info_publisher.publish(self.info) - - 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_time() - delta_time = current_time_sec - self.info.current.time_sec - current_speed = self.vehicle_status.velocity - 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 run(self): - """ - - Control loop - - :return: - """ - - def loop(timer_event=None): - self.update_current_values() - self.vehicle_control_cycle() - self.send_ego_vehicle_control_info_msg() - - self.new_timer(self.control_loop_rate, loop) - self.spin() - - -def main(args=None): - """ - - main function - - :return: - """ - roscomp.init("carla_ackermann_control", args=args) - - try: - controller = CarlaAckermannControl() - controller.run() - except KeyboardInterrupt: - pass - finally: - roscomp.shutdown() - -if __name__ == "__main__": - main() diff --git a/carla_ackermann_control/src/carla_ackermann_control/carla_control_physics.py b/carla_ackermann_control/src/carla_ackermann_control/carla_control_physics.py deleted file mode 100644 index c93fa4d4..00000000 --- a/carla_ackermann_control/src/carla_ackermann_control/carla_control_physics.py +++ /dev/null @@ -1,256 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Tool functions to calculate vehicle physics -""" - -import math - -from transforms3d.euler import quat2euler - - -def get_vehicle_lay_off_engine_acceleration(vehicle_info): - """ - Calculate the acceleration a carla vehicle faces by the engine on lay off - - This respects the following forces: - - engine brake force - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: acceleration the vehicle [m/s^2 < 0] - :rtype: float64 - """ - return -get_engine_brake_force(vehicle_info) / get_vehicle_mass(vehicle_info) - - -def get_engine_brake_force(_): - """ - 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 vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: engine braking force [N] - :rtype: float64 - """ - return 500.0 - - -def get_vehicle_mass(vehicle_info): - """ - Get the mass of a carla vehicle (defaults to 1500kg) - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: mass of a carla vehicle [kg] - :rtype: float64 - """ - mass = 1500.0 - if vehicle_info.mass: - mass = vehicle_info.mass - - return mass - - -def get_vehicle_driving_impedance_acceleration(vehicle_info, vehicle_status, 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 vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :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(vehicle_info) - aerodynamic_drag_force = get_aerodynamic_drag_force(vehicle_status) - slope_force = get_slope_force(vehicle_info, vehicle_status) - if reverse: - slope_force = -slope_force - deceleration = -(rolling_resistance_force + - aerodynamic_drag_force + - slope_force) / \ - get_vehicle_mass(vehicle_info) - - return deceleration - - -def get_rolling_resistance_force(vehicle_info): - """ - Calculate the rolling resistance force of a carla vehicle - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :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 - # @todo: currently not within vehicle_info - rolling_resistance_coefficient = 0.01 - normal_force = get_weight_force(vehicle_info) - - rolling_resistance_force = rolling_resistance_coefficient * normal_force - - return rolling_resistance_force - - -def get_weight_force(vehicle_info): - """ - Get the weight of a carla vehicle - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: weight of the vehicle [N] - :rtype: float64 - """ - weight = get_vehicle_mass(vehicle_info) * \ - get_acceleration_of_gravity(vehicle_info) - - return weight - - -def get_acceleration_of_gravity(_): - """ - Get the acceleration of gravity for a carla vehicle - (for the moment constant at 9.81 m/s^2) - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: acceleration of gravity [m/s^2] - :rtype: float64 - """ - acceleration = 9.81 - - return acceleration - - -def get_aerodynamic_drag_force(vehicle_status): - """ - Calculate the aerodynamic drag force of a carla vehicle - - :param vehicle_status: the ego vehicle status - :type vehicle_status: carla_ros_bridge.CarlaEgoVehicleStatus - :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 - # @todo currently not provided in vehicle_info - drag_area = default_aerodynamic_drag_coefficient * default_drag_reference_area - rho_air_25 = 1.184 - speed_squared = vehicle_status.velocity * vehicle_status.velocity - - aerodynamic_drag_force = 0.5 * drag_area * rho_air_25 * speed_squared - return aerodynamic_drag_force - - -def get_slope_force(vehicle_info, vehicle_status): - """ - Calculate the force of a carla vehicle faces when driving on a slope. - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :param vehicle_status: the ego vehicle status - :type vehicle_status: carla_ros_bridge.CarlaEgoVehicleStatus - :return: slope force [N, >0 uphill, <0 downhill] - :rtype: float64 - """ - dummy_roll, pitch, dummy_yaw = quat2euler( - [vehicle_status.orientation.w, vehicle_status.orientation.x, - vehicle_status.orientation.y, vehicle_status.orientation.z]) - slope_force = get_acceleration_of_gravity( - vehicle_info) * get_vehicle_mass(vehicle_info) * math.sin(-pitch) - return slope_force - - -def get_vehicle_max_steering_angle(vehicle_info): - """ - Get the maximum steering angle of a carla vehicle - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: maximum steering angle [radians] - :rtype: float64 - """ - # 70 degrees is the default max steering angle of a car - max_steering_angle = math.radians(70) - # get max steering angle (use smallest non-zero value of all wheels) - for wheel in vehicle_info.wheels: - if wheel.max_steer_angle: - if wheel.max_steer_angle and wheel.max_steer_angle < max_steering_angle: - max_steering_angle = wheel.max_steer_angle - return max_steering_angle - - -def get_vehicle_max_speed(_): - """ - Get the maximum speed of a carla vehicle - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: maximum speed [m/s] - :rtype: float64 - """ - # 180 km/h is the default max speed of a car - max_speed = 180.0 / 3.6 - - return max_speed - - -def get_vehicle_max_acceleration(_): - """ - Get the maximum acceleration of a carla vehicle - - default: 3.0 m/s^2: 0-100 km/h in 9.2 seconds - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: maximum acceleration [m/s^2 > 0] - :rtype: float64 - """ - max_acceleration = 3.0 - - return max_acceleration - - -def get_vehicle_max_deceleration(_): - """ - Get the maximum deceleration of a carla vehicle - - default: 8 m/s^2 - - :param vehicle_info: the vehicle info - :type vehicle_info: carla_ros_bridge.CarlaEgoVehicleInfo - :return: maximum deceleration [m/s^2 > 0] - :rtype: float64 - """ - max_deceleration = 8.0 - - return max_deceleration diff --git a/carla_ackermann_msgs/CMakeLists.txt b/carla_ackermann_msgs/CMakeLists.txt deleted file mode 100644 index 69398ae9..00000000 --- a/carla_ackermann_msgs/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -project(carla_ackermann_msgs) - -find_package(ros_environment REQUIRED) - -set(ROS_VERSION $ENV{ROS_VERSION}) - -set(MSG_FILES - EgoVehicleControlInfo.msg EgoVehicleControlCurrent.msg - EgoVehicleControlMaxima.msg EgoVehicleControlStatus.msg - EgoVehicleControlTarget.msg) - -if(${ROS_VERSION} EQUAL 1) - cmake_minimum_required(VERSION 2.8.3) - - # Find catkin macros and libraries - find_package(catkin REQUIRED COMPONENTS message_generation std_msgs - carla_msgs) - - add_message_files(DIRECTORY msg FILES ${MSG_FILES}) - - generate_messages(DEPENDENCIES std_msgs carla_msgs) - - catkin_package(CATKIN_DEPENDS message_runtime std_msgs carla_msgs) - -elseif(${ROS_VERSION} EQUAL 2) - - cmake_minimum_required(VERSION 3.5) - - if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_STANDARD 14) - endif() - - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) - endif() - - find_package(ament_cmake REQUIRED) - find_package(builtin_interfaces REQUIRED) - find_package(std_msgs REQUIRED) - find_package(carla_msgs REQUIRED) - find_package(rosidl_default_generators REQUIRED) - - # Apend "msg/" to each file name - set(TEMP_LIST "") - foreach(MSG_FILE ${MSG_FILES}) - list(APPEND TEMP_LIST "msg/${MSG_FILE}") - endforeach() - set(MSG_FILES ${TEMP_LIST}) - - rosidl_generate_interfaces( - ${PROJECT_NAME} - ${MSG_FILES} - DEPENDENCIES - builtin_interfaces - std_msgs - carla_msgs - ADD_LINTER_TESTS) - - ament_export_dependencies(rosidl_default_runtime) - - if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - endif() - - ament_package() - -endif() diff --git a/carla_ackermann_msgs/msg/EgoVehicleControlCurrent.msg b/carla_ackermann_msgs/msg/EgoVehicleControlCurrent.msg deleted file mode 100644 index f2e00ad9..00000000 --- a/carla_ackermann_msgs/msg/EgoVehicleControlCurrent.msg +++ /dev/null @@ -1,12 +0,0 @@ -# -# Copyright (c) 2018-2019 Intel Corporation. -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -# 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/carla_ackermann_msgs/msg/EgoVehicleControlInfo.msg b/carla_ackermann_msgs/msg/EgoVehicleControlInfo.msg deleted file mode 100644 index 2533ebe1..00000000 --- a/carla_ackermann_msgs/msg/EgoVehicleControlInfo.msg +++ /dev/null @@ -1,24 +0,0 @@ -# -# Copyright (c) 2018-2019 Intel Corporation. -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -# This represents an info message of the ego vehicle - -std_msgs/Header header - -# the restrictions -EgoVehicleControlMaxima restrictions - -# the target values -EgoVehicleControlTarget target - -# the currently measured values -EgoVehicleControlCurrent current - -# the current control status -EgoVehicleControlStatus status - -# the current control output to CARLA -carla_msgs/CarlaEgoVehicleControl output diff --git a/carla_ackermann_msgs/msg/EgoVehicleControlMaxima.msg b/carla_ackermann_msgs/msg/EgoVehicleControlMaxima.msg deleted file mode 100644 index 31e950d4..00000000 --- a/carla_ackermann_msgs/msg/EgoVehicleControlMaxima.msg +++ /dev/null @@ -1,15 +0,0 @@ -# -# Copyright (c) 2018-2019 Intel Corporation. -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -# 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/carla_ackermann_msgs/msg/EgoVehicleControlStatus.msg b/carla_ackermann_msgs/msg/EgoVehicleControlStatus.msg deleted file mode 100644 index 57bf9e34..00000000 --- a/carla_ackermann_msgs/msg/EgoVehicleControlStatus.msg +++ /dev/null @@ -1,23 +0,0 @@ -# -# Copyright (c) 2018-2019 Intel Corporation. -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -# This represents the control status variables of the ego vehicle control - -# the current control status -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/carla_ackermann_msgs/msg/EgoVehicleControlTarget.msg b/carla_ackermann_msgs/msg/EgoVehicleControlTarget.msg deleted file mode 100644 index 22f5ecfc..00000000 --- a/carla_ackermann_msgs/msg/EgoVehicleControlTarget.msg +++ /dev/null @@ -1,13 +0,0 @@ -# -# Copyright (c) 2018-2019 Intel Corporation. -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -# 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/carla_ackermann_msgs/package.xml b/carla_ackermann_msgs/package.xml deleted file mode 100644 index 7379ac92..00000000 --- a/carla_ackermann_msgs/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - carla_ackermann_msgs - 1.0.0 - The carla_ackermann_msgs package - CARLA Simulator Team - MIT - http://carla.org/ - https://github.com/carla-simulator/ros-bridge - https://github.com/carla-simulator/ros-bridge/issues - CARLA Simulator Team - - catkin - ament_cmake - - message_generation - rosidl_default_generators - ros_environment - - builtin_interfaces - std_msgs - carla_msgs - - message_runtime - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - catkin - ament_cmake - - diff --git a/carla_ad_agent/src/carla_ad_agent/ad_agent.py b/carla_ad_agent/src/carla_ad_agent/ad_agent.py index 9a7b792d..827bb5aa 100755 --- a/carla_ad_agent/src/carla_ad_agent/ad_agent.py +++ b/carla_ad_agent/src/carla_ad_agent/ad_agent.py @@ -21,7 +21,7 @@ from carla_ad_agent.agent import Agent, AgentState from carla_msgs.msg import ( - CarlaEgoVehicleInfo, + CarlaVehicleInfo, CarlaActorList, CarlaTrafficLightStatusList, CarlaTrafficLightInfoList) @@ -47,13 +47,14 @@ def __init__(self): self.data_lock = threading.Lock() self._ego_vehicle_pose = None + self._ego_vehicle_id = None self._objects = {} self._lights_status = {} self._lights_info = {} self._target_speed = 0. self.speed_command_publisher = self.new_publisher( - Float64, "/carla/{}/speed_command".format(role_name), + Float64, "/carla/vehicles/{}/speed_command".format(role_name), QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) self._odometry_subscriber = self.new_subscription( @@ -63,9 +64,16 @@ def __init__(self): qos_profile=10 ) + self._vehicle_info_subscriber = self.new_subscription( + CarlaVehicleInfo, + "/carla/vehicles/{}/vehicle_info".format(role_name), + self.vehicle_info_cb, + qos_profile=1 + ) + self._target_speed_subscriber = self.new_subscription( Float64, - "/carla/{}/target_speed".format(role_name), + "/carla/vehicles/{}/target_speed".format(role_name), self.target_speed_cb, qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) ) @@ -74,7 +82,7 @@ def __init__(self): self._objects_subscriber = self.new_subscription( ObjectArray, - "/carla/{}/objects".format(role_name), + "/carla/world/objects".format(role_name), self.objects_cb, qos_profile=10 ) @@ -91,6 +99,11 @@ def __init__(self): qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) ) + def vehicle_info_cb(self, vehicle_info_msg): + with self.data_lock: + self._ego_vehicle_id = vehicle_info_msg.id + self._objects.pop(self._ego_vehicle_id) + def odometry_cb(self, odometry_msg): with self.data_lock: self._ego_vehicle_pose = odometry_msg.pose.pose @@ -102,7 +115,8 @@ def target_speed_cb(self, target_speed_msg): def objects_cb(self, objects_msg): objects = {} for obj in objects_msg.objects: - objects[obj.id] = obj + if obj.id != self._ego_vehicle_id: + objects[obj.id] = obj with self.data_lock: self._objects = objects diff --git a/carla_ad_agent/src/carla_ad_agent/agent.py b/carla_ad_agent/src/carla_ad_agent/agent.py index c79157e5..fd760e65 100644 --- a/carla_ad_agent/src/carla_ad_agent/agent.py +++ b/carla_ad_agent/src/carla_ad_agent/agent.py @@ -23,7 +23,7 @@ from carla_ad_agent.misc import is_within_distance_ahead # pylint: disable=relative-import -from carla_msgs.msg import CarlaEgoVehicleInfo, CarlaTrafficLightStatus +from carla_msgs.msg import CarlaVehicleInfo, CarlaTrafficLightStatus from carla_waypoint_types.srv import GetWaypoint from derived_object_msgs.msg import Object @@ -59,8 +59,8 @@ def __init__(self, name="agent"): role_name = self.get_param("role_name", "ego_vehicle") self.loginfo("Waiting for vehicle_info...") vehicle_info = self.wait_for_message( - "/carla/{}/vehicle_info".format(role_name), - CarlaEgoVehicleInfo, + "/carla/vehicles/{}/vehicle_info".format(role_name), + CarlaVehicleInfo, qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) self.loginfo("Vehicle info received.") self._ego_vehicle_id = vehicle_info.id diff --git a/carla_ad_agent/src/carla_ad_agent/local_planner.py b/carla_ad_agent/src/carla_ad_agent/local_planner.py index 202eae51..49e30fba 100755 --- a/carla_ad_agent/src/carla_ad_agent/local_planner.py +++ b/carla_ad_agent/src/carla_ad_agent/local_planner.py @@ -16,12 +16,12 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode -from ros_compatibility.qos import QoSProfile, DurabilityPolicy +from ros_compatibility.qos import QoSProfile, DurabilityPolicy, QoSProfileSubscriber from carla_ad_agent.vehicle_pid_controller import VehiclePIDController from carla_ad_agent.misc import distance_vehicle -from carla_msgs.msg import CarlaEgoVehicleControl # pylint: disable=import-error +from carla_msgs.msg import CarlaVehicleControl # pylint: disable=import-error from nav_msgs.msg import Odometry, Path from std_msgs.msg import Float64 from visualization_msgs.msg import Marker @@ -59,6 +59,7 @@ def __init__(self): self.data_lock = threading.Lock() + self._current_header = None self._current_pose = None self._current_speed = None self._target_speed = 0.0 @@ -70,28 +71,28 @@ def __init__(self): # subscribers self._odometry_subscriber = self.new_subscription( Odometry, - "/carla/{}/odometry".format(role_name), + "/carla/vehicles/{}/odometry".format(role_name), self.odometry_cb, - qos_profile=10) + QoSProfileSubscriber(depth=10)) self._path_subscriber = self.new_subscription( Path, - "/carla/{}/waypoints".format(role_name), + "/carla/vehicles/{}/waypoints".format(role_name), self.path_cb, QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) self._target_speed_subscriber = self.new_subscription( Float64, - "/carla/{}/speed_command".format(role_name), + "/carla/vehicles/{}/speed_command".format(role_name), self.target_speed_cb, QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) # publishers self._target_pose_publisher = self.new_publisher( Marker, - "/carla/{}/next_target".format(role_name), + "/carla/vehicles/{}/next_target".format(role_name), qos_profile=10) self._control_cmd_publisher = self.new_publisher( - CarlaEgoVehicleControl, - "/carla/{}/vehicle_control_cmd".format(role_name), + CarlaVehicleControl, + "/carla/vehicles/{}/control/vehicle_control_cmd".format(role_name), qos_profile=10) # initializing controller @@ -100,20 +101,25 @@ def __init__(self): def odometry_cb(self, odometry_msg): with self.data_lock: + self._current_header = odometry_msg.header + self._current_header.frame_id = odometry_msg.child_frame_id self._current_pose = odometry_msg.pose.pose self._current_speed = math.sqrt(odometry_msg.twist.twist.linear.x ** 2 + odometry_msg.twist.twist.linear.y ** 2 + odometry_msg.twist.twist.linear.z ** 2) * 3.6 + self.logdebug("Receiving odometry: speed={}".format(self._current_speed)) def target_speed_cb(self, target_speed_msg): with self.data_lock: self._target_speed = target_speed_msg.data + self.logdebug("Receiving target_speed: target_speed={}".format(self._target_speed)) def path_cb(self, path_msg): with self.data_lock: self._waypoint_buffer.clear() self._waypoints_queue.clear() self._waypoints_queue.extend([pose.pose for pose in path_msg.poses]) + self.logdebug("Receiving route: resulting queue {}".format(self._waypoints_queue)) def pose_to_marker_msg(self, pose): marker_msg = Marker() @@ -138,6 +144,11 @@ def run_step(self): self.emergency_stop() return + if (self._current_pose is None) or (self._current_speed is None) or (self._current_header is None): + self.loginfo("Waiting for first odometry message...") + self.emergency_stop() + return + # when target speed is 0, brake. if self._target_speed == 0.0: self.emergency_stop() @@ -158,6 +169,7 @@ def run_step(self): # move using PID controllers control_msg = self._vehicle_controller.run_step( self._target_speed, self._current_speed, self._current_pose, target_pose) + control_msg.header = self._current_header # purge the queue of obsolete waypoints max_index = -1 @@ -175,7 +187,9 @@ def run_step(self): self._control_cmd_publisher.publish(control_msg) def emergency_stop(self): - control_msg = CarlaEgoVehicleControl() + control_msg = CarlaVehicleControl() + if self._current_header: + control_msg.header = self._current_header control_msg.steer = 0.0 control_msg.throttle = 0.0 control_msg.brake = 1.0 diff --git a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py index d2229935..670f975f 100644 --- a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py +++ b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py @@ -13,7 +13,7 @@ import numpy as np from transforms3d.euler import quat2euler from geometry_msgs.msg import Point # pylint: disable=import-error -from carla_msgs.msg import CarlaEgoVehicleControl # pylint: disable=import-error +from carla_msgs.msg import CarlaVehicleControl # pylint: disable=import-error class VehiclePIDController(object): # pylint: disable=too-few-public-methods @@ -54,7 +54,7 @@ def run_step(self, target_speed, current_speed, current_pose, waypoint): :param waypoint: target location encoded as a waypoint :return: control signal (throttle and steering) """ - control = CarlaEgoVehicleControl() + control = CarlaVehicleControl() throttle = self._lon_controller.run_step(target_speed, current_speed) steering = self._lat_controller.run_step(current_pose, waypoint) control.steer = -steering diff --git a/carla_ad_demo/launch/carla_ad_demo.launch b/carla_ad_demo/launch/carla_ad_demo.launch index 2d73c568..50db2631 100644 --- a/carla_ad_demo/launch/carla_ad_demo.launch +++ b/carla_ad_demo/launch/carla_ad_demo.launch @@ -20,7 +20,7 @@ - + diff --git a/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch b/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch index b1ade608..5fc9fb82 100644 --- a/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch +++ b/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch @@ -47,7 +47,6 @@ - diff --git a/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch.py b/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch.py index 2c62407b..e11d0450 100644 --- a/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch.py +++ b/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch.py @@ -49,10 +49,10 @@ def generate_launch_description(): name='carla_twist_to_control', remappings=[ ( - ["/carla/", - launch.substitutions.LaunchConfiguration('role_name'), "/vehicle_control_cmd"], - ["/carla/", - launch.substitutions.LaunchConfiguration('role_name'), "/vehicle_control_cmd_manual"] + ["/carla/vehicles/", + launch.substitutions.LaunchConfiguration('role_name'), "/control/vehicle_control_cmd"], + ["/carla/vehicles/", + launch.substitutions.LaunchConfiguration('role_name'), "/control/vehicle_control_cmd_manual"] ) ], parameters=[ diff --git a/carla_manual_control/src/carla_manual_control/carla_manual_control.py b/carla_manual_control/src/carla_manual_control/carla_manual_control.py index 5cc25d15..58e3443c 100755 --- a/carla_manual_control/src/carla_manual_control/carla_manual_control.py +++ b/carla_manual_control/src/carla_manual_control/carla_manual_control.py @@ -62,12 +62,12 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode -from ros_compatibility.qos import QoSProfile, DurabilityPolicy +from ros_compatibility.qos import QoSProfile, DurabilityPolicy, QoSProfileSubscriber from carla_msgs.msg import CarlaStatus -from carla_msgs.msg import CarlaEgoVehicleInfo -from carla_msgs.msg import CarlaEgoVehicleStatus -from carla_msgs.msg import CarlaEgoVehicleControl +from carla_msgs.msg import CarlaVehicleInfo +from carla_msgs.msg import CarlaVehicleControlStatus +from carla_msgs.msg import CarlaVehicleControl from carla_msgs.msg import CarlaLaneInvasionEvent from carla_msgs.msg import CarlaCollisionEvent from nav_msgs.msg import Odometry @@ -93,15 +93,15 @@ def __init__(self, resolution): self.controller = KeyboardControl(self.role_name, self.hud, self) self.image_subscriber = self.new_subscription( - Image, "/carla/{}/rgb_view/image".format(self.role_name), + Image, "/carla/vehicles/{}/rgb_view/image".format(self.role_name), self.on_view_image, qos_profile=10) self.collision_subscriber = self.new_subscription( - CarlaCollisionEvent, "/carla/{}/collision".format(self.role_name), + CarlaCollisionEvent, "/carla/vehicles/{}/collision".format(self.role_name), self.on_collision, qos_profile=10) self.lane_invasion_subscriber = self.new_subscription( - CarlaLaneInvasionEvent, "/carla/{}/lane_invasion".format(self.role_name), + CarlaLaneInvasionEvent, "/carla/vehicles/{}/lane_invasion".format(self.role_name), self.on_lane_invasion, qos_profile=10) def on_collision(self, data): @@ -169,7 +169,7 @@ def __init__(self, role_name, hud, node): self.node = node self._autopilot_enabled = False - self._control = CarlaEgoVehicleControl() + self._control = CarlaVehicleControl() self._steer_cache = 0.0 fast_qos = QoSProfile(depth=10) @@ -177,19 +177,19 @@ def __init__(self, role_name, hud, node): self.vehicle_control_manual_override_publisher = self.node.new_publisher( Bool, - "/carla/{}/vehicle_control_manual_override".format(self.role_name), + "/carla/vehicles/{}/vehicle_control_manual_override".format(self.role_name), qos_profile=fast_latched_qos) self.vehicle_control_manual_override = False self.auto_pilot_enable_publisher = self.node.new_publisher( Bool, - "/carla/{}/enable_autopilot".format(self.role_name), + "/carla/vehicles/{}/enable_autopilot".format(self.role_name), qos_profile=fast_qos) self.vehicle_control_publisher = self.node.new_publisher( - CarlaEgoVehicleControl, - "/carla/{}/vehicle_control_cmd_manual".format(self.role_name), + CarlaVehicleControl, + "/carla/vehicles/{}/control/vehicle_control_cmd_manual".format(self.role_name), qos_profile=fast_qos) self.carla_status_subscriber = self.node.new_subscription( @@ -314,16 +314,21 @@ def __init__(self, role_name, width, height, node): self.help = HelpText(pygame.font.Font(mono, 24), width, height) self._show_info = True self._info_text = [] - self.vehicle_status = CarlaEgoVehicleStatus() + self.vehicle_control_status = CarlaVehicleControlStatus() - self.vehicle_status_subscriber = node.new_subscription( - CarlaEgoVehicleStatus, "/carla/{}/vehicle_status".format(self.role_name), - self.vehicle_status_updated, qos_profile=10) - - self.vehicle_info = CarlaEgoVehicleInfo() + self.vehicle_odometry_subscriber = self.new_subscription( + Odometry, + "/carla/vehicles/{}/odometry".format(self.role_name), + self.vehicle_odometry_updated, + QoSProfileSubscriber(depth=10)) + self.vehicle_control_status_subscriber = node.new_subscription( + CarlaVehicleControlStatus, "/carla/vehicles/{}/vehicle_control_status".format(self.role_name), + self.vehicle_control_status_updated, qos_profile=10) + + self.vehicle_info = CarlaVehicleInfo() self.vehicle_info_subscriber = node.new_subscription( - CarlaEgoVehicleInfo, - "/carla/{}/vehicle_info".format(self.role_name), + CarlaVehicleInfo, + "/carla/vehicles/{}/vehicle_info".format(self.role_name), self.vehicle_info_updated, qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) @@ -335,20 +340,20 @@ def __init__(self, role_name, width, height, node): self.gnss_subscriber = node.new_subscription( NavSatFix, - "/carla/{}/gnss".format(self.role_name), + "/carla/vehicles/{}/gnss".format(self.role_name), self.gnss_updated, qos_profile=10) - self.odometry_subscriber = node.new_subscription( + self.vehicle_control_status_subscriber = node.new_subscription( Odometry, - "/carla/{}/odometry".format(self.role_name), - self.odometry_updated, + "/carla/vehicles/{}/vehicle_control_status".format(self.role_name), + self.vehicle_control_status_updated, qos_profile=10 ) self.manual_control_subscriber = node.new_subscription( Bool, - "/carla/{}/vehicle_control_manual_override".format(self.role_name), + "/carla/vehicles/{}/vehicle_control_manual_override".format(self.role_name), self.manual_control_override_updated, qos_profile=10) @@ -379,11 +384,11 @@ def manual_control_override_updated(self, data): self.manual_control = data.data self.update_info_text() - def vehicle_status_updated(self, vehicle_status): + def vehicle_control_status_updated(self, vehicle_control_status): """ Callback on vehicle status updates """ - self.vehicle_status = vehicle_status + self.vehicle_control_status = vehicle_control_status self.update_info_text() def vehicle_info_updated(self, vehicle_info): @@ -401,16 +406,19 @@ def gnss_updated(self, data): self.longitude = data.longitude self.update_info_text() - def odometry_updated(self, data): - self.x = data.pose.pose.position.x - self.y = data.pose.pose.position.y - self.z = data.pose.pose.position.z + def vehicle_odometry_updated(self, odometry_msg): + self.x = odometry_msg.pose.pose.position.x + self.y = odometry_msg.pose.pose.position.y + self.z = odometry_msg.pose.pose.position.z _, _, yaw = quat2euler( - [data.pose.pose.orientation.w, - data.pose.pose.orientation.x, - data.pose.pose.orientation.y, - data.pose.pose.orientation.z]) + [odometry_msg.pose.pose.orientation.w, + odometry_msg.pose.pose.orientation.x, + odometry_msg.pose.pose.orientation.y, + odometry_msg.pose.pose.orientation.z]) self.yaw = math.degrees(yaw) + self.velocity = math.sqrt(odometry_msg.twist.twist.linear.x ** 2 + + odometry_msg.twist.twist.linear.y ** 2 + + odometry_msg.twist.twist.linear.z ** 2) * 3.6 self.update_info_text() def update_info_text(self): @@ -438,23 +446,23 @@ def update_info_text(self): 'Simulation time: % 12s' % time, 'FPS: % 24.1f' % fps, '', 'Vehicle: % 20s' % ' '.join(self.vehicle_info.type.title().split('.')[1:]), - 'Speed: % 15.0f km/h' % (3.6 * self.vehicle_status.velocity), + 'Speed: % 15.0f km/h' % (self.velocity), u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (yaw, heading), 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (x, y)), 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (self.latitude, self.longitude)), 'Height: % 18.0f m' % z, '' ] self._info_text += [ - ('Throttle:', self.vehicle_status.control.throttle, 0.0, 1.0), - ('Steer:', self.vehicle_status.control.steer, -1.0, 1.0), - ('Brake:', self.vehicle_status.control.brake, 0.0, 1.0), - ('Reverse:', self.vehicle_status.control.reverse), - ('Hand brake:', self.vehicle_status.control.hand_brake), - ('Manual:', self.vehicle_status.control.manual_gear_shift), + ('Throttle:', self.vehicle_control_status.last_applied_vehicle_control.throttle, 0.0, 1.0), + ('Steer:', self.vehicle_control_status.last_applied_vehicle_control.steer, -1.0, 1.0), + ('Brake:', self.vehicle_control_status.last_applied_vehicle_control.brake, 0.0, 1.0), + ('Reverse:', self.vehicle_control_status.last_applied_vehicle_control.reverse), + ('Hand brake:', self.vehicle_control_status.last_applied_vehicle_control.hand_brake), + ('Manual:', self.vehicle_control_status.last_applied_vehicle_control.manual_gear_shift), 'Gear: %s' % { -1: 'R', 0: 'N' - }.get(self.vehicle_status.control.gear, self.vehicle_status.control.gear), '' + }.get(self.vehicle_control_status.last_applied_vehicle_control.gear, self.vehicle_control_status.last_applied_vehicle_control.gear), '' ] self._info_text += [('Manual ctrl:', self.manual_control)] if self.carla_status.synchronous_mode: diff --git a/carla_msgs b/carla_msgs index 081fdcdc..117bcb7a 160000 --- a/carla_msgs +++ b/carla_msgs @@ -1 +1 @@ -Subproject commit 081fdcdc8a4f8d4ce936b792c4ef13aba3fd7478 +Subproject commit 117bcb7a98216a627e50b54495fb090c894639c2 diff --git a/carla_ros_bridge/CMakeLists.txt b/carla_ros_bridge/CMakeLists.txt deleted file mode 100644 index 5ea9b12f..00000000 --- a/carla_ros_bridge/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(carla_ros_bridge) - -find_package(ros_environment REQUIRED) -set(ROS_VERSION $ENV{ROS_VERSION}) - -if(${ROS_VERSION} EQUAL 1) - - find_package(catkin REQUIRED COMPONENTS rospy sensor_msgs geometry_msgs - derived_object_msgs tf roslaunch) - - catkin_python_setup() - - catkin_package() - - if(CATKIN_ENABLE_TESTING) - roslaunch_add_file_check(launch) - roslaunch_add_file_check(test) - endif() - - include_directories(${catkin_INCLUDE_DIRS}) - - install(PROGRAMS src/carla_ros_bridge/bridge.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - - install(FILES src/carla_ros_bridge/CARLA_VERSION - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - - install(FILES test/ros_bridge_client.test - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - - install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - - if(CATKIN_ENABLE_TESTING AND NOT $ENV{CI}) - find_package(rostest REQUIRED) - add_rostest(test/ros_bridge_client.test) - endif() - -endif() diff --git a/carla_ros_bridge/launch/carla_ros_bridge.launch b/carla_ros_bridge/launch/carla_ros_bridge.launch deleted file mode 100644 index e92eb0e1..00000000 --- a/carla_ros_bridge/launch/carla_ros_bridge.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/carla_ros_bridge/launch/carla_ros_bridge.launch.py b/carla_ros_bridge/launch/carla_ros_bridge.launch.py deleted file mode 100644 index 0492bd86..00000000 --- a/carla_ros_bridge/launch/carla_ros_bridge.launch.py +++ /dev/null @@ -1,106 +0,0 @@ -import launch -import launch_ros.actions - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='host', - default_value='localhost', - description='IP of the CARLA server' - ), - launch.actions.DeclareLaunchArgument( - name='port', - default_value='2000', - description='TCP port of the CARLA server' - ), - launch.actions.DeclareLaunchArgument( - name='timeout', - default_value='2', - description='Time to wait for a successful connection to the CARLA server' - ), - launch.actions.DeclareLaunchArgument( - name='passive', - default_value='False', - description='When enabled, the ROS bridge will take a backseat and another client must tick the world (only in synchronous mode)' - ), - launch.actions.DeclareLaunchArgument( - name='synchronous_mode', - default_value='True', - description='Enable/disable synchronous mode. If enabled, the ROS bridge waits until the expected data is received for all sensors' - ), - launch.actions.DeclareLaunchArgument( - name='synchronous_mode_wait_for_vehicle_control_command', - default_value='False', - description='When enabled, pauses the tick until a vehicle control is completed (only in synchronous mode)' - ), - launch.actions.DeclareLaunchArgument( - name='fixed_delta_seconds', - default_value='0.05', - description='Simulation time (delta seconds) between simulation steps' - ), - launch.actions.DeclareLaunchArgument( - name='town', - default_value='Town01', - description='Either use an available CARLA town (eg. "Town01") or an OpenDRIVE file (ending in .xodr)' - ), - launch.actions.DeclareLaunchArgument( - name='register_all_sensors', - default_value='True', - description='Enable/disable the registration of all sensors. If disabled, only sensors spawned by the bridge are registered' - ), - launch.actions.DeclareLaunchArgument( - name='ego_vehicle_role_name', - default_value=["hero", "ego_vehicle", "hero0", "hero1", "hero2", - "hero3", "hero4", "hero5", "hero6", "hero7", "hero8", "hero9"], - description='Role names to identify ego vehicles. ' - ), - launch_ros.actions.Node( - package='carla_ros_bridge', - executable='bridge', - name='carla_ros_bridge', - output='screen', - emulate_tty='True', - on_exit=launch.actions.Shutdown(), - parameters=[ - { - 'use_sim_time': True - }, - { - 'host': launch.substitutions.LaunchConfiguration('host') - }, - { - 'port': launch.substitutions.LaunchConfiguration('port') - }, - { - 'timeout': launch.substitutions.LaunchConfiguration('timeout') - }, - { - 'passive': launch.substitutions.LaunchConfiguration('passive') - }, - { - 'synchronous_mode': launch.substitutions.LaunchConfiguration('synchronous_mode') - }, - { - 'synchronous_mode_wait_for_vehicle_control_command': launch.substitutions.LaunchConfiguration('synchronous_mode_wait_for_vehicle_control_command') - }, - { - 'fixed_delta_seconds': launch.substitutions.LaunchConfiguration('fixed_delta_seconds') - }, - { - 'town': launch.substitutions.LaunchConfiguration('town') - }, - { - 'register_all_sensors': launch.substitutions.LaunchConfiguration('register_all_sensors') - }, - { - 'ego_vehicle_role_name': launch.substitutions.LaunchConfiguration('ego_vehicle_role_name') - } - ] - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch b/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch deleted file mode 100644 index 3ce9dedb..00000000 --- a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py b/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py deleted file mode 100644 index 8714df58..00000000 --- a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py +++ /dev/null @@ -1,92 +0,0 @@ -import os - -import launch -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='host', - default_value='localhost' - ), - launch.actions.DeclareLaunchArgument( - name='port', - default_value='2000' - ), - launch.actions.DeclareLaunchArgument( - name='timeout', - default_value='10' - ), - launch.actions.DeclareLaunchArgument( - name='role_name', - default_value='ego_vehicle' - ), - launch.actions.DeclareLaunchArgument( - name='vehicle_filter', - default_value='vehicle.*' - ), - launch.actions.DeclareLaunchArgument( - name='spawn_point', - default_value='None' - ), - launch.actions.DeclareLaunchArgument( - name='town', - default_value='Town01' - ), - launch.actions.DeclareLaunchArgument( - name='passive', - default_value='False' - ), - launch.actions.DeclareLaunchArgument( - name='synchronous_mode_wait_for_vehicle_control_command', - default_value='False' - ), - launch.actions.DeclareLaunchArgument( - name='fixed_delta_seconds', - default_value='0.05' - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'carla_ros_bridge'), 'carla_ros_bridge.launch.py') - ), - launch_arguments={ - 'host': launch.substitutions.LaunchConfiguration('host'), - 'port': launch.substitutions.LaunchConfiguration('port'), - 'town': launch.substitutions.LaunchConfiguration('town'), - 'timeout': launch.substitutions.LaunchConfiguration('timeout'), - 'passive': launch.substitutions.LaunchConfiguration('passive'), - 'synchronous_mode_wait_for_vehicle_control_command': launch.substitutions.LaunchConfiguration('synchronous_mode_wait_for_vehicle_control_command'), - 'fixed_delta_seconds': launch.substitutions.LaunchConfiguration('fixed_delta_seconds') - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'carla_spawn_objects'), 'carla_example_ego_vehicle.launch.py') - ), - launch_arguments={ - 'host': launch.substitutions.LaunchConfiguration('host'), - 'port': launch.substitutions.LaunchConfiguration('port'), - 'timeout': launch.substitutions.LaunchConfiguration('timeout'), - 'vehicle_filter': launch.substitutions.LaunchConfiguration('vehicle_filter'), - 'role_name': launch.substitutions.LaunchConfiguration('role_name'), - 'spawn_point': launch.substitutions.LaunchConfiguration('spawn_point') - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'carla_manual_control'), 'carla_manual_control.launch.py') - ), - launch_arguments={ - 'role_name': launch.substitutions.LaunchConfiguration('role_name') - }.items() - ) - ]) - return ld - - -if __name__ == '__main__': - generate_launch_description() diff --git a/carla_ros_bridge/package.xml b/carla_ros_bridge/package.xml deleted file mode 100644 index 30dd3ef1..00000000 --- a/carla_ros_bridge/package.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - carla_ros_bridge - 0.0.1 - The carla_ros_bridge package - CARLA Simulator Team - MIT - - std_msgs - geometry_msgs - derived_object_msgs - shape_msgs - nav_msgs - tf2_msgs - rosgraph_msgs - sensor_msgs - visualization_msgs - carla_common - carla_msgs - carla_spawn_objects - carla_manual_control - cv_bridge - python-transforms3d-pip - ros_compatibility - - - rclpy - tf2_ros - rviz2 - - - catkin - roslaunch - rospy - tf - tf2 - rosbag_storage - rviz - - - catkin - ament_python - - diff --git a/carla_ros_bridge/resource/carla_ros_bridge b/carla_ros_bridge/resource/carla_ros_bridge deleted file mode 100644 index e69de29b..00000000 diff --git a/carla_ros_bridge/setup.cfg b/carla_ros_bridge/setup.cfg deleted file mode 100644 index 0086a7f7..00000000 --- a/carla_ros_bridge/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/carla_ros_bridge -[install] -install_scripts=$base/lib/carla_ros_bridge diff --git a/carla_ros_bridge/setup.py b/carla_ros_bridge/setup.py deleted file mode 100644 index 18517fd9..00000000 --- a/carla_ros_bridge/setup.py +++ /dev/null @@ -1,41 +0,0 @@ -""" -Setup for carla_ros_bridge -""" -import os -from glob import glob -ROS_VERSION = int(os.environ['ROS_VERSION']) - -if ROS_VERSION == 1: - from distutils.core import setup - from catkin_pkg.python_setup import generate_distutils_setup - - d = generate_distutils_setup(packages=['carla_ros_bridge'], package_dir={'': 'src'}) - - setup(**d) - -elif ROS_VERSION == 2: - from setuptools import setup - - package_name = 'carla_ros_bridge' - setup( - name=package_name, - version='0.0.0', - packages=[package_name], - data_files=[('share/ament_index/resource_index/packages', ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name), glob('launch/*.launch.py')), - (os.path.join('share', package_name + '/test'), glob('test/test_objects.json'))], - install_requires=['setuptools'], - zip_safe=True, - maintainer='CARLA Simulator Team', - maintainer_email='carla.simulator@gmail.com', - description='CARLA ROS2 bridge', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': ['bridge = carla_ros_bridge.bridge:main'], - }, - package_dir={'': 'src'}, - package_data={'': ['CARLA_VERSION']}, - include_package_data=True - ) diff --git a/carla_ros_bridge/src/carla_ros_bridge/CARLA_VERSION b/carla_ros_bridge/src/carla_ros_bridge/CARLA_VERSION deleted file mode 100644 index 62ea2590..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/CARLA_VERSION +++ /dev/null @@ -1 +0,0 @@ -0.9.13 diff --git a/carla_ros_bridge/src/carla_ros_bridge/__init__.py b/carla_ros_bridge/src/carla_ros_bridge/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor.py b/carla_ros_bridge/src/carla_ros_bridge/actor.py deleted file mode 100644 index adc59b2a..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/actor.py +++ /dev/null @@ -1,116 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Base Classes to handle Actor objects -""" - -import carla_common.transforms as trans - -from carla_ros_bridge.pseudo_actor import PseudoActor - -from geometry_msgs.msg import TransformStamped # pylint: disable=import-error - - -class Actor(PseudoActor): - - """ - Generic base class for all carla actors - """ - - def __init__(self, uid, name, parent, node, carla_actor): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - """ - super(Actor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - self.carla_actor = carla_actor - self.carla_actor_id = carla_actor.id - - def destroy(self): - """ - Function (override) to destroy this object. - Remove the reference to the carla.Actor object. - :return: - """ - self.carla_actor = None - super(Actor, self).destroy() - - def get_current_ros_pose(self): - """ - Function to provide the current ROS pose - - :return: the ROS pose of this actor - :rtype: geometry_msgs.msg.Pose - """ - return trans.carla_transform_to_ros_pose( - self.carla_actor.get_transform()) - - def get_current_ros_transform(self): - """ - Function to provide the current ROS pose - - :return: the ROS pose of this actor - :rtype: geometry_msgs.msg.Pose - """ - return trans.carla_transform_to_ros_transform( - self.carla_actor.get_transform()) - - def get_current_ros_twist_rotated(self): - """ - Function to provide the current ROS twist rotated - - :return: the ROS twist of this actor - :rtype: geometry_msgs.msg.Twist - """ - return trans.carla_velocity_to_ros_twist( - self.carla_actor.get_velocity(), - self.carla_actor.get_angular_velocity(), - self.carla_actor.get_transform().rotation) - - def get_current_ros_twist(self): - """ - Function to provide the current ROS twist - - :return: the ROS twist of this actor - :rtype: geometry_msgs.msg.Twist - """ - return trans.carla_velocity_to_ros_twist( - self.carla_actor.get_velocity(), - self.carla_actor.get_angular_velocity()) - - def get_current_ros_accel(self): - """ - Function to provide the current ROS accel - - :return: the ROS twist of this actor - :rtype: geometry_msgs.msg.Twist - """ - return trans.carla_acceleration_to_ros_accel( - self.carla_actor.get_acceleration()) - - def get_id(self): - """ - Getter for the carla_id of this. - :return: unique carla_id of this object - :rtype: int64 - """ - return self.carla_actor_id diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_control.py b/carla_ros_bridge/src/carla_ros_bridge/actor_control.py deleted file mode 100644 index 65fa8749..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_control.py +++ /dev/null @@ -1,107 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -provide functions to control actors -""" - -import math - -import numpy -from carla import Vector3D - -import carla_common.transforms as trans - -from carla_ros_bridge.pseudo_actor import PseudoActor -from carla_ros_bridge.sensor import Sensor - -from geometry_msgs.msg import Pose, Twist - - -class ActorControl(PseudoActor): - - """ - provide functions to control actors - """ - - def __init__(self, uid, name, parent, node): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identifying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - """ - - super(ActorControl, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - - self.set_location_subscriber = self.node.new_subscription(Pose, - self.get_topic_prefix() + "/set_transform", - self.on_pose, - qos_profile=10) - - self.twist_control_subscriber = self.node.new_subscription(Twist, - self.get_topic_prefix() + "/set_target_velocity", - self.on_twist, - qos_profile=10) - - def destroy(self): - """ - Function (override) to destroy this object. - - Terminate ROS subscriptions - Finally forward call to super class. - - :return: - """ - self.node.destroy_subscription(self.set_location_subscriber) - self.node.destroy_subscription(self.twist_control_subscriber) - super(ActorControl, self).destroy() - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo actor - :return: name - """ - return "actor.pseudo.control" - - def on_pose(self, pose): - if self.parent and self.parent.carla_actor.is_alive: - self.parent.carla_actor.set_transform(trans.ros_pose_to_carla_transform(pose)) - if isinstance(self.parent, Sensor): - self.parent.relative_spawn_pose = pose - - def on_twist(self, twist): - """ - Set angular/linear velocity (this does not respect vehicle dynamics) - """ - if not self.parent.vehicle_control_override: - angular_velocity = Vector3D() - angular_velocity.z = math.degrees(twist.angular.z) - - rotation_matrix = trans.carla_rotation_to_numpy_rotation_matrix( - self.parent.carla_actor.get_transform().rotation) - linear_vector = numpy.array([twist.linear.x, twist.linear.y, twist.linear.z]) - rotated_linear_vector = rotation_matrix.dot(linear_vector) - linear_velocity = Vector3D() - linear_velocity.x = rotated_linear_vector[0] - linear_velocity.y = -rotated_linear_vector[1] - linear_velocity.z = rotated_linear_vector[2] - - self.node.logdebug("Set velocity linear: {}, angular: {}".format( - linear_velocity, angular_velocity)) - self.parent.carla_actor.set_target_velocity(linear_velocity) - self.parent.carla_actor.set_target_angular_velocity(angular_velocity) diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py deleted file mode 100755 index a6963556..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ /dev/null @@ -1,428 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# - -import itertools -try: - import queue -except ImportError: - import Queue as queue -import time -from enum import Enum -from threading import Thread, Lock - -import carla -import numpy as np - -import carla_common.transforms as trans - -from carla_ros_bridge.actor import Actor -from carla_ros_bridge.actor_control import ActorControl -from carla_ros_bridge.actor_list_sensor import ActorListSensor -from carla_ros_bridge.camera import Camera, RgbCamera, DepthCamera, SemanticSegmentationCamera, DVSCamera -from carla_ros_bridge.collision_sensor import CollisionSensor -from carla_ros_bridge.ego_vehicle import EgoVehicle -from carla_ros_bridge.gnss import Gnss -from carla_ros_bridge.imu import ImuSensor -from carla_ros_bridge.lane_invasion_sensor import LaneInvasionSensor -from carla_ros_bridge.lidar import Lidar, SemanticLidar -from carla_ros_bridge.marker_sensor import MarkerSensor -from carla_ros_bridge.object_sensor import ObjectSensor -from carla_ros_bridge.odom_sensor import OdometrySensor -from carla_ros_bridge.opendrive_sensor import OpenDriveSensor -from carla_ros_bridge.pseudo_actor import PseudoActor -from carla_ros_bridge.radar import Radar -from carla_ros_bridge.rss_sensor import RssSensor -from carla_ros_bridge.sensor import Sensor -from carla_ros_bridge.spectator import Spectator -from carla_ros_bridge.speedometer_sensor import SpeedometerSensor -from carla_ros_bridge.tf_sensor import TFSensor -from carla_ros_bridge.traffic import Traffic, TrafficLight -from carla_ros_bridge.traffic_lights_sensor import TrafficLightsSensor -from carla_ros_bridge.vehicle import Vehicle -from carla_ros_bridge.walker import Walker - -# to generate a random spawning position or vehicles -import random -secure_random = random.SystemRandom() - - -class ActorFactory(object): - - TIME_BETWEEN_UPDATES = 0.1 - - class TaskType(Enum): - SPAWN_ACTOR = 0 - SPAWN_PSEUDO_ACTOR = 1 - DESTROY_ACTOR = 2 - - def __init__(self, node, world, sync_mode=False): - self.node = node - self.world = world - self.blueprint_lib = self.world.get_blueprint_library() - self.spawn_points = self.world.get_map().get_spawn_points() - self.sync_mode = sync_mode - - self._active_actors = set() - self.actors = {} - - self._task_queue = queue.Queue() - self._known_actor_ids = [] # used to immediately reply to spawn_actor/destroy_actor calls - - self.lock = Lock() - self.spawn_lock = Lock() - - # id generator for pseudo sensors - self.id_gen = itertools.count(10000) - - self.thread = Thread(target=self._update_thread) - - def start(self): - # create initially existing actors - self.update_available_objects() - self.thread.start() - - def _update_thread(self): - """ - execution loop for async mode actor discovery - """ - while not self.node.shutdown.is_set(): - time.sleep(ActorFactory.TIME_BETWEEN_UPDATES) - self.world.wait_for_tick() - self.update_available_objects() - - def update_available_objects(self): - """ - update the available actors - """ - # The carla.World.get_actors() method does not return actors that has been spawned in the same frame. - # This is a known bug and will be fixed in future release of CARLA. - current_actors = set([actor.id for actor in self.world.get_actors()]) - spawned_actors = current_actors - self._active_actors - destroyed_actors = self._active_actors - current_actors - self._active_actors = current_actors - - # Create/destroy actors not managed by the bridge. - self.lock.acquire() - for actor_id in spawned_actors: - carla_actor = self.world.get_actor(actor_id) - if self.node.parameters["register_all_sensors"] or not isinstance(carla_actor, carla.Sensor): - self._create_object_from_actor(carla_actor) - - for actor_id in destroyed_actors: - self._destroy_object(actor_id, delete_actor=False) - - # Create/destroy objects managed by the bridge. - with self.spawn_lock: - while not self._task_queue.empty(): - task = self._task_queue.get() - task_type = task[0] - actor_id, req = task[1] - - if task_type == ActorFactory.TaskType.SPAWN_ACTOR and not self.node.shutdown.is_set(): - carla_actor = self.world.get_actor(actor_id) - self._create_object_from_actor(carla_actor, req) - elif task_type == ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR and not self.node.shutdown.is_set(): - self._create_object(actor_id, req.type, req.id, req.attach_to, req.transform) - elif task_type == ActorFactory.TaskType.DESTROY_ACTOR: - self._destroy_object(actor_id, delete_actor=True) - - self.lock.release() - - def update_actor_states(self, frame_id, timestamp): - """ - update the state of all known actors - """ - with self.lock: - for actor_id in self.actors: - try: - self.actors[actor_id].update(frame_id, timestamp) - except RuntimeError as e: - self.node.logwarn("Update actor {}({}) failed: {}".format( - self.actors[actor_id].__class__.__name__, actor_id, e)) - continue - - def clear(self): - for _, actor in self.actors.items(): - actor.destroy() - self.actors.clear() - - def spawn_actor(self, req): - """ - spawns an object - - No object instances are created here. Instead carla-actors are created, - and pseudo objects are appended to a list to get created later. - """ - with self.spawn_lock: - if "pseudo" in req.type: - # only allow spawning pseudo objects if parent actor already exists in carla - if req.attach_to != 0: - carla_actor = self.world.get_actor(req.attach_to) - if carla_actor is None: - raise IndexError("Parent actor {} not found".format(req.attach_to)) - id_ = next(self.id_gen) - self._task_queue.put((ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR, (id_, req))) - else: - id_ = self._spawn_carla_actor(req) - self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, req))) - self._known_actor_ids.append(id_) - return id_ - - def destroy_actor(self, uid): - - def get_objects_to_destroy(uid): - objects_to_destroy = [] - if uid in self._known_actor_ids: - objects_to_destroy.append(uid) - self._known_actor_ids.remove(uid) - - # remove actors that have the actor to be removed as parent. - for actor in list(self.actors.values()): - if actor.parent is not None and actor.parent.uid == uid: - objects_to_destroy.extend(get_objects_to_destroy(actor.uid)) - - return objects_to_destroy - - with self.spawn_lock: - objects_to_destroy = set(get_objects_to_destroy(uid)) - for obj in objects_to_destroy: - self._task_queue.put((ActorFactory.TaskType.DESTROY_ACTOR, (obj, None))) - return objects_to_destroy - - def _spawn_carla_actor(self, req): - """ - spawns an actor in carla - """ - if "*" in req.type: - blueprint = secure_random.choice( - self.blueprint_lib.filter(req.type)) - else: - blueprint = self.blueprint_lib.find(req.type) - blueprint.set_attribute('role_name', req.id) - for attribute in req.attributes: - blueprint.set_attribute(attribute.key, attribute.value) - if req.random_pose is False: - transform = trans.ros_pose_to_carla_transform(req.transform) - else: - # get a random pose - transform = secure_random.choice( - self.spawn_points) if self.spawn_points else carla.Transform() - - attach_to = None - if req.attach_to != 0: - attach_to = self.world.get_actor(req.attach_to) - if attach_to is None: - raise IndexError("Parent actor {} not found".format(req.attach_to)) - - carla_actor = self.world.spawn_actor(blueprint, transform, attach_to) - return carla_actor.id - - def _create_object_from_actor(self, carla_actor, req=None): - """ - create a object for a given carla actor - Creates also the object for its parent, if not yet existing - """ - parent = None - # the transform relative to the map - relative_transform = trans.carla_transform_to_ros_pose(carla_actor.get_transform()) - if carla_actor.parent: - if carla_actor.parent.id in self.actors: - parent = self.actors[carla_actor.parent.id] - else: - parent = self._create_object_from_actor(carla_actor.parent) - if req is not None: - relative_transform = req.transform - else: - # calculate relative transform to the parent - actor_transform_matrix = trans.ros_pose_to_transform_matrix(relative_transform) - parent_transform_matrix = trans.ros_pose_to_transform_matrix( - trans.carla_transform_to_ros_pose(carla_actor.parent.get_transform())) - relative_transform_matrix = np.matrix( - parent_transform_matrix).getI() * np.matrix(actor_transform_matrix) - relative_transform = trans.transform_matrix_to_ros_pose(relative_transform_matrix) - - parent_id = 0 - if parent is not None: - parent_id = parent.uid - - name = carla_actor.attributes.get("role_name", "") - if not name: - name = str(carla_actor.id) - obj = self._create_object(carla_actor.id, carla_actor.type_id, name, - parent_id, relative_transform, carla_actor) - return obj - - def _destroy_object(self, actor_id, delete_actor): - if actor_id not in self.actors: - return - actor = self.actors[actor_id] - del self.actors[actor_id] - carla_actor = None - if isinstance(actor, Actor): - carla_actor = actor.carla_actor - actor.destroy() - if carla_actor and delete_actor: - carla_actor.destroy() - self.node.loginfo("Removed {}(id={})".format(actor.__class__.__name__, actor.uid)) - - def get_pseudo_sensor_types(self): - pseudo_sensors = [] - for cls in PseudoActor.__subclasses__(): - if cls.__name__ != "Actor": - pseudo_sensors.append(cls.get_blueprint_name()) - return pseudo_sensors - - def _create_object(self, uid, type_id, name, attach_to, spawn_pose, carla_actor=None): - # check that the actor is not already created. - if carla_actor is not None and carla_actor.id in self.actors: - return None - - if attach_to != 0: - if attach_to not in self.actors: - raise IndexError("Parent object {} not found".format(attach_to)) - - parent = self.actors[attach_to] - else: - parent = None - - if type_id == TFSensor.get_blueprint_name(): - actor = TFSensor(uid=uid, name=name, parent=parent, node=self.node) - - elif type_id == OdometrySensor.get_blueprint_name(): - actor = OdometrySensor(uid=uid, - name=name, - parent=parent, - node=self.node) - - elif type_id == SpeedometerSensor.get_blueprint_name(): - actor = SpeedometerSensor(uid=uid, - name=name, - parent=parent, - node=self.node) - - elif type_id == MarkerSensor.get_blueprint_name(): - actor = MarkerSensor(uid=uid, - name=name, - parent=parent, - node=self.node, - actor_list=self.actors, - world=self.world) - - elif type_id == ActorListSensor.get_blueprint_name(): - actor = ActorListSensor(uid=uid, - name=name, - parent=parent, - node=self.node, - actor_list=self.actors) - - elif type_id == ObjectSensor.get_blueprint_name(): - actor = ObjectSensor( - uid=uid, - name=name, - parent=parent, - node=self.node, - actor_list=self.actors, - ) - - elif type_id == TrafficLightsSensor.get_blueprint_name(): - actor = TrafficLightsSensor( - uid=uid, - name=name, - parent=parent, - node=self.node, - actor_list=self.actors, - ) - - elif type_id == OpenDriveSensor.get_blueprint_name(): - actor = OpenDriveSensor(uid=uid, - name=name, - parent=parent, - node=self.node, - carla_map=self.world.get_map()) - - elif type_id == ActorControl.get_blueprint_name(): - actor = ActorControl(uid=uid, - name=name, - parent=parent, - node=self.node) - - elif carla_actor.type_id.startswith('traffic'): - if carla_actor.type_id == "traffic.traffic_light": - actor = TrafficLight(uid, name, parent, self.node, carla_actor) - else: - actor = Traffic(uid, name, parent, self.node, carla_actor) - elif carla_actor.type_id.startswith("vehicle"): - if carla_actor.attributes.get('role_name')\ - in self.node.parameters['ego_vehicle']['role_name']: - actor = EgoVehicle( - uid, name, parent, self.node, carla_actor, - self.node._ego_vehicle_control_applied_callback) - else: - actor = Vehicle(uid, name, parent, self.node, carla_actor) - elif carla_actor.type_id.startswith("sensor"): - if carla_actor.type_id.startswith("sensor.camera"): - if carla_actor.type_id.startswith("sensor.camera.rgb"): - actor = RgbCamera(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith("sensor.camera.depth"): - actor = DepthCamera(uid, name, parent, spawn_pose, - self.node, carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith( - "sensor.camera.semantic_segmentation"): - actor = SemanticSegmentationCamera(uid, name, parent, - spawn_pose, self.node, - carla_actor, - self.sync_mode) - elif carla_actor.type_id.startswith("sensor.camera.dvs"): - actor = DVSCamera(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - else: - actor = Camera(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith("sensor.lidar"): - if carla_actor.type_id.endswith("sensor.lidar.ray_cast"): - actor = Lidar(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.endswith( - "sensor.lidar.ray_cast_semantic"): - actor = SemanticLidar(uid, name, parent, spawn_pose, - self.node, carla_actor, - self.sync_mode) - elif carla_actor.type_id.startswith("sensor.other.radar"): - actor = Radar(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith("sensor.other.gnss"): - actor = Gnss(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith("sensor.other.imu"): - actor = ImuSensor(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith("sensor.other.collision"): - actor = CollisionSensor(uid, name, parent, spawn_pose, - self.node, carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith("sensor.other.rss"): - actor = RssSensor(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith("sensor.other.lane_invasion"): - actor = LaneInvasionSensor(uid, name, parent, spawn_pose, - self.node, carla_actor, - self.sync_mode) - else: - actor = Sensor(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.startswith("spectator"): - actor = Spectator(uid, name, parent, self.node, carla_actor) - elif carla_actor.type_id.startswith("walker"): - actor = Walker(uid, name, parent, self.node, carla_actor) - else: - actor = Actor(uid, name, parent, self.node, carla_actor) - - self.actors[actor.uid] = actor - self.node.loginfo("Created {}(id={})".format(actor.__class__.__name__, actor.uid)) - - return actor diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_list_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/actor_list_sensor.py deleted file mode 100644 index 9bddbcff..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_list_sensor.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -handle a actor list sensor -""" - -from carla_ros_bridge.actor import Actor -from carla_ros_bridge.pseudo_actor import PseudoActor - -from carla_msgs.msg import CarlaActorList, CarlaActorInfo - - -class ActorListSensor(PseudoActor): - - """ - Pseudo actor list sensor - """ - - def __init__(self, uid, name, parent, node, actor_list): - """ - Constructor - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param carla_world: carla world object - :type carla_world: carla.World - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - :param actor_list: current list of actors - :type actor_list: map(carla-actor-id -> python-actor-object) - """ - - super(ActorListSensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - self.actor_list = actor_list - self.actor_list_publisher = node.new_publisher(CarlaActorList, self.get_topic_prefix(), qos_profile=10) - - def destroy(self): - """ - Function to destroy this object. - :return: - """ - super(ActorListSensor, self).destroy() - self.actor_list = None - self.node.destroy_publisher(self.actor_list_publisher) - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - return "sensor.pseudo.actor_list" - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - """ - ros_actor_list = CarlaActorList() - - for actor_id in self.actor_list.keys(): - if not isinstance(self.actor_list[actor_id], Actor): - continue - - actor = self.actor_list[actor_id].carla_actor - ros_actor = CarlaActorInfo() - ros_actor.id = actor.id - ros_actor.type = actor.type_id - try: - ros_actor.rolename = str(actor.attributes.get('role_name')) - except ValueError: - pass - - if actor.parent: - ros_actor.parent_id = actor.parent.id - else: - ros_actor.parent_id = 0 - - ros_actor_list.actors.append(ros_actor) - - self.actor_list_publisher.publish(ros_actor_list) diff --git a/carla_ros_bridge/src/carla_ros_bridge/bridge.py b/carla_ros_bridge/src/carla_ros_bridge/bridge.py deleted file mode 100755 index 9e34f499..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/bridge.py +++ /dev/null @@ -1,459 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2018-2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Rosbridge class: - -Class that handle communication between CARLA and ROS -""" - -import os -import pkg_resources -try: - import queue -except ImportError: - import Queue as queue -import sys -from distutils.version import LooseVersion -from threading import Thread, Lock, Event - -import carla - -import ros_compatibility as roscomp -from ros_compatibility.node import CompatibleNode - -from carla_ros_bridge.actor import Actor -from carla_ros_bridge.actor_factory import ActorFactory -from carla_ros_bridge.carla_status_publisher import CarlaStatusPublisher -from carla_ros_bridge.debug_helper import DebugHelper -from carla_ros_bridge.ego_vehicle import EgoVehicle -from carla_ros_bridge.world_info import WorldInfo - -from carla_msgs.msg import CarlaControl, CarlaWeatherParameters -from carla_msgs.srv import SpawnObject, DestroyObject, GetBlueprints -from rosgraph_msgs.msg import Clock - - -class CarlaRosBridge(CompatibleNode): - - """ - Carla Ros bridge - """ - - with open(os.path.join(os.path.dirname(__file__), "CARLA_VERSION")) as f: - CARLA_VERSION = f.read()[:-1] - - # in synchronous mode, if synchronous_mode_wait_for_vehicle_control_command is True, - # wait for this time until a next tick is triggered. - VEHICLE_CONTROL_TIMEOUT = 1. - - def __init__(self): - """ - Constructor - - :param carla_world: carla world object - :type carla_world: carla.World - :param params: dict of parameters, see settings.yaml - :type params: dict - """ - super(CarlaRosBridge, self).__init__("ros_bridge_node") - - # pylint: disable=attribute-defined-outside-init - def initialize_bridge(self, carla_world, params): - """ - Initialize the bridge - """ - self.parameters = params - self.carla_world = carla_world - - self.ros_timestamp = roscomp.ros_timestamp() - self.callback_group = roscomp.callback_groups.ReentrantCallbackGroup() - - self.synchronous_mode_update_thread = None - self.shutdown = Event() - - self.carla_settings = carla_world.get_settings() - if not self.parameters["passive"]: - # workaround: settings can only applied within non-sync mode - if self.carla_settings.synchronous_mode: - self.carla_settings.synchronous_mode = False - carla_world.apply_settings(self.carla_settings) - - self.loginfo("synchronous_mode: {}".format( - self.parameters["synchronous_mode"])) - self.carla_settings.synchronous_mode = self.parameters["synchronous_mode"] - self.loginfo("fixed_delta_seconds: {}".format( - self.parameters["fixed_delta_seconds"])) - self.carla_settings.fixed_delta_seconds = self.parameters["fixed_delta_seconds"] - carla_world.apply_settings(self.carla_settings) - - self.loginfo("Parameters:") - for key in self.parameters: - self.loginfo(" {}: {}".format(key, self.parameters[key])) - - # active sync mode in the ros bridge only if CARLA world is configured in sync mode and - # passive mode is not enabled. - self.sync_mode = self.carla_settings.synchronous_mode and not self.parameters["passive"] - if self.carla_settings.synchronous_mode and self.parameters["passive"]: - self.loginfo( - "Passive mode is enabled and CARLA world is configured in synchronous mode. This configuration requires another client ticking the CARLA world.") - - self.carla_control_queue = queue.Queue() - - # actor factory - self.actor_factory = ActorFactory(self, carla_world, self.sync_mode) - - # add world info - self.world_info = WorldInfo(carla_world=self.carla_world, node=self) - # add debug helper - self.debug_helper = DebugHelper(carla_world.debug, self) - - # Communication topics - self.clock_publisher = self.new_publisher(Clock, 'clock', 10) - - self.status_publisher = CarlaStatusPublisher( - self.carla_settings.synchronous_mode, - self.carla_settings.fixed_delta_seconds, - self) - - # for waiting for ego vehicle control commands in synchronous mode, - # their ids are maintained in a list. - # Before tick(), the list is filled and the loop waits until the list is empty. - self._all_vehicle_control_commands_received = Event() - self._expected_ego_vehicle_control_command_ids = [] - self._expected_ego_vehicle_control_command_ids_lock = Lock() - - if self.sync_mode: - self.carla_run_state = CarlaControl.PLAY - - self.carla_control_subscriber = \ - self.new_subscription(CarlaControl, "/carla/control", - lambda control: self.carla_control_queue.put(control.command), - qos_profile=10, callback_group=self.callback_group) - - self.synchronous_mode_update_thread = Thread( - target=self._synchronous_mode_update) - self.synchronous_mode_update_thread.start() - else: - self.timestamp_last_run = 0.0 - - self.actor_factory.start() - - # register callback to update actors - self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick) - - # services configuration. - self._registered_actors = [] - self.spawn_object_service = self.new_service(SpawnObject, "/carla/spawn_object", - self.spawn_object) - self.destroy_object_service = self.new_service(DestroyObject, "/carla/destroy_object", - self.destroy_object) - - self.get_blueprints_service = self.new_service(GetBlueprints, "/carla/get_blueprints", - self.get_blueprints, callback_group=self.callback_group) - - self.carla_weather_subscriber = \ - self.new_subscription(CarlaWeatherParameters, "/carla/weather_control", - self.on_weather_changed, qos_profile=10, callback_group=self.callback_group) - - def spawn_object(self, req, response=None): - response = roscomp.get_service_response(SpawnObject) - if not self.shutdown.is_set(): - try: - id_ = self.actor_factory.spawn_actor(req) - self._registered_actors.append(id_) - response.id = id_ - except Exception as e: - self.logwarn("Error spawning object '{}': {}".format(req.type, e)) - response.id = -1 - response.error_string = str(e) - else: - response.id = -1 - response.error_string = 'Bridge is shutting down, object will not be spawned.' - return response - - def destroy_object(self, req, response=None): - response = roscomp.get_service_response(DestroyObject) - destroyed_actors = self.actor_factory.destroy_actor(req.id) - response.success = bool(destroyed_actors) - for actor in destroyed_actors: - if actor in self._registered_actors: - self._registered_actors.remove(actor) - return response - - def get_blueprints(self, req): - response = roscomp.get_service_response(GetBlueprints) - if req.filter: - bp_filter = req.filter - else: - bp_filter = "*" - - response.blueprints = [ - bp.id for bp in self.carla_world.get_blueprint_library().filter(bp_filter)] - response.blueprints.extend(self.actor_factory.get_pseudo_sensor_types()) - response.blueprints.sort() - return response - - def on_weather_changed(self, weather_parameters): - """ - Callback on new weather parameters - :return: - """ - if not self.carla_world: - return - self.loginfo("Applying weather parameters...") - weather = carla.WeatherParameters() - weather.cloudiness = weather_parameters.cloudiness - weather.precipitation = weather_parameters.precipitation - weather.precipitation_deposits = weather_parameters.precipitation_deposits - weather.wind_intensity = weather_parameters.wind_intensity - weather.fog_density = weather_parameters.fog_density - weather.fog_distance = weather_parameters.fog_distance - weather.wetness = weather_parameters.wetness - weather.sun_azimuth_angle = weather_parameters.sun_azimuth_angle - weather.sun_altitude_angle = weather_parameters.sun_altitude_angle - self.carla_world.set_weather(weather) - - def process_run_state(self): - """ - process state changes - """ - command = None - - # get last command - while not self.carla_control_queue.empty(): - command = self.carla_control_queue.get() - - while command is not None and roscomp.ok(): - self.carla_run_state = command - - if self.carla_run_state == CarlaControl.PAUSE: - # wait for next command - self.loginfo("State set to PAUSED") - self.status_publisher.set_synchronous_mode_running(False) - command = self.carla_control_queue.get() - elif self.carla_run_state == CarlaControl.PLAY: - self.loginfo("State set to PLAY") - self.status_publisher.set_synchronous_mode_running(True) - return - elif self.carla_run_state == CarlaControl.STEP_ONCE: - self.loginfo("Execute single step.") - self.status_publisher.set_synchronous_mode_running(True) - self.carla_control_queue.put(CarlaControl.PAUSE) - return - - def _synchronous_mode_update(self): - """ - execution loop for synchronous mode - """ - while not self.shutdown.is_set() and roscomp.ok(): - self.process_run_state() - - if self.parameters['synchronous_mode_wait_for_vehicle_control_command']: - # fill list of available ego vehicles - self._expected_ego_vehicle_control_command_ids = [] - with self._expected_ego_vehicle_control_command_ids_lock: - for actor_id, actor in self.actor_factory.actors.items(): - if isinstance(actor, EgoVehicle): - self._expected_ego_vehicle_control_command_ids.append( - actor_id) - - self.actor_factory.update_available_objects() - frame = self.carla_world.tick() - - world_snapshot = self.carla_world.get_snapshot() - - self.status_publisher.set_frame(frame) - self.update_clock(world_snapshot.timestamp) - self.logdebug("Tick for frame {} returned. Waiting for sensor data...".format( - frame)) - self._update(frame, world_snapshot.timestamp.elapsed_seconds) - self.logdebug("Waiting for sensor data finished.") - - if self.parameters['synchronous_mode_wait_for_vehicle_control_command']: - # wait for all ego vehicles to send a vehicle control command - if self._expected_ego_vehicle_control_command_ids: - if not self._all_vehicle_control_commands_received.wait(CarlaRosBridge.VEHICLE_CONTROL_TIMEOUT): - self.logwarn("Timeout ({}s) while waiting for vehicle control commands. " - "Missing command from actor ids {}".format(CarlaRosBridge.VEHICLE_CONTROL_TIMEOUT, - self._expected_ego_vehicle_control_command_ids)) - self._all_vehicle_control_commands_received.clear() - - def _carla_time_tick(self, carla_snapshot): - """ - Private callback registered at carla.World.on_tick() - to trigger cyclic updates. - - After successful locking the update mutex - (only perform trylock to respect bridge processing time) - the clock and the children are updated. - Finally the ROS messages collected to be published are sent out. - - :param carla_timestamp: the current carla time - :type carla_timestamp: carla.Timestamp - :return: - """ - if not self.shutdown.is_set(): - if self.timestamp_last_run < carla_snapshot.timestamp.elapsed_seconds: - self.timestamp_last_run = carla_snapshot.timestamp.elapsed_seconds - self.update_clock(carla_snapshot.timestamp) - self.status_publisher.set_frame(carla_snapshot.frame) - self._update(carla_snapshot.frame, - carla_snapshot.timestamp.elapsed_seconds) - - def _update(self, frame_id, timestamp): - """ - update all actors - :return: - """ - self.world_info.update(frame_id, timestamp) - self.actor_factory.update_actor_states(frame_id, timestamp) - - def _ego_vehicle_control_applied_callback(self, ego_vehicle_id): - if not self.sync_mode or \ - not self.parameters['synchronous_mode_wait_for_vehicle_control_command']: - return - with self._expected_ego_vehicle_control_command_ids_lock: - if ego_vehicle_id in self._expected_ego_vehicle_control_command_ids: - self._expected_ego_vehicle_control_command_ids.remove( - ego_vehicle_id) - else: - self.logwarn( - "Unexpected vehicle control command received from {}".format(ego_vehicle_id)) - if not self._expected_ego_vehicle_control_command_ids: - self._all_vehicle_control_commands_received.set() - - def update_clock(self, carla_timestamp): - """ - perform the update of the clock - - :param carla_timestamp: the current carla time - :type carla_timestamp: carla.Timestamp - :return: - """ - if roscomp.ok(): - self.ros_timestamp = roscomp.ros_timestamp(carla_timestamp.elapsed_seconds, from_sec=True) - self.clock_publisher.publish(Clock(clock=self.ros_timestamp)) - - def destroy(self): - """ - Function to destroy this object. - - :return: - """ - self.loginfo("Shutting down...") - self.shutdown.set() - if not self.sync_mode: - if self.on_tick_id: - self.carla_world.remove_on_tick(self.on_tick_id) - self.actor_factory.thread.join() - else: - self.synchronous_mode_update_thread.join() - self.loginfo("Object update finished.") - self.debug_helper.destroy() - self.status_publisher.destroy() - self.destroy_service(self.spawn_object_service) - self.destroy_service(self.destroy_object_service) - self.destroy_subscription(self.carla_weather_subscriber) - self.carla_control_queue.put(CarlaControl.STEP_ONCE) - - for uid in self._registered_actors: - self.actor_factory.destroy_actor(uid) - self.actor_factory.update_available_objects() - self.actor_factory.clear() - super(CarlaRosBridge, self).destroy() - - -def main(args=None): - """ - main function for carla simulator ROS bridge - maintaining the communication client and the CarlaBridge object - """ - roscomp.init("bridge", args=args) - - carla_bridge = None - carla_world = None - carla_client = None - executor = None - parameters = {} - - executor = roscomp.executors.MultiThreadedExecutor() - carla_bridge = CarlaRosBridge() - executor.add_node(carla_bridge) - - roscomp.on_shutdown(carla_bridge.destroy) - - parameters['host'] = carla_bridge.get_param('host', 'localhost') - parameters['port'] = carla_bridge.get_param('port', 2000) - parameters['timeout'] = carla_bridge.get_param('timeout', 2) - parameters['passive'] = carla_bridge.get_param('passive', False) - parameters['synchronous_mode'] = carla_bridge.get_param('synchronous_mode', True) - parameters['synchronous_mode_wait_for_vehicle_control_command'] = carla_bridge.get_param( - 'synchronous_mode_wait_for_vehicle_control_command', False) - parameters['fixed_delta_seconds'] = carla_bridge.get_param('fixed_delta_seconds', - 0.05) - parameters['register_all_sensors'] = carla_bridge.get_param('register_all_sensors', True) - parameters['town'] = carla_bridge.get_param('town', 'Town01') - role_name = carla_bridge.get_param('ego_vehicle_role_name', - ["hero", "ego_vehicle", "hero1", "hero2", "hero3"]) - parameters["ego_vehicle"] = {"role_name": role_name} - - carla_bridge.loginfo("Trying to connect to {host}:{port}".format( - host=parameters['host'], port=parameters['port'])) - - try: - carla_client = carla.Client( - host=parameters['host'], - port=parameters['port']) - carla_client.set_timeout(parameters['timeout']) - - # check carla version - dist = pkg_resources.get_distribution("carla") - if LooseVersion(dist.version) != LooseVersion(CarlaRosBridge.CARLA_VERSION): - carla_bridge.logfatal("CARLA python module version {} required. Found: {}".format( - CarlaRosBridge.CARLA_VERSION, dist.version)) - sys.exit(1) - - if LooseVersion(carla_client.get_server_version()) != \ - LooseVersion(carla_client.get_client_version()): - carla_bridge.logwarn( - "Version mismatch detected: You are trying to connect to a simulator that might be incompatible with this API. Client API version: {}. Simulator API version: {}" - .format(carla_client.get_client_version(), - carla_client.get_server_version())) - - carla_world = carla_client.get_world() - - if "town" in parameters and not parameters['passive']: - if parameters["town"].endswith(".xodr"): - carla_bridge.loginfo( - "Loading opendrive world from file '{}'".format(parameters["town"])) - with open(parameters["town"]) as od_file: - data = od_file.read() - carla_world = carla_client.generate_opendrive_world(str(data)) - else: - if carla_world.get_map().name != parameters["town"]: - carla_bridge.loginfo("Loading town '{}' (previous: '{}').".format( - parameters["town"], carla_world.get_map().name)) - carla_world = carla_client.load_world(parameters["town"]) - carla_world.tick() - - carla_bridge.initialize_bridge(carla_client.get_world(), parameters) - - carla_bridge.spin() - - except (IOError, RuntimeError) as e: - carla_bridge.logerr("Error: {}".format(e)) - except KeyboardInterrupt: - pass - finally: - roscomp.shutdown() - del carla_world - del carla_client - - -if __name__ == "__main__": - main() diff --git a/carla_ros_bridge/src/carla_ros_bridge/camera.py b/carla_ros_bridge/src/carla_ros_bridge/camera.py deleted file mode 100644 index c9404e2d..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/camera.py +++ /dev/null @@ -1,464 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Class to handle Carla camera sensors -""" - -import math -import os -from abc import abstractmethod - -import carla -import numpy -import transforms3d -from cv_bridge import CvBridge - -import carla_common.transforms as trans -from ros_compatibility.core import get_ros_version - -from carla_ros_bridge.sensor import Sensor, create_cloud - -from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField - -ROS_VERSION = get_ros_version() - - -class Camera(Sensor): - - """ - Sensor implementation details for cameras - """ - - # global cv bridge to convert image between opencv and ros - cv_bridge = CvBridge() - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode, is_event_sensor=False): # pylint: disable=too-many-arguments - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(Camera, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode, - is_event_sensor=is_event_sensor) - - if self.__class__.__name__ == "Camera": - self.node.logwarn("Created Unsupported Camera Actor" - "(id={}, type={}, attributes={})".format(self.get_id(), - self.carla_actor.type_id, - self.carla_actor.attributes)) - else: - self._build_camera_info() - - self.camera_info_publisher = node.new_publisher(CameraInfo, self.get_topic_prefix() + - '/camera_info', qos_profile=10) - self.camera_image_publisher = node.new_publisher(Image, self.get_topic_prefix() + - '/' + 'image', qos_profile=10) - - def destroy(self): - super(Camera, self).destroy() - self.node.destroy_publisher(self.camera_info_publisher) - self.node.destroy_publisher(self.camera_image_publisher) - - def _build_camera_info(self): - """ - Private function to compute camera info - - camera info doesn't change over time - """ - camera_info = CameraInfo() - # store info without header - camera_info.header = self.get_msg_header() - camera_info.width = int(self.carla_actor.attributes['image_size_x']) - camera_info.height = int(self.carla_actor.attributes['image_size_y']) - camera_info.distortion_model = 'plumb_bob' - cx = camera_info.width / 2.0 - cy = camera_info.height / 2.0 - fx = camera_info.width / ( - 2.0 * math.tan(float(self.carla_actor.attributes['fov']) * math.pi / 360.0)) - fy = fx - if ROS_VERSION == 1: - camera_info.K = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] - camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0] - camera_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - camera_info.P = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] - elif ROS_VERSION == 2: - # pylint: disable=assigning-non-slot - camera_info.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] - camera_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] - camera_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - camera_info.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] - self._camera_info = camera_info - - # pylint: disable=arguments-differ - def sensor_data_updated(self, carla_camera_data): - """ - Function (override) to transform the received carla camera data - into a ROS image message - """ - img_msg = self.get_ros_image(carla_camera_data) - - cam_info = self._camera_info - cam_info.header = img_msg.header - self.camera_info_publisher.publish(cam_info) - self.camera_image_publisher.publish(img_msg) - - def get_ros_transform(self, pose, timestamp): - """ - Function (override) to modify the tf messages sent by this camera. - The camera transformation has to be altered to look at the same axis - as the opencv projection in order to get easy depth cloud for RGBD camera - :return: the filled tf message - :rtype: geometry_msgs.msg.TransformStamped - """ - tf_msg = super(Camera, self).get_ros_transform(pose, timestamp) - rotation = tf_msg.transform.rotation - - quat = [rotation.w, rotation.x, rotation.y, rotation.z] - quat_swap = transforms3d.quaternions.mat2quat(numpy.matrix( - [[0, 0, 1], - [-1, 0, 0], - [0, -1, 0]])) - quat = transforms3d.quaternions.qmult(quat, quat_swap) - - tf_msg.transform.rotation.w = quat[0] - tf_msg.transform.rotation.x = quat[1] - tf_msg.transform.rotation.y = quat[2] - tf_msg.transform.rotation.z = quat[3] - - return tf_msg - - def get_ros_image(self, carla_camera_data): - """ - Function to transform the received carla camera data into a ROS image message - """ - if ((carla_camera_data.height != self._camera_info.height) or - (carla_camera_data.width != self._camera_info.width)): - self.node.logerr( - "Camera{} received image not matching configuration".format(self.get_prefix())) - image_data_array, encoding = self.get_carla_image_data_array( - carla_camera_data) - img_msg = Camera.cv_bridge.cv2_to_imgmsg(image_data_array, encoding=encoding) - # the camera data is in respect to the camera's own frame - img_msg.header = self.get_msg_header(timestamp=carla_camera_data.timestamp) - - return img_msg - - @abstractmethod - def get_carla_image_data_array(self, carla_camera_data): - """ - Virtual function to convert the carla camera data to a numpy data array - as input for the cv_bridge.cv2_to_imgmsg() function - - :return tuple (numpy data array containing the image information, encoding) - :rtype tuple(numpy.ndarray, string) - """ - raise NotImplementedError( - "This function has to be re-implemented by derived classes") - - -class RgbCamera(Camera): - - """ - Camera implementation details for rgb camera - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the relative spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(RgbCamera, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode) - - self.listen() - - def get_carla_image_data_array(self, carla_image): - """ - Function (override) to convert the carla image to a numpy data array - as input for the cv_bridge.cv2_to_imgmsg() function - - The RGB camera provides a 4-channel int8 color format (bgra). - - :param carla_image: carla image object - :type carla_image: carla.Image - :return tuple (numpy data array containing the image information, encoding) - :rtype tuple(numpy.ndarray, string) - """ - - carla_image_data_array = numpy.ndarray( - shape=(carla_image.height, carla_image.width, 4), - dtype=numpy.uint8, buffer=carla_image.raw_data) - - return carla_image_data_array, 'bgra8' - - -class DepthCamera(Camera): - - """ - Camera implementation details for depth camera - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the relative spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(DepthCamera, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode) - - self.listen() - - def get_carla_image_data_array(self, carla_image): - """ - Function (override) to convert the carla image to a numpy data array - as input for the cv_bridge.cv2_to_imgmsg() function - - The depth camera raw image is converted to a linear depth image - having 1-channel float32. - - :param carla_image: carla image object - :type carla_image: carla.Image - :return tuple (numpy data array containing the image information, encoding) - :rtype tuple(numpy.ndarray, string) - """ - - # color conversion within C++ code is broken, when transforming a - # 4-channel uint8 color pixel into a 1-channel float32 grayscale pixel - # therefore, we do it on our own here - # - # @todo: After fixing https://github.com/carla-simulator/carla/issues/1041 - # the final code in here should look like: - # - # carla_image.convert(carla.ColorConverter.Depth) - # - # carla_image_data_array = numpy.ndarray( - # shape=(carla_image.height, carla_image.width, 1), - # dtype=numpy.float32, buffer=carla_image.raw_data) - # - bgra_image = numpy.ndarray( - shape=(carla_image.height, carla_image.width, 4), - dtype=numpy.uint8, buffer=carla_image.raw_data) - - # Apply (R + G * 256 + B * 256 * 256) / (256**3 - 1) * 1000 - # according to the documentation: - # https://carla.readthedocs.io/en/latest/cameras_and_sensors/#camera-depth-map - scales = numpy.array([65536.0, 256.0, 1.0, 0]) / (256**3 - 1) * 1000 - depth_image = numpy.dot(bgra_image, scales).astype(numpy.float32) - - # actually we want encoding '32FC1' - # which is automatically selected by cv bridge with passthrough - return depth_image, 'passthrough' - - -class SemanticSegmentationCamera(Camera): - - """ - Camera implementation details for segmentation camera - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the relative spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super( - SemanticSegmentationCamera, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - synchronous_mode=synchronous_mode, - carla_actor=carla_actor) - - self.listen() - - def get_carla_image_data_array(self, carla_image): - """ - Function (override) to convert the carla image to a numpy data array - as input for the cv_bridge.cv2_to_imgmsg() function - - The segmentation camera raw image is converted to the city scapes palette image - having 4-channel uint8. - - :param carla_image: carla image object - :type carla_image: carla.Image - :return tuple (numpy data array containing the image information, encoding) - :rtype tuple(numpy.ndarray, string) - """ - - carla_image.convert(carla.ColorConverter.CityScapesPalette) - carla_image_data_array = numpy.ndarray( - shape=(carla_image.height, carla_image.width, 4), - dtype=numpy.uint8, buffer=carla_image.raw_data) - return carla_image_data_array, 'bgra8' - - -class DVSCamera(Camera): - - """ - Sensor implementation details for dvs cameras - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): # pylint: disable=too-many-arguments - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the relative spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(DVSCamera, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode, - is_event_sensor=True) - - self._dvs_events = None - self.dvs_camera_publisher = node.new_publisher(PointCloud2, - self.get_topic_prefix() + - '/events', qos_profile=10) - - self.listen() - - def destroy(self): - super(DVSCamera, self).destroy() - self.node.destroy_publisher(self.dvs_camera_publisher) - - # pylint: disable=arguments-differ - def sensor_data_updated(self, carla_dvs_event_array): - """ - Function to transform the received DVS event array into a ROS message - - :param carla_dvs_event_array: dvs event array object - :type carla_image: carla.DVSEventArray - """ - super(DVSCamera, self).sensor_data_updated(carla_dvs_event_array) - - header = self.get_msg_header(timestamp=carla_dvs_event_array.timestamp) - fields = [ - PointField(name='x', offset=0, datatype=PointField.UINT16, count=1), - PointField(name='y', offset=2, datatype=PointField.UINT16, count=1), - PointField(name='t', offset=4, datatype=PointField.FLOAT64, count=1), - PointField(name='pol', offset=12, datatype=PointField.INT8, count=1) - ] - - dvs_events_msg = create_cloud(header, fields, self._dvs_events.tolist()) - self.dvs_camera_publisher.publish(dvs_events_msg) - - # pylint: disable=arguments-differ - def get_carla_image_data_array(self, carla_dvs_event_array): - """ - Function (override) to convert the carla dvs event array to a numpy data array - as input for the cv_bridge.cv2_to_imgmsg() function - - The carla.DVSEventArray is converted into a 3-channel int8 color image format (bgr). - - :param carla_dvs_event_array: dvs event array object - :type carla_dvs_event_array: carla.DVSEventArray - :return tuple (numpy data array containing the image information, encoding) - :rtype tuple(numpy.ndarray, string) - """ - self._dvs_events = numpy.frombuffer(carla_dvs_event_array.raw_data, - dtype=numpy.dtype([ - ('x', numpy.uint16), - ('y', numpy.uint16), - ('t', numpy.int64), - ('pol', numpy.bool) - ])) - carla_image_data_array = numpy.zeros( - (carla_dvs_event_array.height, carla_dvs_event_array.width, 3), - dtype=numpy.uint8) - # Blue is positive, red is negative - carla_image_data_array[self._dvs_events[:]['y'], self._dvs_events[:]['x'], - self._dvs_events[:]['pol'] * 2] = 255 - - return carla_image_data_array, 'bgr8' diff --git a/carla_ros_bridge/src/carla_ros_bridge/carla_status_publisher.py b/carla_ros_bridge/src/carla_ros_bridge/carla_status_publisher.py deleted file mode 100644 index 7e39eb52..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/carla_status_publisher.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -report the carla status -""" -import os - -import ros_compatibility as roscomp - -from carla_msgs.msg import CarlaStatus # pylint: disable=import-error - - -class CarlaStatusPublisher(object): - """ - report the carla status - """ - - def __init__(self, synchronous_mode, fixed_delta_seconds, node): - """ - Constructor - - """ - self.synchronous_mode = synchronous_mode - self.synchronous_mode_running = True - self.fixed_delta_seconds = fixed_delta_seconds - self.node = node - if self.fixed_delta_seconds is None: - self.fixed_delta_seconds = 0. - self.frame = 0 - - callback_group = roscomp.callback_groups.ReentrantCallbackGroup() - self.carla_status_publisher = self.node.new_publisher(CarlaStatus, "/carla/status", qos_profile=10, - callback_group=callback_group) - self.publish() - - def destroy(self): - self.node.destroy_publisher(self.carla_status_publisher) - - def publish(self): - """ - publish the current status - - """ - status_msg = CarlaStatus() - status_msg.frame = self.frame - status_msg.synchronous_mode = self.synchronous_mode - status_msg.synchronous_mode_running = self.synchronous_mode_running - status_msg.fixed_delta_seconds = self.fixed_delta_seconds - self.carla_status_publisher.publish(status_msg) - - def set_synchronous_mode_running(self, running): - """ - set the value 'synchronous_mode_running' - """ - if self.synchronous_mode_running != running: - self.synchronous_mode_running = running - self.publish() - - def set_frame(self, frame): - """ - set the value 'synchronous_mode_running' - """ - if self.frame != frame: - self.frame = frame - self.publish() diff --git a/carla_ros_bridge/src/carla_ros_bridge/collision_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/collision_sensor.py deleted file mode 100644 index aaaf4ae7..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/collision_sensor.py +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle collision events -""" - -from carla_ros_bridge.sensor import Sensor - -from carla_msgs.msg import CarlaCollisionEvent - - -class CollisionSensor(Sensor): - - """ - Actor implementation details for a collision sensor - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the relative spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(CollisionSensor, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode, - is_event_sensor=True) - - self.collision_publisher = node.new_publisher(CarlaCollisionEvent, - self.get_topic_prefix(), - qos_profile=10) - self.listen() - - def destroy(self): - super(CollisionSensor, self).destroy() - self.node.destroy_publisher(self.collision_publisher) - - # pylint: disable=arguments-differ - def sensor_data_updated(self, collision_event): - """ - Function to wrap the collision event into a ros messsage - - :param collision_event: carla collision event object - :type collision_event: carla.CollisionEvent - """ - collision_msg = CarlaCollisionEvent() - collision_msg.header = self.get_msg_header(timestamp=collision_event.timestamp) - collision_msg.other_actor_id = collision_event.other_actor.id - collision_msg.normal_impulse.x = collision_event.normal_impulse.x - collision_msg.normal_impulse.y = collision_event.normal_impulse.y - collision_msg.normal_impulse.z = collision_event.normal_impulse.z - - self.collision_publisher.publish(collision_msg) diff --git a/carla_ros_bridge/src/carla_ros_bridge/debug_helper.py b/carla_ros_bridge/src/carla_ros_bridge/debug_helper.py deleted file mode 100644 index 4f6ee567..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/debug_helper.py +++ /dev/null @@ -1,153 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Class to draw marker -""" -import math -import os - -import carla -from transforms3d.euler import quat2euler - -from visualization_msgs.msg import Marker, MarkerArray # pylint: disable=import-error - - -class DebugHelper(object): - """ - Helper to draw markers in CARLA - """ - - def __init__(self, carla_debug_helper, node): - """ - Constructor - - :param carla_debug_helper: carla debug helper - :type carla_debug_helper: carla.DebugHelper - """ - self.debug = carla_debug_helper - self.node = node - self.marker_subscriber = self.node.new_subscription(MarkerArray, "/carla/debug_marker", - self.on_marker, qos_profile=10) - - def destroy(self): - """ - Function (override) to destroy this object. - - Terminate ROS subscriptions - - :return: - """ - self.node.logdebug("Destroy DebugHelper") - self.debug = None - self.node.destroy_subscription(self.marker_subscriber) - - def on_marker(self, marker_array): - """ - Receive markers from ROS and apply in CARLA - """ - for marker in marker_array.markers: - if marker.header.frame_id != "map": - self.node.logwarn("Could not draw marker in frame '{}'. Only 'map' supported.".format( - marker.header.frame_id)) - continue - lifetime = -1. - if marker.lifetime: - lifetime = marker.lifetime.to_sec() - color = carla.Color(int(marker.color.r * 255), int(marker.color.g * 255), - int(marker.color.b * 255), int(marker.color.a * 255)) - - if marker.type == Marker.POINTS: - self.draw_points(marker, lifetime, color) - elif marker.type == Marker.LINE_STRIP: - self.draw_line_strips(marker, lifetime, color) - elif marker.type == Marker.ARROW: - self.draw_arrow(marker, lifetime, color) - elif marker.type == Marker.CUBE: - self.draw_box(marker, lifetime, color) - else: - self.node.logwarn( - "Marker type '{}' not supported.".format(marker.type)) - - def draw_arrow(self, marker, lifetime, color): - """ - draw arrow from ros marker - """ - if marker.points: - if not len(marker.points) == 2: - self.node.logwarn("Drawing arrow from points requires two points. Received {}".format( - len(marker.points))) - return - thickness = marker.scale.x - arrow_size = marker.scale.y - start = carla.Location(x=marker.points[0].x, y=-marker.points[0].y, - z=marker.points[0].z) - end = carla.Location(x=marker.points[1].x, y=-marker.points[1].y, z=marker.points[1].z) - self.node.loginfo("Draw Arrow from {} to {} (color: {}, lifetime: {}, " - "thickness: {}, arrow_size: {})".format(start, end, color, lifetime, - thickness, arrow_size)) - self.debug.draw_arrow(start, end, thickness=thickness, arrow_size=arrow_size, - color=color, life_time=lifetime) - - else: - self.node.logwarn("Drawing arrow from Position/Orientation not yet supported. " - "Please use points.") - - def draw_points(self, marker, lifetime, color): - """ - draw points from ros marker - """ - for point in marker.points: - location = carla.Location(x=point.x, y=-point.y, z=point.z) - size = marker.scale.x - self.node.loginfo("Draw Point {} (color: {}, lifetime: {}, size: {})".format( - location, color, lifetime, size)) - self.debug.draw_point(location, size=size, color=color, life_time=lifetime) - - def draw_line_strips(self, marker, lifetime, color): - """ - draw lines from ros marker - """ - if len(marker.points) < 2: - self.node.logwarn("Drawing line-strip requires at least two points. Received {}".format( - len(marker.points))) - return - last_point = None - thickness = marker.scale.x - for point in marker.points: - if last_point: - start = carla.Location(x=last_point.x, y=-last_point.y, z=last_point.z) - end = carla.Location(x=point.x, y=-point.y, z=point.z) - self.node.loginfo("Draw Line from {} to {} (color: {}, lifetime: {}, " - "thickness: {})".format(start, end, color, lifetime, thickness)) - self.debug.draw_line(start, end, thickness=thickness, color=color, - life_time=lifetime) - last_point = point - - def draw_box(self, marker, lifetime, color): - """ - draw box from ros marker - """ - box = carla.BoundingBox() - box.location.x = marker.pose.position.x - box.location.y = -marker.pose.position.y - box.location.z = marker.pose.position.z - box.extent.x = marker.scale.x / 2 - box.extent.y = marker.scale.y / 2 - box.extent.z = marker.scale.z / 2 - - roll, pitch, yaw = quat2euler([ - marker.pose.orientation.w, marker.pose.orientation.x, marker.pose.orientation.y, marker.pose.orientation.z - - ]) - rotation = carla.Rotation() - rotation.roll = math.degrees(roll) - rotation.pitch = math.degrees(pitch) - rotation.yaw = -math.degrees(yaw) - self.node.loginfo("Draw Box {} (rotation: {}, color: {}, lifetime: {})".format( - box, rotation, color, lifetime)) - self.debug.draw_box(box, rotation, thickness=0.1, color=color, life_time=lifetime) diff --git a/carla_ros_bridge/src/carla_ros_bridge/ego_vehicle.py b/carla_ros_bridge/src/carla_ros_bridge/ego_vehicle.py deleted file mode 100644 index 5b5b4319..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/ego_vehicle.py +++ /dev/null @@ -1,287 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla vehicles -""" -import math -import os - -import numpy -from carla import VehicleControl - -from ros_compatibility.qos import QoSProfile, DurabilityPolicy - -from carla_ros_bridge.vehicle import Vehicle - -from carla_msgs.msg import ( - CarlaEgoVehicleInfo, - CarlaEgoVehicleInfoWheel, - CarlaEgoVehicleControl, - CarlaEgoVehicleStatus -) -from std_msgs.msg import Bool # pylint: disable=import-error -from std_msgs.msg import ColorRGBA # pylint: disable=import-error - - -class EgoVehicle(Vehicle): - - """ - Vehicle implementation details for the ego vehicle - """ - - def __init__(self, uid, name, parent, node, carla_actor, vehicle_control_applied_callback): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - """ - super(EgoVehicle, self).__init__(uid=uid, - name=name, - parent=parent, - node=node, - carla_actor=carla_actor) - - self.vehicle_info_published = False - self.vehicle_control_override = False - self._vehicle_control_applied_callback = vehicle_control_applied_callback - - self.vehicle_status_publisher = node.new_publisher( - CarlaEgoVehicleStatus, - self.get_topic_prefix() + "/vehicle_status", - qos_profile=10) - self.vehicle_info_publisher = node.new_publisher( - CarlaEgoVehicleInfo, - self.get_topic_prefix() + - "/vehicle_info", - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - - self.control_subscriber = node.new_subscription( - CarlaEgoVehicleControl, - self.get_topic_prefix() + "/vehicle_control_cmd", - lambda data: self.control_command_updated(data, manual_override=False), - qos_profile=10) - - self.manual_control_subscriber = node.new_subscription( - CarlaEgoVehicleControl, - self.get_topic_prefix() + "/vehicle_control_cmd_manual", - lambda data: self.control_command_updated(data, manual_override=True), - qos_profile=10) - - self.control_override_subscriber = node.new_subscription( - Bool, - self.get_topic_prefix() + "/vehicle_control_manual_override", - self.control_command_override, - qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - - self.enable_autopilot_subscriber = node.new_subscription( - Bool, - self.get_topic_prefix() + "/enable_autopilot", - self.enable_autopilot_updated, - qos_profile=10) - - def get_marker_color(self): - """ - Function (override) to return the color for marker messages. - - The ego vehicle uses a different marker color than other vehicles. - - :return: the color used by a ego vehicle marker - :rtpye : std_msgs.msg.ColorRGBA - """ - color = ColorRGBA() - color.r = 0.0 - color.g = 255.0 - color.b = 0.0 - return color - - def send_vehicle_msgs(self, frame, timestamp): - """ - send messages related to vehicle status - - :return: - """ - vehicle_status = CarlaEgoVehicleStatus( - header=self.get_msg_header("map", timestamp=timestamp)) - vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor) - vehicle_status.acceleration.linear = self.get_current_ros_accel().linear - vehicle_status.orientation = self.get_current_ros_pose().orientation - vehicle_status.control.throttle = self.carla_actor.get_control().throttle - vehicle_status.control.steer = self.carla_actor.get_control().steer - vehicle_status.control.brake = self.carla_actor.get_control().brake - vehicle_status.control.hand_brake = self.carla_actor.get_control().hand_brake - vehicle_status.control.reverse = self.carla_actor.get_control().reverse - vehicle_status.control.gear = self.carla_actor.get_control().gear - vehicle_status.control.manual_gear_shift = self.carla_actor.get_control().manual_gear_shift - self.vehicle_status_publisher.publish(vehicle_status) - - # only send vehicle once (in latched-mode) - if not self.vehicle_info_published: - self.vehicle_info_published = True - vehicle_info = CarlaEgoVehicleInfo() - vehicle_info.id = self.carla_actor.id - vehicle_info.type = self.carla_actor.type_id - vehicle_info.rolename = self.carla_actor.attributes.get('role_name') - vehicle_physics = self.carla_actor.get_physics_control() - - for wheel in vehicle_physics.wheels: - wheel_info = CarlaEgoVehicleInfoWheel() - wheel_info.tire_friction = wheel.tire_friction - wheel_info.damping_rate = wheel.damping_rate - wheel_info.max_steer_angle = math.radians(wheel.max_steer_angle) - wheel_info.radius = wheel.radius - wheel_info.max_brake_torque = wheel.max_brake_torque - wheel_info.max_handbrake_torque = wheel.max_handbrake_torque - - inv_T = numpy.array(self.carla_actor.get_transform().get_inverse_matrix(), dtype=float) - wheel_pos_in_map = numpy.array([wheel.position.x/100.0, - wheel.position.y/100.0, - wheel.position.z/100.0, - 1.0]) - wheel_pos_in_ego_vehicle = numpy.matmul(inv_T, wheel_pos_in_map) - wheel_info.position.x = wheel_pos_in_ego_vehicle[0] - wheel_info.position.y = -wheel_pos_in_ego_vehicle[1] - wheel_info.position.z = wheel_pos_in_ego_vehicle[2] - vehicle_info.wheels.append(wheel_info) - - vehicle_info.max_rpm = vehicle_physics.max_rpm - vehicle_info.max_rpm = vehicle_physics.max_rpm - vehicle_info.moi = vehicle_physics.moi - vehicle_info.damping_rate_full_throttle = vehicle_physics.damping_rate_full_throttle - vehicle_info.damping_rate_zero_throttle_clutch_engaged = \ - vehicle_physics.damping_rate_zero_throttle_clutch_engaged - vehicle_info.damping_rate_zero_throttle_clutch_disengaged = \ - vehicle_physics.damping_rate_zero_throttle_clutch_disengaged - vehicle_info.use_gear_autobox = vehicle_physics.use_gear_autobox - vehicle_info.gear_switch_time = vehicle_physics.gear_switch_time - vehicle_info.clutch_strength = vehicle_physics.clutch_strength - vehicle_info.mass = vehicle_physics.mass - vehicle_info.drag_coefficient = vehicle_physics.drag_coefficient - vehicle_info.center_of_mass.x = vehicle_physics.center_of_mass.x - vehicle_info.center_of_mass.y = vehicle_physics.center_of_mass.y - vehicle_info.center_of_mass.z = vehicle_physics.center_of_mass.z - - self.vehicle_info_publisher.publish(vehicle_info) - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - - On update ego vehicle calculates and sends the new values for VehicleControl() - - :return: - """ - self.send_vehicle_msgs(frame, timestamp) - super(EgoVehicle, self).update(frame, timestamp) - - def destroy(self): - """ - Function (override) to destroy this object. - - Terminate ROS subscriptions - Finally forward call to super class. - - :return: - """ - self.node.logdebug("Destroy Vehicle(id={})".format(self.get_id())) - self.node.destroy_subscription(self.control_subscriber) - self.node.destroy_subscription(self.enable_autopilot_subscriber) - self.node.destroy_subscription(self.control_override_subscriber) - self.node.destroy_subscription(self.manual_control_subscriber) - self.node.destroy_publisher(self.vehicle_status_publisher) - self.node.destroy_publisher(self.vehicle_info_publisher) - Vehicle.destroy(self) - - def control_command_override(self, enable): - """ - Set the vehicle control mode according to ros topic - """ - self.vehicle_control_override = enable.data - - def control_command_updated(self, ros_vehicle_control, manual_override): - """ - Receive a CarlaEgoVehicleControl msg and send to CARLA - - This function gets called whenever a ROS CarlaEgoVehicleControl is received. - If the mode is valid (either normal or manual), the received ROS message is - converted into carla.VehicleControl command and sent to CARLA. - This bridge is not responsible for any restrictions on velocity or steering. - It's just forwarding the ROS input to CARLA - - :param manual_override: manually override the vehicle control command - :param ros_vehicle_control: current vehicle control input received via ROS - :type ros_vehicle_control: carla_msgs.msg.CarlaEgoVehicleControl - :return: - """ - if manual_override == self.vehicle_control_override: - vehicle_control = VehicleControl() - vehicle_control.hand_brake = ros_vehicle_control.hand_brake - vehicle_control.brake = ros_vehicle_control.brake - vehicle_control.steer = ros_vehicle_control.steer - vehicle_control.throttle = ros_vehicle_control.throttle - vehicle_control.reverse = ros_vehicle_control.reverse - vehicle_control.manual_gear_shift = ros_vehicle_control.manual_gear_shift - vehicle_control.gear = ros_vehicle_control.gear - self.carla_actor.apply_control(vehicle_control) - self._vehicle_control_applied_callback(self.get_id()) - - def enable_autopilot_updated(self, enable_auto_pilot): - """ - Enable/disable auto pilot - - :param enable_auto_pilot: should the autopilot be enabled? - :type enable_auto_pilot: std_msgs.Bool - :return: - """ - self.node.logdebug("Ego vehicle: Set autopilot to {}".format(enable_auto_pilot.data)) - self.carla_actor.set_autopilot(enable_auto_pilot.data) - - @staticmethod - 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 - - @staticmethod - 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 EgoVehicle.get_vector_length_squared(carla_vehicle.get_velocity()) - - @staticmethod - 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(EgoVehicle.get_vehicle_speed_squared(carla_vehicle)) - return speed diff --git a/carla_ros_bridge/src/carla_ros_bridge/gnss.py b/carla_ros_bridge/src/carla_ros_bridge/gnss.py deleted file mode 100644 index b560add2..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/gnss.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla gnsss -""" - -from carla_ros_bridge.sensor import Sensor - -from sensor_msgs.msg import NavSatFix - - -class Gnss(Sensor): - - """ - Actor implementation details for gnss sensor - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the relative spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(Gnss, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode) - - self.gnss_publisher = node.new_publisher(NavSatFix, - self.get_topic_prefix(), - qos_profile=10) - self.listen() - - def destroy(self): - super(Gnss, self).destroy() - self.node.destroy_publisher(self.gnss_publisher) - - # pylint: disable=arguments-differ - def sensor_data_updated(self, carla_gnss_measurement): - """ - Function to transform a received gnss event into a ROS NavSatFix message - - :param carla_gnss_measurement: carla gnss measurement object - :type carla_gnss_measurement: carla.GnssMeasurement - """ - navsatfix_msg = NavSatFix() - navsatfix_msg.header = self.get_msg_header(timestamp=carla_gnss_measurement.timestamp) - navsatfix_msg.latitude = carla_gnss_measurement.latitude - navsatfix_msg.longitude = carla_gnss_measurement.longitude - navsatfix_msg.altitude = carla_gnss_measurement.altitude - self.gnss_publisher.publish(navsatfix_msg) diff --git a/carla_ros_bridge/src/carla_ros_bridge/imu.py b/carla_ros_bridge/src/carla_ros_bridge/imu.py deleted file mode 100644 index 9ba52ec0..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/imu.py +++ /dev/null @@ -1,88 +0,0 @@ -#!usr/bin/env python -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla imu sensor -""" - -from transforms3d.euler import euler2quat - -import carla_common.transforms as trans - -from carla_ros_bridge.sensor import Sensor - -from sensor_msgs.msg import Imu - - -class ImuSensor(Sensor): - - """ - Actor implementation details for imu sensor - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the relative spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor : carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(ImuSensor, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode) - - self.imu_publisher = node.new_publisher(Imu, self.get_topic_prefix(), qos_profile=10) - self.listen() - - def destroy(self): - super(ImuSensor, self).destroy() - self.node.destroy_publisher(self.imu_publisher) - - # pylint: disable=arguments-differ - def sensor_data_updated(self, carla_imu_measurement): - """ - Function to transform a received imu measurement into a ROS Imu message - - :param carla_imu_measurement: carla imu measurement object - :type carla_imu_measurement: carla.IMUMeasurement - """ - imu_msg = Imu() - imu_msg.header = self.get_msg_header(timestamp=carla_imu_measurement.timestamp) - - # Carla uses a left-handed coordinate convention (X forward, Y right, Z up). - # Here, these measurements are converted to the right-handed ROS convention - # (X forward, Y left, Z up). - imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x - imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y - imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z - - imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x - imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y - imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z - - roll, pitch, yaw = trans.carla_rotation_to_RPY(carla_imu_measurement.transform.rotation) - quat = euler2quat(roll, pitch, yaw) - imu_msg.orientation.w = quat[0] - imu_msg.orientation.x = quat[1] - imu_msg.orientation.y = quat[2] - imu_msg.orientation.z = quat[3] - - self.imu_publisher.publish(imu_msg) diff --git a/carla_ros_bridge/src/carla_ros_bridge/lane_invasion_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/lane_invasion_sensor.py deleted file mode 100644 index 0c77da05..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/lane_invasion_sensor.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle lane invasion events -""" - -from carla_ros_bridge.sensor import Sensor - -from carla_msgs.msg import CarlaLaneInvasionEvent - - -class LaneInvasionSensor(Sensor): - - """ - Actor implementation details for a lane invasion sensor - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(LaneInvasionSensor, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode, - is_event_sensor=True) - - self.lane_invasion_publisher = node.new_publisher(CarlaLaneInvasionEvent, - self.get_topic_prefix(), - qos_profile=10) - self.listen() - - def destroy(self): - super(LaneInvasionSensor, self).destroy() - self.node.destroy_publisher(self.lane_invasion_publisher) - - # pylint: disable=arguments-differ - def sensor_data_updated(self, lane_invasion_event): - """ - Function to wrap the lane invasion event into a ros messsage - - :param lane_invasion_event: carla lane invasion event object - :type lane_invasion_event: carla.LaneInvasionEvent - """ - lane_invasion_msg = CarlaLaneInvasionEvent() - lane_invasion_msg.header = self.get_msg_header(timestamp=lane_invasion_event.timestamp) - for marking in lane_invasion_event.crossed_lane_markings: - lane_invasion_msg.crossed_lane_markings.append(marking.type) - self.lane_invasion_publisher.publish(lane_invasion_msg) diff --git a/carla_ros_bridge/src/carla_ros_bridge/lidar.py b/carla_ros_bridge/src/carla_ros_bridge/lidar.py deleted file mode 100644 index 9cb39ab1..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/lidar.py +++ /dev/null @@ -1,165 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018, Willow Garage, Inc. -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla lidars -""" - -import numpy - -from carla_ros_bridge.sensor import Sensor, create_cloud - -from sensor_msgs.msg import PointCloud2, PointField - - -class Lidar(Sensor): - - """ - Actor implementation details for lidars - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(Lidar, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode) - - self.lidar_publisher = node.new_publisher(PointCloud2, - self.get_topic_prefix(), - qos_profile=10) - self.listen() - - def destroy(self): - super(Lidar, self).destroy() - self.node.destroy_publisher(self.lidar_publisher) - - # pylint: disable=arguments-differ - def sensor_data_updated(self, carla_lidar_measurement): - """ - Function to transform the a received lidar measurement into a ROS point cloud message - - :param carla_lidar_measurement: carla lidar measurement object - :type carla_lidar_measurement: carla.LidarMeasurement - """ - header = self.get_msg_header(timestamp=carla_lidar_measurement.timestamp) - fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), - PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1) - ] - - lidar_data = numpy.fromstring( - bytes(carla_lidar_measurement.raw_data), dtype=numpy.float32) - lidar_data = numpy.reshape( - lidar_data, (int(lidar_data.shape[0] / 4), 4)) - # we take the opposite of y axis - # (as lidar point are express in left handed coordinate system, and ros need right handed) - lidar_data[:, 1] *= -1 - point_cloud_msg = create_cloud(header, fields, lidar_data) - self.lidar_publisher.publish(point_cloud_msg) - - -class SemanticLidar(Sensor): - - """ - Actor implementation details for semantic lidars - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(SemanticLidar, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode) - - self.semantic_lidar_publisher = node.new_publisher( - PointCloud2, - self.get_topic_prefix(), - qos_profile=10) - self.listen() - - def destroy(self): - super(SemanticLidar, self).destroy() - self.node.destroy_publisher(self.semantic_lidar_publisher) - - # pylint: disable=arguments-differ - def sensor_data_updated(self, carla_lidar_measurement): - """ - Function to transform a received semantic lidar measurement into a ROS point cloud message - - :param carla_lidar_measurement: carla semantic lidar measurement object - :type carla_lidar_measurement: carla.SemanticLidarMeasurement - """ - header = self.get_msg_header(timestamp=carla_lidar_measurement.timestamp) - fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), - PointField(name='CosAngle', offset=12, datatype=PointField.FLOAT32, count=1), - PointField(name='ObjIdx', offset=16, datatype=PointField.UINT32, count=1), - PointField(name='ObjTag', offset=20, datatype=PointField.UINT32, count=1) - ] - - lidar_data = numpy.fromstring(bytes(carla_lidar_measurement.raw_data), - dtype=numpy.dtype([ - ('x', numpy.float32), - ('y', numpy.float32), - ('z', numpy.float32), - ('CosAngle', numpy.float32), - ('ObjIdx', numpy.uint32), - ('ObjTag', numpy.uint32) - ])) - - # we take the oposite of y axis - # (as lidar point are express in left handed coordinate system, and ros need right handed) - lidar_data['y'] *= -1 - point_cloud_msg = create_cloud(header, fields, lidar_data.tolist()) - self.semantic_lidar_publisher.publish(point_cloud_msg) diff --git a/carla_ros_bridge/src/carla_ros_bridge/marker_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/marker_sensor.py deleted file mode 100644 index ad13cd13..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/marker_sensor.py +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -handle a marker sensor -""" - -import itertools - -import carla - -from ros_compatibility.qos import QoSProfile, DurabilityPolicy - -from carla_ros_bridge.pseudo_actor import PseudoActor -from carla_ros_bridge.traffic_participant import TrafficParticipant -from carla_common.transforms import carla_location_to_ros_point, carla_rotation_to_ros_quaternion - -from std_msgs.msg import ColorRGBA -from visualization_msgs.msg import MarkerArray, Marker - -# Using colors from CityScapesPalette specified here: -# https://carla.readthedocs.io/en/latest/ref_sensors/#semantic-segmentation-camera -OBJECT_LABELS = { - #carla.CityObjectLabel.None: ColorRGBA(r=0.0, g=0.0, b=0.0, a=0.8), - carla.CityObjectLabel.Buildings: ColorRGBA(r=70.0/255.0, g=70.0/255.0, b=70.0/255.0, a=0.8), - carla.CityObjectLabel.Fences: ColorRGBA(r=100.0/255.0, g=40.0/255.0, b=40.0/255.0, a=0.8), - #carla.CityObjectLabel.Other: ColorRGBA(r=55.0/255.0, g=90.0/255.0, b=80.0/255.0, a=0.8), - #carla.CityObjectLabel.Pedestrians: ColorRGBA(r=220.0/255.0, g=20.0/255.0, b=60.0/255.0, a=0.8), - carla.CityObjectLabel.Poles: ColorRGBA(r=153.0/255.0, g=153.0/255.0, b=153.0/255.0, a=0.8), - carla.CityObjectLabel.RoadLines: ColorRGBA(r=157.0/255.0, g=234.0/255.0, b=50.0/255.0, a=0.8), - carla.CityObjectLabel.Roads: ColorRGBA(r=128.0/255.0, g=64.0/255.0, b=128.0/255.0, a=0.8), - carla.CityObjectLabel.Sidewalks: ColorRGBA(r=244.0/255.0, g=35.0/255.0, b=232.0/255.0, a=0.8), - carla.CityObjectLabel.Vegetation: ColorRGBA(r=107.0/255.0, g=142.0/255.0, b=35.0/255.0, a=0.8), - #carla.CityObjectLabel.Vehicles: ColorRGBA(r=0.0/255.0, g=0.0/255.0, b=142.0/255.0, a=0.8), - carla.CityObjectLabel.Walls: ColorRGBA(r=102.0/255.0, g=102.0/255.0, b=156.0/255.0, a=0.8), - carla.CityObjectLabel.TrafficSigns: ColorRGBA(r=220.0/255.0, g=220.0/255.0, b=0.0/255.0, a=0.8), - #carla.CityObjectLabel.Sky: ColorRGBA(r=70.0/255.0, g=130.0/255.0, b=180.0/255.0, a=0.8), - #carla.CityObjectLabel.Ground: ColorRGBA(r=81.0/255.0, g=0.0/255.0, b=81.0/255.0, a=0.8), - carla.CityObjectLabel.Bridge: ColorRGBA(r=150.0/255.0, g=100.0/255.0, b=100.0/255.0, a=0.8), - carla.CityObjectLabel.RailTrack: ColorRGBA(r=230.0/255.0, g=150.0/255.0, b=140.0/255.0, a=0.8), - carla.CityObjectLabel.GuardRail: ColorRGBA(r=180.0/255.0, g=165.0/255.0, b=180.0/255.0, a=0.8), - carla.CityObjectLabel.TrafficLight: ColorRGBA(r=250.0/255.0, g=170.0/255.0, b=30.0/255.0, a=0.8), - #carla.CityObjectLabel.Static: ColorRGBA(r=110.0/255.0, g=190.0/255.0, b=160.0/255.0, a=0.8), - #carla.CityObjectLabel.Dynamic: ColorRGBA(r=170.0/255.0, g=120.0/255.0, b=50.0/255.0, a=0.8), - #carla.CityObjectLabel.Water: ColorRGBA(r=45.0/255.0, g=60.0/255.0, b=150.0/255.0, a=0.8), - #carla.CityObjectLabel.Terrain: ColorRGBA(r=145.0/255.0, g=170.0/255.0, b=100.0/255.0, a=0.8), -} - - -class MarkerSensor(PseudoActor): - - """ - Pseudo marker sensor - """ - - def __init__(self, uid, name, parent, node, actor_list, world): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param carla_world: carla world object - :type carla_world: carla.World - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - :param actor_list: current list of actors - :type actor_list: map(carla-actor-id -> python-actor-object) - """ - - super(MarkerSensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - self.actor_list = actor_list - self.world = world - self.node = node - - self.marker_publisher = node.new_publisher(MarkerArray, - self.get_topic_prefix(), - qos_profile=10) - self.static_marker_publisher = node.new_publisher(MarkerArray, - self.get_topic_prefix() + "/static", - qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - - # id generator for static objects. - self.static_id_gen = itertools.count(1) - - static_markers = self._get_static_markers(OBJECT_LABELS.keys()) - if static_markers: - self.static_marker_publisher.publish(static_markers) - - def destroy(self): - """ - Function to destroy this object. - :return: - """ - self.actor_list = None - self.node.destroy_publisher(self.marker_publisher) - self.node.destroy_publisher(self.static_marker_publisher) - super(MarkerSensor, self).destroy() - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - return "sensor.pseudo.markers" - - def _get_marker_from_environment_object(self, environment_object): - marker = Marker(header=self.get_msg_header(frame_id="map")) - marker.ns = str(environment_object.type) - marker.id = next(self.static_id_gen) - - box = environment_object.bounding_box - marker.pose.position = carla_location_to_ros_point(box.location) - marker.pose.orientation = carla_rotation_to_ros_quaternion(box.rotation) - marker.scale.x = max(0.1, 2*box.extent.x) - marker.scale.y = max(0.1, 2*box.extent.y) - marker.scale.z = max(0.1, 2*box.extent.z) - marker.type = Marker.CUBE - - marker.color = OBJECT_LABELS.get(environment_object.type, ColorRGBA(r=1.0, g=0.0, b=0.0, a=0.8)) - return marker - - def _get_static_markers(self, object_types): - static_markers = MarkerArray() - for object_type in object_types: - objects = self.world.get_environment_objects(object_type) - for obj in objects: - marker = self._get_marker_from_environment_object(obj) - static_markers.markers.append(marker) - - return static_markers - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - On update map sends: - - tf global frame - :return: - """ - marker_array_msg = MarkerArray() - for actor in self.actor_list.values(): - if isinstance(actor, TrafficParticipant): - marker_array_msg.markers.append(actor.get_marker(timestamp)) - self.marker_publisher.publish(marker_array_msg) diff --git a/carla_ros_bridge/src/carla_ros_bridge/object_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/object_sensor.py deleted file mode 100644 index 7d881444..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/object_sensor.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -handle a object sensor -""" - -from carla_ros_bridge.pseudo_actor import PseudoActor -from carla_ros_bridge.vehicle import Vehicle -from carla_ros_bridge.walker import Walker - -from derived_object_msgs.msg import ObjectArray - - -class ObjectSensor(PseudoActor): - - """ - Pseudo object sensor - """ - - def __init__(self, uid, name, parent, node, actor_list): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param actor_list: current list of actors - :type actor_list: map(carla-actor-id -> python-actor-object) - """ - - super(ObjectSensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - self.actor_list = actor_list - self.object_publisher = node.new_publisher(ObjectArray, - self.get_topic_prefix(), - qos_profile=10) - - def destroy(self): - """ - Function to destroy this object. - :return: - """ - super(ObjectSensor, self).destroy() - self.actor_list = None - self.node.destroy_publisher(self.object_publisher) - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - return "sensor.pseudo.objects" - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - On update map sends: - - tf global frame - :return: - """ - ros_objects = ObjectArray() - ros_objects.header = self.get_msg_header(frame_id="map", timestamp=timestamp) - for actor_id in self.actor_list.keys(): - # currently only Vehicles and Walkers are added to the object array - if self.parent is None or self.parent.uid != actor_id: - actor = self.actor_list[actor_id] - if isinstance(actor, Vehicle): - ros_objects.objects.append(actor.get_object_info()) - elif isinstance(actor, Walker): - ros_objects.objects.append(actor.get_object_info()) - self.object_publisher.publish(ros_objects) diff --git a/carla_ros_bridge/src/carla_ros_bridge/odom_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/odom_sensor.py deleted file mode 100644 index ac6279fb..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/odom_sensor.py +++ /dev/null @@ -1,74 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -handle a odom sensor -""" - -from carla_ros_bridge.pseudo_actor import PseudoActor - -from nav_msgs.msg import Odometry - - -class OdometrySensor(PseudoActor): - - """ - Pseudo odometry sensor - """ - - def __init__(self, uid, name, parent, node): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param carla_world: carla world object - :type carla_world: carla.World - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - """ - - super(OdometrySensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - - self.odometry_publisher = node.new_publisher(Odometry, - self.get_topic_prefix(), - qos_profile=10) - - def destroy(self): - super(OdometrySensor, self).destroy() - self.node.destroy_publisher(self.odometry_publisher) - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - return "sensor.pseudo.odom" - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - """ - odometry = Odometry(header=self.parent.get_msg_header("map", timestamp=timestamp)) - odometry.child_frame_id = self.parent.get_prefix() - try: - odometry.pose.pose = self.parent.get_current_ros_pose() - odometry.twist.twist = self.parent.get_current_ros_twist_rotated() - except AttributeError: - # parent actor disappeared, do not send tf - self.node.logwarn( - "OdometrySensor could not publish. parent actor {} not found".format(self.parent.uid)) - return - self.odometry_publisher.publish(odometry) diff --git a/carla_ros_bridge/src/carla_ros_bridge/opendrive_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/opendrive_sensor.py deleted file mode 100644 index 83a5a4c4..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/opendrive_sensor.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -handle a opendrive sensor -""" - -from carla_ros_bridge.pseudo_actor import PseudoActor - -from ros_compatibility.qos import QoSProfile, DurabilityPolicy - -from std_msgs.msg import String - - -class OpenDriveSensor(PseudoActor): - - """ - Pseudo opendrive sensor - """ - - def __init__(self, uid, name, parent, node, carla_map): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - :param carla_map: carla map object - :type carla_map: carla.Map - """ - super(OpenDriveSensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - self.carla_map = carla_map - self._map_published = False - self.map_publisher = node.new_publisher( - String, - self.get_topic_prefix(), - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - - def destroy(self): - super(OpenDriveSensor, self).destroy() - self.node.destroy_publisher(self.map_publisher) - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - return "sensor.pseudo.opendrive_map" - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - """ - if not self._map_published: - self.map_publisher.publish(String(data=self.carla_map.to_opendrive())) - self._map_published = True diff --git a/carla_ros_bridge/src/carla_ros_bridge/pseudo_actor.py b/carla_ros_bridge/src/carla_ros_bridge/pseudo_actor.py deleted file mode 100644 index 6cdfa0f9..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/pseudo_actor.py +++ /dev/null @@ -1,108 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Base Class to handle Pseudo Actors (that are not existing in Carla world) -""" - -import numpy as np - -import ros_compatibility as roscomp - -from std_msgs.msg import Header - - -class PseudoActor(object): - - """ - Generic base class for Pseudo actors (that are not existing in Carla world) - """ - - def __init__(self, uid, name, parent, node): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.PseudoActor - :param prefix: the topic prefix to be used for this actor - :type prefix: string - :param node: node-handle - :type node: CompatibleNode - """ - self.uid = uid - self.name = name - self.parent = parent - self.node = node - - if self.uid is None: - raise TypeError("Actor ID is not set") - - if self.uid > np.iinfo(np.uint32).max: - raise ValueError("Actor ID exceeds maximum supported value '{}'".format(self.uid)) - - def destroy(self): - """ - Function to destroy this object. - :return: - """ - self.parent = None - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - raise NotImplementedError( - "The pseudo actor is missing a blueprint name") - - def get_msg_header(self, frame_id=None, timestamp=None): - """ - Get a filled ROS message header - :return: ROS message header - :rtype: std_msgs.msg.Header - """ - header = Header() - if frame_id: - header.frame_id = frame_id - else: - header.frame_id = self.get_prefix() - - if not timestamp: - timestamp = self.node.get_time() - header.stamp = roscomp.ros_timestamp(sec=timestamp, from_sec=True) - return header - - def get_prefix(self): - """ - get the fully qualified prefix of object - :return: prefix - :rtype: string - """ - if self.parent is not None: - return self.parent.get_prefix() + "/" + self.name - else: - return self.name - - def get_topic_prefix(self): - """ - get the topic name of the current entity. - - :return: the final topic name of this object - :rtype: string - """ - return "/carla/" + self.get_prefix() - - def update(self, frame, timestamp): - """ - Function to update this object. Derived classes can add code. - """ - pass diff --git a/carla_ros_bridge/src/carla_ros_bridge/radar.py b/carla_ros_bridge/src/carla_ros_bridge/radar.py deleted file mode 100644 index a2afc8cd..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/radar.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2019-2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla Radar -""" - -import numpy as np - -from carla_ros_bridge.sensor import Sensor, create_cloud - -from sensor_msgs.msg import PointCloud2, PointField - - -class Radar(Sensor): - - """ - Actor implementation details of Carla RADAR - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - """ - super(Radar, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor, - synchronous_mode=synchronous_mode) - - self.radar_publisher = node.new_publisher(PointCloud2, self.get_topic_prefix(), qos_profile=10) - self.listen() - - def destroy(self): - super(Radar, self).destroy() - self.node.destroy_publisher(self.radar_publisher) - - # pylint: disable=arguments-differ - def sensor_data_updated(self, carla_radar_measurement): - """ - Function to transform the a received Radar measurement into a ROS message - :param carla_radar_measurement: carla Radar measurement object - :type carla_radar_measurement: carla.RadarMeasurement - """ - fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), - PointField(name='Range', offset=12, datatype=PointField.FLOAT32, count=1), - PointField(name='Velocity', offset=16, datatype=PointField.FLOAT32, count=1), - PointField(name='AzimuthAngle', offset=20, datatype=PointField.FLOAT32, count=1), - PointField(name='ElevationAngle', offset=28, datatype=PointField.FLOAT32, count=1)] - points = [] - for detection in carla_radar_measurement: - points.append([detection.depth * np.cos(detection.azimuth) * np.cos(-detection.altitude), - detection.depth * np.sin(-detection.azimuth) * - np.cos(detection.altitude), - detection.depth * np.sin(detection.altitude), - detection.depth, detection.velocity, detection.azimuth, detection.altitude]) - radar_msg = create_cloud(self.get_msg_header( - timestamp=carla_radar_measurement.timestamp), fields, points) - self.radar_publisher.publish(radar_msg) diff --git a/carla_ros_bridge/src/carla_ros_bridge/rss_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/rss_sensor.py deleted file mode 100644 index f81fec25..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/rss_sensor.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Class to handle rss sensor -""" - -from carla_ros_bridge.actor import Actor - - -class RssSensor(Actor): - - """ - Actor implementation details for a RSS sensor - - As the RSS sensor in CARLA requires additional - utilization it's not handled as a sensor here. - """ - - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, _): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - """ - - super(RssSensor, self).__init__(uid=uid, - name=name, - parent=parent, - relative_spawn_pose=relative_spawn_pose, - node=node, - carla_actor=carla_actor) diff --git a/carla_ros_bridge/src/carla_ros_bridge/sensor.py b/carla_ros_bridge/src/carla_ros_bridge/sensor.py deleted file mode 100644 index 1bdd36c3..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/sensor.py +++ /dev/null @@ -1,312 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla sensors -""" - -from __future__ import print_function - -import ctypes -import os -try: - import queue -except ImportError: - import Queue as queue -import struct -import sys -from abc import abstractmethod -from threading import Lock - -import carla_common.transforms as trans -import ros_compatibility as roscomp -import tf2_ros - -from carla_ros_bridge.actor import Actor - -from sensor_msgs.msg import PointCloud2, PointField - -ROS_VERSION = roscomp.get_ros_version() - -_DATATYPES = {} -_DATATYPES[PointField.INT8] = ('b', 1) -_DATATYPES[PointField.UINT8] = ('B', 1) -_DATATYPES[PointField.INT16] = ('h', 2) -_DATATYPES[PointField.UINT16] = ('H', 2) -_DATATYPES[PointField.INT32] = ('i', 4) -_DATATYPES[PointField.UINT32] = ('I', 4) -_DATATYPES[PointField.FLOAT32] = ('f', 4) -_DATATYPES[PointField.FLOAT64] = ('d', 8) - - -class Sensor(Actor): - - """ - Actor implementation details for sensors - """ - - def __init__(self, # pylint: disable=too-many-arguments - uid, - name, - parent, - relative_spawn_pose, - node, - carla_actor, - synchronous_mode, - is_event_sensor=False, # only relevant in synchronous_mode: - # if a sensor only delivers data on special events, - # do not wait for it. That means you might get data from a - # sensor, that belongs to a different frame - ): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param relative_spawn_pose: the relative spawn pose of this - :type relative_spawn_pose: geometry_msgs.Pose - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - :param synchronous_mode: use in synchronous mode? - :type synchronous_mode: bool - :param prefix: the topic prefix to be used for this actor - :type prefix: string - """ - super(Sensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node, - carla_actor=carla_actor) - - self.relative_spawn_pose = relative_spawn_pose - self.synchronous_mode = synchronous_mode - self.queue = queue.Queue() - self.next_data_expected_time = None - self.sensor_tick_time = None - self.is_event_sensor = is_event_sensor - self._callback_active = Lock() - try: - self.sensor_tick_time = float(carla_actor.attributes["sensor_tick"]) - node.logdebug("Sensor tick time is {}".format(self.sensor_tick_time)) - except (KeyError, ValueError): - self.sensor_tick_time = None - - if ROS_VERSION == 1: - self._tf_broadcaster = tf2_ros.TransformBroadcaster() - elif ROS_VERSION == 2: - self._tf_broadcaster = tf2_ros.TransformBroadcaster(node) - - def get_ros_transform(self, pose, timestamp): - if self.synchronous_mode: - if not self.relative_spawn_pose: - self.node.logwarn("{}: No relative spawn pose defined".format(self.get_prefix())) - return - pose = self.relative_spawn_pose - child_frame_id = self.get_prefix() - if self.parent is not None: - frame_id = self.parent.get_prefix() - else: - frame_id = "map" - - else: - child_frame_id = self.get_prefix() - frame_id = "map" - - transform = tf2_ros.TransformStamped() - transform.header.stamp = roscomp.ros_timestamp(sec=timestamp, from_sec=True) - transform.header.frame_id = frame_id - transform.child_frame_id = child_frame_id - - transform.transform.translation.x = pose.position.x - transform.transform.translation.y = pose.position.y - transform.transform.translation.z = pose.position.z - - transform.transform.rotation.x = pose.orientation.x - transform.transform.rotation.y = pose.orientation.y - transform.transform.rotation.z = pose.orientation.z - transform.transform.rotation.w = pose.orientation.w - - return transform - - def publish_tf(self, pose, timestamp): - transform = self.get_ros_transform(pose, timestamp) - try: - self._tf_broadcaster.sendTransform(transform) - except roscomp.exceptions.ROSException: - if roscomp.ok(): - self.node.logwarn("Sensor {} failed to send transform.".format(self.uid)) - - def listen(self): - self.carla_actor.listen(self._callback_sensor_data) - - def destroy(self): - """ - Function (override) to destroy this object. - - Stop listening to the carla.Sensor actor. - Finally forward call to super class. - - :return: - """ - self._callback_active.acquire() - if self.carla_actor.is_listening: - self.carla_actor.stop() - super(Sensor, self).destroy() - - def _callback_sensor_data(self, carla_sensor_data): - """ - Callback function called whenever new sensor data is received - - :param carla_sensor_data: carla sensor data object - :type carla_sensor_data: carla.SensorData - """ - if not self._callback_active.acquire(False): - # if acquire fails, sensor is currently getting destroyed - return - if self.synchronous_mode: - if self.sensor_tick_time: - self.next_data_expected_time = carla_sensor_data.timestamp + \ - float(self.sensor_tick_time) - self.queue.put(carla_sensor_data) - else: - self.publish_tf(trans.carla_transform_to_ros_pose( - carla_sensor_data.transform), carla_sensor_data.timestamp) - try: - self.sensor_data_updated(carla_sensor_data) - except roscomp.exceptions.ROSException: - if roscomp.ok(): - self.node.logwarn( - "Sensor {}: Error while executing sensor_data_updated().".format(self.uid)) - self._callback_active.release() - - @abstractmethod - def sensor_data_updated(self, carla_sensor_data): - """ - Pure-virtual function to transform the received carla sensor data - into a corresponding ROS message - - :param carla_sensor_data: carla sensor data object - :type carla_sensor_data: carla.SensorData - """ - raise NotImplementedError( - "This function has to be implemented by the derived classes") - - def _update_synchronous_event_sensor(self, frame, timestamp): - while True: - try: - carla_sensor_data = self.queue.get(block=False) - if carla_sensor_data.frame != frame: - self.node.logwarn("{}({}): Received event for frame {}" - " (expected {}). Process it anyways.".format( - self.__class__.__name__, self.get_id(), - carla_sensor_data.frame, frame)) - self.node.logdebug("{}({}): process {}".format( - self.__class__.__name__, self.get_id(), frame)) - self.publish_tf(trans.carla_transform_to_ros_pose( - carla_sensor_data.transform), timestamp) - self.sensor_data_updated(carla_sensor_data) - except queue.Empty: - return - - def _update_synchronous_sensor(self, frame, timestamp): - while not self.next_data_expected_time or \ - (not self.queue.empty() or - self.next_data_expected_time and - self.next_data_expected_time < timestamp): - while True: - try: - carla_sensor_data = self.queue.get(timeout=1.0) - if carla_sensor_data.frame == frame: - self.node.logdebug("{}({}): process {}".format(self.__class__.__name__, - self.get_id(), frame)) - self.publish_tf(trans.carla_transform_to_ros_pose( - carla_sensor_data.transform), timestamp) - self.sensor_data_updated(carla_sensor_data) - return - elif carla_sensor_data.frame < frame: - self.node.logwarn("{}({}): skipping old frame {}, expected {}".format( - self.__class__.__name__, - self.get_id(), - carla_sensor_data.frame, - frame)) - except queue.Empty: - if roscomp.ok(): - self.node.logwarn("{}({}): Expected Frame {} not received".format( - self.__class__.__name__, self.get_id(), frame)) - return - - def update(self, frame, timestamp): - if self.synchronous_mode: - if self.is_event_sensor: - self._update_synchronous_event_sensor(frame, timestamp) - else: - self._update_synchronous_sensor(frame, timestamp) - - super(Sensor, self).update(frame, timestamp) - - -# http://docs.ros.org/indigo/api/sensor_msgs/html/point__cloud2_8py_source.html - -def _get_struct_fmt(is_bigendian, fields, field_names=None): - fmt = '>' if is_bigendian else '<' - - offset = 0 - for field in (f for f in sorted(fields, key=lambda f: f.offset) - if field_names is None or f.name in field_names): - if offset < field.offset: - fmt += 'x' * (field.offset - offset) - offset = field.offset - if field.datatype not in _DATATYPES: - print('Skipping unknown PointField datatype [{}]' % field.datatype, file=sys.stderr) - else: - datatype_fmt, datatype_length = _DATATYPES[field.datatype] - fmt += field.count * datatype_fmt - offset += field.count * datatype_length - - return fmt - - -def create_cloud(header, fields, points): - """ - Create a L{sensor_msgs.msg.PointCloud2} message. - @param header: The point cloud header. - @type header: L{std_msgs.msg.Header} - @param fields: The point cloud fields. - @type fields: iterable of L{sensor_msgs.msg.PointField} - @param points: The point cloud points. - @type points: list of iterables, i.e. one iterable for each point, with the - elements of each iterable being the values of the fields for - that point (in the same order as the fields parameter) - @return: The point cloud. - @rtype: L{sensor_msgs.msg.PointCloud2} - """ - - cloud_struct = struct.Struct(_get_struct_fmt(False, fields)) - - buff = ctypes.create_string_buffer(cloud_struct.size * len(points)) - - point_step, pack_into = cloud_struct.size, cloud_struct.pack_into - offset = 0 - for p in points: - pack_into(buff, offset, *p) - offset += point_step - - return PointCloud2(header=header, - height=1, - width=len(points), - is_dense=False, - is_bigendian=False, - fields=fields, - point_step=cloud_struct.size, - row_step=cloud_struct.size * len(points), - data=buff.raw) diff --git a/carla_ros_bridge/src/carla_ros_bridge/spectator.py b/carla_ros_bridge/src/carla_ros_bridge/spectator.py deleted file mode 100644 index 731366b2..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/spectator.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla spectator -""" - -from carla_ros_bridge.actor import Actor - - -class Spectator(Actor): - - """ - Actor implementation details for spectators - """ - - def __init__(self, uid, name, parent, node, carla_actor): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - """ - super(Spectator, self).__init__(uid=uid, - name=name, - parent=parent, - node=node, - carla_actor=carla_actor) diff --git a/carla_ros_bridge/src/carla_ros_bridge/speedometer_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/speedometer_sensor.py deleted file mode 100644 index 5f92be99..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/speedometer_sensor.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -handle a speedometer sensor -""" - -import numpy as np - -from carla_ros_bridge.pseudo_actor import PseudoActor - -from std_msgs.msg import Float32 - - -class SpeedometerSensor(PseudoActor): - - """ - Pseudo speedometer sensor - """ - - def __init__(self, uid, name, parent, node): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying the sensor - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - """ - - super(SpeedometerSensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - - self.speedometer_publisher = node.new_publisher(Float32, - self.get_topic_prefix(), - qos_profile=10) - - def destroy(self): - super(SpeedometerSensor, self).destroy() - self.node.destroy_publisher(self.speedometer_publisher) - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - return "sensor.pseudo.speedometer" - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - """ - try: - velocity = self.parent.carla_actor.get_velocity() - transform = self.parent.carla_actor.get_transform() - except AttributeError: - # parent actor disappeared, do not send tf - self.node.logwarn( - "SpeedometerSensor could not publish. Parent actor {} not found".format(self.parent.uid)) - return - - vel_np = np.array([velocity.x, velocity.y, velocity.z]) - pitch = np.deg2rad(transform.rotation.pitch) - yaw = np.deg2rad(transform.rotation.yaw) - orientation = np.array([ - np.cos(pitch) * np.cos(yaw), - np.cos(pitch) * np.sin(yaw), - np.sin(pitch) - ]) - speed = np.dot(vel_np, orientation) - - self.speedometer_publisher.publish(Float32(data=speed)) diff --git a/carla_ros_bridge/src/carla_ros_bridge/tf_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/tf_sensor.py deleted file mode 100644 index 0dd9dcf3..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/tf_sensor.py +++ /dev/null @@ -1,81 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -handle a tf sensor -""" - -import os - -import tf2_ros - -import ros_compatibility as roscomp - -from carla_ros_bridge.pseudo_actor import PseudoActor - -from geometry_msgs.msg import TransformStamped - -ROS_VERSION = roscomp.get_ros_version() - - -class TFSensor(PseudoActor): - - """ - Pseudo tf sensor - """ - - def __init__(self, uid, name, parent, node): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying the sensor - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - """ - - super(TFSensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - - if ROS_VERSION == 1: - self._tf_broadcaster = tf2_ros.TransformBroadcaster() - elif ROS_VERSION == 2: - self._tf_broadcaster = tf2_ros.TransformBroadcaster(node) - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - return "sensor.pseudo.tf" - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - """ - self.parent.get_prefix() - - transform = None - try: - transform = self.parent.get_current_ros_transform() - except AttributeError: - # parent actor disappeared, do not send tf - self.node.logwarn( - "TFSensor could not publish transform. Actor {} not found".format(self.parent.uid)) - return - - self._tf_broadcaster.sendTransform(TransformStamped( - header=self.get_msg_header("map", timestamp=timestamp), - child_frame_id=self.parent.get_prefix(), - transform=transform)) diff --git a/carla_ros_bridge/src/carla_ros_bridge/traffic.py b/carla_ros_bridge/src/carla_ros_bridge/traffic.py deleted file mode 100644 index 462ba825..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/traffic.py +++ /dev/null @@ -1,108 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla traffic objects -""" - -from carla import TrafficLightState - -import carla_common.transforms as trans - -from carla_ros_bridge.actor import Actor - -from carla_msgs.msg import CarlaTrafficLightStatus, CarlaTrafficLightInfo - - -class Traffic(Actor): - - """ - Actor implementation details for traffic objects - """ - - def __init__(self, uid, name, parent, node, carla_actor): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - """ - super(Traffic, self).__init__(uid=uid, - name=name, - parent=parent, - node=node, - carla_actor=carla_actor) - - -class TrafficLight(Actor): - - """ - Traffic implementation details for traffic lights - """ - - def __init__(self, uid, name, parent, node, carla_actor): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.TrafficLight - """ - super(TrafficLight, self).__init__(uid=uid, - name=name, - parent=parent, - node=node, - carla_actor=carla_actor) - - def get_status(self): - """ - Get the current state of the traffic light - """ - status = CarlaTrafficLightStatus() - status.id = self.get_id() - carla_state = self.carla_actor.get_state() - if carla_state == TrafficLightState.Red: - status.state = CarlaTrafficLightStatus.RED - elif carla_state == TrafficLightState.Yellow: - status.state = CarlaTrafficLightStatus.YELLOW - elif carla_state == TrafficLightState.Green: - status.state = CarlaTrafficLightStatus.GREEN - elif carla_state == TrafficLightState.Off: - status.state = CarlaTrafficLightStatus.OFF - else: - status.state = CarlaTrafficLightStatus.UNKNOWN - return status - - def get_info(self): - """ - Get the info of the traffic light - """ - info = CarlaTrafficLightInfo() - info.id = self.get_id() - info.transform = self.get_current_ros_pose() - info.trigger_volume.center = trans.carla_location_to_ros_vector3( - self.carla_actor.trigger_volume.location) - info.trigger_volume.size.x = self.carla_actor.trigger_volume.extent.x * 2.0 - info.trigger_volume.size.y = self.carla_actor.trigger_volume.extent.y * 2.0 - info.trigger_volume.size.z = self.carla_actor.trigger_volume.extent.z * 2.0 - return info diff --git a/carla_ros_bridge/src/carla_ros_bridge/traffic_lights_sensor.py b/carla_ros_bridge/src/carla_ros_bridge/traffic_lights_sensor.py deleted file mode 100644 index 0635229c..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/traffic_lights_sensor.py +++ /dev/null @@ -1,100 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -a sensor that reports the state of all traffic lights -""" - -from ros_compatibility.qos import QoSProfile, DurabilityPolicy - -from carla_ros_bridge.pseudo_actor import PseudoActor -from carla_ros_bridge.traffic import TrafficLight - -from carla_msgs.msg import ( - CarlaTrafficLightStatusList, - CarlaTrafficLightInfoList -) - - -class TrafficLightsSensor(PseudoActor): - """ - a sensor that reports the state of all traffic lights - """ - - def __init__(self, uid, name, parent, node, actor_list): - """ - Constructor - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying the sensor - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param actor_list: current list of actors - :type actor_list: map(carla-actor-id -> python-actor-object) - """ - - super(TrafficLightsSensor, self).__init__(uid=uid, - name=name, - parent=parent, - node=node) - - self.actor_list = actor_list - self.traffic_light_status = CarlaTrafficLightStatusList() - self.traffic_light_actors = [] - - self.traffic_lights_info_publisher = node.new_publisher( - CarlaTrafficLightInfoList, - self.get_topic_prefix() + "/info", - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self.traffic_lights_status_publisher = node.new_publisher( - CarlaTrafficLightStatusList, - self.get_topic_prefix() + "/status", - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - - def destroy(self): - """ - Function to destroy this object. - :return: - """ - super(TrafficLightsSensor, self).destroy() - self.actor_list = None - self.node.destroy_publisher(self.traffic_lights_info_publisher) - self.node.destroy_publisher(self.traffic_lights_status_publisher) - - @staticmethod - def get_blueprint_name(): - """ - Get the blueprint identifier for the pseudo sensor - :return: name - """ - return "sensor.pseudo.traffic_lights" - - def update(self, frame, timestamp): - """ - Get the state of all known traffic lights - """ - traffic_light_status = CarlaTrafficLightStatusList() - traffic_light_actors = [] - for actor_id in self.actor_list: - actor = self.actor_list[actor_id] - if isinstance(actor, TrafficLight): - traffic_light_actors.append(actor) - traffic_light_status.traffic_lights.append(actor.get_status()) - - if traffic_light_actors != self.traffic_light_actors: - self.traffic_light_actors = traffic_light_actors - traffic_light_info_list = CarlaTrafficLightInfoList() - for traffic_light in traffic_light_actors: - traffic_light_info_list.traffic_lights.append(traffic_light.get_info()) - self.traffic_lights_info_publisher.publish(traffic_light_info_list) - - if traffic_light_status != self.traffic_light_status: - self.traffic_light_status = traffic_light_status - self.traffic_lights_status_publisher.publish(traffic_light_status) \ No newline at end of file diff --git a/carla_ros_bridge/src/carla_ros_bridge/traffic_participant.py b/carla_ros_bridge/src/carla_ros_bridge/traffic_participant.py deleted file mode 100644 index 43935102..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/traffic_participant.py +++ /dev/null @@ -1,143 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla traffic participants -""" - -import carla_common.transforms as trans - -from carla_ros_bridge.actor import Actor - -from derived_object_msgs.msg import Object -from shape_msgs.msg import SolidPrimitive -from std_msgs.msg import ColorRGBA -from visualization_msgs.msg import Marker - - -class TrafficParticipant(Actor): - - """ - actor implementation details for traffic participant - """ - - def __init__(self, uid, name, parent, node, carla_actor): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla actor object - :type carla_actor: carla.Actor - """ - self.classification_age = 0 - super(TrafficParticipant, self).__init__(uid=uid, - name=name, - parent=parent, - node=node, - carla_actor=carla_actor) - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - - On update vehicles send: - - tf global frame - - object message - - marker message - - :return: - """ - self.classification_age += 1 - super(TrafficParticipant, self).update(frame, timestamp) - - def get_object_info(self): - """ - Function to send object messages of this traffic participant. - - A derived_object_msgs.msg.Object is prepared to be published via '/carla/objects' - - :return: - """ - obj = Object(header=self.get_msg_header("map")) - # ID - obj.id = self.get_id() - # Pose - obj.pose = self.get_current_ros_pose() - # Twist - obj.twist = self.get_current_ros_twist() - # Acceleration - obj.accel = self.get_current_ros_accel() - # Shape - obj.shape.type = SolidPrimitive.BOX - obj.shape.dimensions.extend([ - self.carla_actor.bounding_box.extent.x * 2.0, - self.carla_actor.bounding_box.extent.y * 2.0, - self.carla_actor.bounding_box.extent.z * 2.0]) - - # Classification if available in attributes - if self.get_classification() != Object.CLASSIFICATION_UNKNOWN: - obj.object_classified = True - obj.classification = self.get_classification() - obj.classification_certainty = 255 - obj.classification_age = self.classification_age - - return obj - - def get_classification(self): # pylint: disable=no-self-use - """ - Function to get object classification (overridden in subclasses) - """ - return Object.CLASSIFICATION_UNKNOWN - - def get_marker_color(self): # pylint: disable=no-self-use - """ - Function (override) to return the color for marker messages. - - :return: default color used by traffic participants - :rtpye : std_msgs.msg.ColorRGBA - """ - color = ColorRGBA() - color.r = 0. - color.g = 0. - color.b = 255. - return color - - def get_marker_pose(self): - """ - Function to return the pose for traffic participants. - - :return: the pose of the traffic participant. - :rtype: geometry_msgs.msg.Pose - """ - return trans.carla_transform_to_ros_pose(self.carla_actor.get_transform()) - - def get_marker(self, timestamp=None): - """ - Helper function to create a ROS visualization_msgs.msg.Marker for the actor - - :return: - visualization_msgs.msg.Marker - """ - marker = Marker(header=self.get_msg_header(frame_id="map", timestamp=timestamp)) - marker.color = self.get_marker_color() - marker.color.a = 0.3 - marker.id = self.get_id() - marker.type = Marker.CUBE - - marker.pose = self.get_marker_pose() - marker.scale.x = self.carla_actor.bounding_box.extent.x * 2.0 - marker.scale.y = self.carla_actor.bounding_box.extent.y * 2.0 - marker.scale.z = self.carla_actor.bounding_box.extent.z * 2.0 - return marker diff --git a/carla_ros_bridge/src/carla_ros_bridge/vehicle.py b/carla_ros_bridge/src/carla_ros_bridge/vehicle.py deleted file mode 100644 index 0ac73127..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/vehicle.py +++ /dev/null @@ -1,92 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla vehicles -""" - -import carla_common.transforms as trans - -from carla_ros_bridge.traffic_participant import TrafficParticipant - -from derived_object_msgs.msg import Object -from std_msgs.msg import ColorRGBA - - -class Vehicle(TrafficParticipant): - - """ - Actor implementation details for vehicles - """ - - def __init__(self, uid, name, parent, node, carla_actor): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: carla_ros_bridge.CarlaRosBridge - :param carla_actor: carla vehicle actor object - :type carla_actor: carla.Vehicle - """ - self.classification = Object.CLASSIFICATION_CAR - if 'object_type' in carla_actor.attributes: - if carla_actor.attributes['object_type'] == 'car': - self.classification = Object.CLASSIFICATION_CAR - elif carla_actor.attributes['object_type'] == 'bike': - self.classification = Object.CLASSIFICATION_BIKE - elif carla_actor.attributes['object_type'] == 'motorcycle': - self.classification = Object.CLASSIFICATION_MOTORCYCLE - elif carla_actor.attributes['object_type'] == 'truck': - self.classification = Object.CLASSIFICATION_TRUCK - elif carla_actor.attributes['object_type'] == 'other': - self.classification = Object.CLASSIFICATION_OTHER_VEHICLE - - super(Vehicle, self).__init__(uid=uid, - name=name, - parent=parent, - node=node, - carla_actor=carla_actor) - - def get_marker_color(self): # pylint: disable=no-self-use - """ - Function (override) to return the color for marker messages. - - :return: the color used by a vehicle marker - :rtpye : std_msgs.msg.ColorRGBA - """ - color = ColorRGBA() - color.r = 255.0 - color.g = 0.0 - color.b = 0.0 - return color - - def get_marker_pose(self): - """ - Function to return the pose for vehicles. - - :return: the pose of the vehicle - :rtype: geometry_msgs.msg.Pose - """ - # Moving pivot point from the bottom (CARLA) to the center (ROS) of the bounding box. - extent = self.carla_actor.bounding_box.extent - marker_transform = self.carla_actor.get_transform() - marker_transform.location += marker_transform.get_up_vector() * extent.z - return trans.carla_transform_to_ros_pose(marker_transform) - - def get_classification(self): - """ - Function (override) to get classification - :return: - """ - return self.classification diff --git a/carla_ros_bridge/src/carla_ros_bridge/walker.py b/carla_ros_bridge/src/carla_ros_bridge/walker.py deleted file mode 100644 index 41f828d2..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/walker.py +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Classes to handle Carla pedestrians -""" - -from carla import WalkerControl - -from carla_ros_bridge.traffic_participant import TrafficParticipant - -from carla_msgs.msg import CarlaWalkerControl -from derived_object_msgs.msg import Object - - -class Walker(TrafficParticipant): - - """ - Actor implementation details for pedestrians - """ - - def __init__(self, uid, name, parent, node, carla_actor): - """ - Constructor - - :param uid: unique identifier for this object - :type uid: int - :param name: name identiying this object - :type name: string - :param parent: the parent of this - :type parent: carla_ros_bridge.Parent - :param node: node-handle - :type node: CompatibleNode - :param carla_actor: carla walker actor object - :type carla_actor: carla.Walker - """ - super(Walker, self).__init__(uid=uid, - name=name, - parent=parent, - node=node, - carla_actor=carla_actor) - - self.control_subscriber = self.node.new_subscription( - CarlaWalkerControl, - self.get_topic_prefix() + "/walker_control_cmd", - self.control_command_updated, - qos_profile=10) - - def destroy(self): - """ - Function (override) to destroy this object. - - Terminate ROS subscriptions - Finally forward call to super class. - - :return: - """ - super(Walker, self).destroy() - self.node.destroy_subscription(self.control_subscriber) - - def control_command_updated(self, ros_walker_control): - """ - Receive a CarlaWalkerControl msg and send to CARLA - This function gets called whenever a ROS message is received via - '/carla//walker_control_cmd' topic. - The received ROS message is converted into carla.WalkerControl command and - sent to CARLA. - :param ros_walker_control: current walker control input received via ROS - :type self.info.output: carla_ros_bridge.msg.CarlaWalkerControl - :return: - """ - walker_control = WalkerControl() - walker_control.direction.x = ros_walker_control.direction.x - walker_control.direction.y = -ros_walker_control.direction.y - walker_control.direction.z = ros_walker_control.direction.z - walker_control.speed = ros_walker_control.speed - walker_control.jump = ros_walker_control.jump - self.carla_actor.apply_control(walker_control) - - def get_classification(self): - """ - Function (override) to get classification - :return: - """ - return Object.CLASSIFICATION_PEDESTRIAN diff --git a/carla_ros_bridge/src/carla_ros_bridge/world_info.py b/carla_ros_bridge/src/carla_ros_bridge/world_info.py deleted file mode 100755 index c41d3ef7..00000000 --- a/carla_ros_bridge/src/carla_ros_bridge/world_info.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/env python - -# -# Copyright (c) 2018-2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -""" -Class to handle the carla map -""" - -from ros_compatibility.qos import QoSProfile, DurabilityPolicy - -from carla_msgs.msg import CarlaWorldInfo - - -class WorldInfo(object): - - """ - Publish the map - """ - - def __init__(self, carla_world, node): - """ - Constructor - - :param carla_world: carla world object - :type carla_world: carla.World - :param node: node-handle - :type node: CompatibleNode - """ - self.node = node - self.carla_map = carla_world.get_map() - - self.map_published = False - - self.world_info_publisher = node.new_publisher( - CarlaWorldInfo, - "/carla/world_info", - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - - def destroy(self): - """ - Function (override) to destroy this object. - - Remove reference to carla.Map object. - Finally forward call to super class. - - :return: - """ - self.node.destroy_publisher(self.world_info_publisher) - self.carla_map = None - - def update(self, frame, timestamp): - """ - Function (override) to update this object. - - :return: - """ - if not self.map_published: - open_drive_msg = CarlaWorldInfo() - open_drive_msg.map_name = self.carla_map.name - open_drive_msg.opendrive = self.carla_map.to_opendrive() - self.world_info_publisher.publish(open_drive_msg) - self.map_published = True diff --git a/carla_ros_bridge/test/ros_bridge_client.py b/carla_ros_bridge/test/ros_bridge_client.py deleted file mode 100755 index 65cd4ecb..00000000 --- a/carla_ros_bridge/test/ros_bridge_client.py +++ /dev/null @@ -1,282 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2019 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# - -""" -Class for testing nodes -""" -# pylint: disable=no-member - -import unittest -import rospy -import rostest -from std_msgs.msg import Header, String -from rosgraph_msgs.msg import Clock -from sensor_msgs.msg import CameraInfo, NavSatFix, Image, PointCloud2, Imu -from geometry_msgs.msg import Quaternion, Vector3, Pose -from nav_msgs.msg import Odometry -from derived_object_msgs.msg import ObjectArray -from visualization_msgs.msg import Marker, MarkerArray -from carla_msgs.msg import (CarlaEgoVehicleStatus, CarlaEgoVehicleInfo, CarlaWorldInfo, - CarlaActorList, CarlaTrafficLightStatusList, - CarlaTrafficLightInfoList) - -PKG = 'test_roslaunch' -TIMEOUT = 20 - - -class TestClock(unittest.TestCase): - - """ - Handles testing of the all nodes - """ - - def test_clock(self): - """ - Tests clock - """ - rospy.init_node('test_node', anonymous=True) - clock_msg = rospy.wait_for_message("/clock", Clock, timeout=TIMEOUT) - self.assertNotEqual(Clock(), clock_msg) - - def test_vehicle_status(self): - """ - Tests vehicle_status - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/vehicle_status", CarlaEgoVehicleStatus, timeout=TIMEOUT) - self.assertNotEqual(msg.header, Header()) - self.assertEqual(msg.header.frame_id, 'map') - self.assertNotEqual(msg.orientation, Quaternion()) - - def test_vehicle_info(self): - """ - Tests vehicle_info - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/vehicle_info", CarlaEgoVehicleInfo, timeout=TIMEOUT) - self.assertNotEqual(msg.id, 0) - self.assertEqual(msg.type, "vehicle.tesla.model3") - self.assertEqual(msg.rolename, "ego_vehicle") - self.assertEqual(len(msg.wheels), 4) - self.assertNotEqual(msg.max_rpm, 0.0) - self.assertNotEqual(msg.moi, 0.0) - self.assertNotEqual(msg.damping_rate_full_throttle, 0.0) - self.assertNotEqual(msg.damping_rate_zero_throttle_clutch_engaged, 0.0) - self.assertNotEqual( - msg.damping_rate_zero_throttle_clutch_disengaged, 0.0) - self.assertTrue(msg.use_gear_autobox) - self.assertNotEqual(msg.gear_switch_time, 0.0) - self.assertNotEqual(msg.mass, 0.0) - self.assertNotEqual(msg.clutch_strength, 0.0) - self.assertNotEqual(msg.drag_coefficient, 0.0) - self.assertNotEqual(msg.center_of_mass, Vector3()) - - def test_odometry(self): - """ - Tests Odometry - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/odometry", Odometry, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "map") - self.assertEqual(msg.child_frame_id, "ego_vehicle") - self.assertNotEqual(msg.pose, Pose()) - - def test_gnss(self): - """ - Tests Gnss - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/gnss", NavSatFix, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/gnss") - self.assertNotEqual(msg.latitude, 0.0) - self.assertNotEqual(msg.longitude, 0.0) - self.assertNotEqual(msg.altitude, 0.0) - - def test_imu(self): - """ - Tests IMU sensor node - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message("/carla/ego_vehicle/imu", Imu, timeout=15) - self.assertEqual(msg.header.frame_id, "ego_vehicle/imu") - self.assertNotEqual(msg.linear_acceleration, 0.0) - self.assertNotEqual(msg.angular_velocity, 0.0) - self.assertNotEqual(msg.orientation, 0.0) - - def test_camera_info(self): - """ - Tests camera_info - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/rgb_front/camera_info", CameraInfo, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/rgb_front") - self.assertEqual(msg.height, 600) - self.assertEqual(msg.width, 800) - - def test_camera_image(self): - """ - Tests camera_images - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/rgb_front/image", Image, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/rgb_front") - self.assertEqual(msg.height, 600) - self.assertEqual(msg.width, 800) - self.assertEqual(msg.encoding, "bgra8") - - def test_dvs_camera_info(self): - """ - Tests dvs camera info - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/dvs_front/camera_info", CameraInfo, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front") - self.assertEqual(msg.height, 70) - self.assertEqual(msg.width, 400) - - def test_dvs_camera_image(self): - """ - Tests dvs camera images - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/dvs_front/image", Image, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front") - self.assertEqual(msg.height, 70) - self.assertEqual(msg.width, 400) - self.assertEqual(msg.encoding, "bgr8") - - def test_dvs_camera_events(self): - """ - Tests dvs camera events - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/dvs_front/events", PointCloud2, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front") - - def test_lidar(self): - """ - Tests Lidar sensor node - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/lidar", PointCloud2, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/lidar") - - def test_semantic_lidar(self): - """ - Tests semantic_lidar sensor node - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/semantic_lidar", PointCloud2, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/semantic_lidar") - - def test_radar(self): - """ - Tests Radar sensor node - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/radar_front", PointCloud2, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/radar_front") - - def test_ego_vehicle_objects(self): - """ - Tests objects node for ego_vehicle - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/ego_vehicle/objects", ObjectArray, timeout=15) - self.assertEqual(msg.header.frame_id, "map") - self.assertEqual(len(msg.objects), 0) - - def test_objects(self): - """ - Tests carla objects - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message("/carla/objects", ObjectArray, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "map") - self.assertEqual(len(msg.objects), 1) # only ego vehicle exists - - def test_marker(self): - """ - Tests marker - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message("/carla/markers", MarkerArray, timeout=TIMEOUT) - self.assertEqual(len(msg.markers), 1) # only ego vehicle exists - - ego_marker = msg.markers[0] - self.assertEqual(ego_marker.header.frame_id, "map") - self.assertNotEqual(ego_marker.id, 0) - self.assertEqual(ego_marker.type, 1) - self.assertNotEqual(ego_marker.pose, Pose()) - self.assertNotEqual(ego_marker.scale, Vector3()) - self.assertEqual(ego_marker.color.r, 0.0) - self.assertEqual(ego_marker.color.g, 255.0) - self.assertEqual(ego_marker.color.b, 0.0) - - def test_map(self): - """ - Tests map - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/map", String, timeout=TIMEOUT) - self.assertNotEqual(len(msg.data), 0) - - def test_world_info(self): - """ - Tests world_info - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/world_info", CarlaWorldInfo, timeout=TIMEOUT) - self.assertNotEqual(len(msg.map_name), 0) - self.assertNotEqual(len(msg.opendrive), 0) - - def test_actor_list(self): - """ - Tests actor_list - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/actor_list", CarlaActorList, timeout=TIMEOUT) - self.assertNotEqual(len(msg.actors), 0) - - def test_traffic_lights(self): - """ - Tests traffic_lights - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/traffic_lights/status", CarlaTrafficLightStatusList, timeout=TIMEOUT) - self.assertNotEqual(len(msg.traffic_lights), 0) - - def test_traffic_lights_info(self): - """ - Tests traffic_lights - """ - rospy.init_node('test_node', anonymous=True) - msg = rospy.wait_for_message( - "/carla/traffic_lights/info", CarlaTrafficLightInfoList, timeout=TIMEOUT) - self.assertNotEqual(len(msg.traffic_lights), 0) - - -if __name__ == '__main__': - rostest.rosrun(PKG, 'tests', TestClock) diff --git a/carla_ros_bridge/test/ros_bridge_client.test b/carla_ros_bridge/test/ros_bridge_client.test deleted file mode 100644 index a4b3a71d..00000000 --- a/carla_ros_bridge/test/ros_bridge_client.test +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/carla_ros_bridge/test/ros_bridge_client_ros2_test.py b/carla_ros_bridge/test/ros_bridge_client_ros2_test.py deleted file mode 100644 index 0828db92..00000000 --- a/carla_ros_bridge/test/ros_bridge_client_ros2_test.py +++ /dev/null @@ -1,526 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# -import os -import unittest - -import launch -import launch.actions - -import launch_testing -import launch_testing.actions - -from ament_index_python.packages import get_package_share_directory - -import ros_compatibility as roscomp -from ros_compatibility.node import CompatibleNode -from ros_compatibility.qos import QoSProfile, DurabilityPolicy - -from std_msgs.msg import Header, String -from rosgraph_msgs.msg import Clock -from sensor_msgs.msg import CameraInfo, NavSatFix, Image, PointCloud2, Imu -from geometry_msgs.msg import Quaternion, Vector3, Pose -from nav_msgs.msg import Odometry -from derived_object_msgs.msg import ObjectArray -from visualization_msgs.msg import MarkerArray -from carla_msgs.msg import (CarlaEgoVehicleStatus, CarlaEgoVehicleInfo, CarlaWorldInfo, - CarlaActorList, CarlaTrafficLightStatusList, - CarlaTrafficLightInfoList) - - -def generate_test_description(): - ld = launch.LaunchDescription([ - launch.actions.DeclareLaunchArgument( - name='host', - default_value='localhost' - ), - launch.actions.DeclareLaunchArgument( - name='port', - default_value='2000' - ), - launch.actions.DeclareLaunchArgument( - name='timeout', - default_value='2' - ), - launch.actions.DeclareLaunchArgument( - name='passive', - default_value='False' - ), - launch.actions.DeclareLaunchArgument( - name='synchronous_mode', - default_value='False' - ), - launch.actions.DeclareLaunchArgument( - name='synchronous_mode_wait_for_vehicle_control_command', - default_value='True' - ), - launch.actions.DeclareLaunchArgument( - name='fixed_delta_seconds', - default_value='0.05' - ), - launch.actions.DeclareLaunchArgument( - name='role_name', - default_value='ego_vehicle' - ), - launch.actions.DeclareLaunchArgument( - name='vehicle_filter', - default_value='vehicle.tesla.model3' - ), - launch.actions.DeclareLaunchArgument( - name='ego_vehicle_role_names', - default_value=["hero", "ego_vehicle", "hero0", "hero1", "hero2", - "hero3", "hero4", "hero5", "hero6", "hero7", "hero8", "hero9"] - ), - launch.actions.DeclareLaunchArgument( - name='spawn_point', - default_value='None' - ), - launch.actions.DeclareLaunchArgument( - name='objects_definition_file', - default_value=get_package_share_directory( - 'carla_ros_bridge') + '/test/test_objects.json' - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'carla_ros_bridge'), 'carla_ros_bridge.launch.py') - ), - launch_arguments={ - 'host': launch.substitutions.LaunchConfiguration('host'), - 'port': launch.substitutions.LaunchConfiguration('port'), - 'timeout': launch.substitutions.LaunchConfiguration('timeout'), - 'passive': launch.substitutions.LaunchConfiguration('passive'), - 'synchronous_mode': launch.substitutions.LaunchConfiguration('synchronous_mode'), - 'synchronous_mode_wait_for_vehicle_control_command': launch.substitutions.LaunchConfiguration('synchronous_mode_wait_for_vehicle_control_command'), - 'fixed_delta_seconds': launch.substitutions.LaunchConfiguration('fixed_delta_seconds') - }.items() - ), - launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'carla_spawn_objects'), 'carla_spawn_objects.launch.py') - ), - launch_arguments={ - 'objects_definition_file': launch.substitutions.LaunchConfiguration('objects_definition_file') - }.items() - ), - # Start tests - launch_testing.actions.ReadyToTest() - ]) - return ld - - -TIMEOUT = 30 - - -class TestClock(unittest.TestCase): - - """ - Handles testing of the all nodes - """ - - def test_clock(self): - """ - Tests clock - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message("/clock", Clock, timeout=TIMEOUT) - self.assertNotEqual(Clock(), msg) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_vehicle_status(self, proc_output): - """ - Tests vehicle_status - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/vehicle_status", CarlaEgoVehicleStatus) - self.assertNotEqual(msg.header, Header()) - self.assertEqual(msg.header.frame_id, 'map') - self.assertNotEqual(msg.orientation, Quaternion()) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_vehicle_info(self): - """ - Tests vehicle_info - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/vehicle_info", CarlaEgoVehicleInfo, timeout=TIMEOUT, - qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self.assertNotEqual(msg.id, 0) - self.assertEqual(msg.type, "vehicle.tesla.model3") - self.assertEqual(msg.rolename, "ego_vehicle") - self.assertEqual(len(msg.wheels), 4) - self.assertNotEqual(msg.max_rpm, 0.0) - self.assertNotEqual(msg.moi, 0.0) - self.assertNotEqual(msg.damping_rate_full_throttle, 0.0) - self.assertNotEqual(msg.damping_rate_zero_throttle_clutch_engaged, 0.0) - self.assertNotEqual( - msg.damping_rate_zero_throttle_clutch_disengaged, 0.0) - self.assertTrue(msg.use_gear_autobox) - self.assertNotEqual(msg.gear_switch_time, 0.0) - self.assertNotEqual(msg.mass, 0.0) - self.assertNotEqual(msg.clutch_strength, 0.0) - self.assertNotEqual(msg.drag_coefficient, 0.0) - self.assertNotEqual(msg.center_of_mass, Vector3()) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_odometry(self): - """ - Tests Odometry - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/odometry", Odometry, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "map") - self.assertEqual(msg.child_frame_id, "ego_vehicle") - self.assertNotEqual(msg.pose, Pose()) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_gnss(self): - """ - Tests Gnss - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/gnss", NavSatFix, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/gnss") - self.assertNotEqual(msg.latitude, 0.0) - self.assertNotEqual(msg.longitude, 0.0) - self.assertNotEqual(msg.altitude, 0.0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_imu(self): - """ - Tests IMU sensor node - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message("/carla/ego_vehicle/imu", Imu, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/imu") - self.assertNotEqual(msg.linear_acceleration, 0.0) - self.assertNotEqual(msg.angular_velocity, 0.0) - self.assertNotEqual(msg.orientation, 0.0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_camera_info(self): - """ - Tests camera_info - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/rgb_front/camera_info", CameraInfo, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/rgb_front") - self.assertEqual(msg.height, 600) - self.assertEqual(msg.width, 800) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_camera_image(self): - """ - Tests camera_images - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/rgb_front/image", Image, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/rgb_front") - self.assertEqual(msg.height, 600) - self.assertEqual(msg.width, 800) - self.assertEqual(msg.encoding, "bgra8") - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_dvs_camera_info(self): - """ - Tests dvs camera info - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/dvs_front/camera_info", CameraInfo, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front") - self.assertEqual(msg.height, 70) - self.assertEqual(msg.width, 400) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_dvs_camera_image(self): - """ - Tests dvs camera images - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/dvs_front/image", Image, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front") - self.assertEqual(msg.height, 70) - self.assertEqual(msg.width, 400) - self.assertEqual(msg.encoding, "bgr8") - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_dvs_camera_events(self): - """ - Tests dvs camera events - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/dvs_front/events", PointCloud2, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/dvs_front") - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_lidar(self): - """ - Tests Lidar sensor node - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/lidar", PointCloud2, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/lidar") - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_semantic_lidar(self): - """ - Tests semantic_lidar sensor node - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/semantic_lidar", PointCloud2, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/semantic_lidar") - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_radar(self): - """ - Tests Radar sensor node - """ - try: - node = None - roscomp.init("test_node") - node = None - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/radar_front", PointCloud2, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "ego_vehicle/radar_front") - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_ego_vehicle_objects(self): - """ - Tests objects node for ego_vehicle - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/ego_vehicle/objects", ObjectArray, timeout=15) - self.assertEqual(msg.header.frame_id, "map") - self.assertEqual(len(msg.objects), 0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_objects(self): - """ - Tests carla objects - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message("/carla/objects", ObjectArray, timeout=TIMEOUT) - self.assertEqual(msg.header.frame_id, "map") - self.assertEqual(len(msg.objects), 1) # only ego vehicle exists - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_marker(self): - """ - Tests marker - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message("/carla/markers", MarkerArray, timeout=TIMEOUT) - self.assertEqual(len(msg.markers), 1) # only ego vehicle exists - - ego_marker = msg.markers[0] - self.assertEqual(ego_marker.header.frame_id, "map") - self.assertNotEqual(ego_marker.id, 0) - self.assertEqual(ego_marker.type, 1) - self.assertNotEqual(ego_marker.pose, Pose()) - self.assertNotEqual(ego_marker.scale, Vector3()) - self.assertEqual(ego_marker.color.r, 0.0) - self.assertEqual(ego_marker.color.g, 255.0) - self.assertEqual(ego_marker.color.b, 0.0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_map(self): - """ - Tests map - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/map", String, timeout=TIMEOUT, - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self.assertNotEqual(len(msg.data), 0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_world_info(self): - """ - Tests world_info - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/world_info", CarlaWorldInfo, timeout=TIMEOUT, - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self.assertNotEqual(len(msg.map_name), 0) - self.assertNotEqual(len(msg.opendrive), 0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_actor_list(self): - """ - Tests actor_list - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/actor_list", CarlaActorList, timeout=TIMEOUT) - self.assertNotEqual(len(msg.actors), 0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_traffic_lights(self): - """ - Tests traffic_lights - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/traffic_lights/status", CarlaTrafficLightStatusList, timeout=TIMEOUT, - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self.assertNotEqual(len(msg.traffic_lights), 0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() - - def test_traffic_lights_info(self): - """ - Tests traffic_lights - """ - try: - node = None - roscomp.init("test_node") - node = CompatibleNode('test_node') - msg = node.wait_for_message( - "/carla/traffic_lights/info", CarlaTrafficLightInfoList, timeout=TIMEOUT, - qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self.assertNotEqual(len(msg.traffic_lights), 0) - finally: - if node is not None: - node.destroy_node() - roscomp.shutdown() diff --git a/carla_ros_bridge/test/settings.yaml b/carla_ros_bridge/test/settings.yaml deleted file mode 100644 index 80a2b9d2..00000000 --- a/carla_ros_bridge/test/settings.yaml +++ /dev/null @@ -1,21 +0,0 @@ -ego_vehicle_node: - ros__parameters: - use_sim_time: true - carla: - # the network connection for the python connection to CARLA - host: localhost - port: 2000 - # vehicles - vehicle_filter: vehicle.* - # enable/disable synchronous mode. If enabled ros-bridge waits until - # expected data is received for all sensors - synchronous_mode: true - synchronous_mode_wait_for_vehicle_control_command: false - # set the fixed timestep length - fixed_delta_seconds: 0.05 - # configuration values for the ego vehicle - ego_vehicle: - # the role name of the vehicles that acts as ego vehicle for this ros bridge instance - # Only the vehicles within this list are controllable from within ROS. - # (the vehicle from CARLA is selected which has the attribute 'role_name' set to this value) - role_name: ["hero", "ego_vehicle"] diff --git a/carla_ros_bridge/test/test_objects.json b/carla_ros_bridge/test/test_objects.json deleted file mode 100644 index b426c5da..00000000 --- a/carla_ros_bridge/test/test_objects.json +++ /dev/null @@ -1,248 +0,0 @@ -{ - "objects": - [ - { - "type": "sensor.pseudo.traffic_lights", - "id": "traffic_lights" - }, - { - "type": "sensor.pseudo.objects", - "id": "objects" - }, - { - "type": "sensor.pseudo.actor_list", - "id": "actor_list" - }, - { - "type": "sensor.pseudo.markers", - "id": "markers" - }, - { - "type": "sensor.pseudo.opendrive_map", - "id": "map" - }, - { - "type": "vehicle.tesla.model3", - "id": "ego_vehicle", - "sensors": - [ - { - "type": "sensor.camera.rgb", - "id": "rgb_front", - "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "image_size_x": 800, - "image_size_y": 600, - "fov": 90.0, - "sensor_tick": 0.05, - "gamma": 2.2, - "shutter_speed": 200.0, - "iso": 100.0, - "fstop": 8.0, - "min_fstop": 1.2, - "blade_count": 5, - "exposure_mode": "histogram", - "exposure_compensation": 0.0, - "exposure_min_bright": 7.0, - "exposure_max_bright": 9.0, - "exposure_speed_up": 3.0, - "exposure_speed_down": 1.0, - "calibration_constant": 16.0, - "focal_distance": 1000.0, - "blur_amount": 1.0, - "blur_radius": 0.0, - "motion_blur_intensity": 0.45, - "motion_blur_max_distortion": 0.35, - "motion_blur_min_object_screen_size": 0.1, - "slope": 0.88, - "toe": 0.55, - "shoulder": 0.26, - "black_clip": 0.0, - "white_clip": 0.04, - "temp": 6500.0, - "tint": 0.0, - "chromatic_aberration_intensity": 0.0, - "chromatic_aberration_offset": 0.0, - "enable_postprocess_effects": "True", - "lens_circle_falloff": 5.0, - "lens_circle_multiplier": 0.0, - "lens_k": -1.0, - "lens_kcube": 0.0, - "lens_x_size": 0.08, - "lens_y_size": 0.08, - "bloom_intensity": 0.675, - "lens_flare_intensity": 0.1 - }, - { - "type": "sensor.camera.rgb", - "id": "rgb_view", - "spawn_point": {"x": -4.5, "y": 0.0, "z": 2.8, "roll": 0.0, "pitch": 20.0, "yaw": 0.0}, - "image_size_x": 800, - "image_size_y": 600, - "fov": 90.0, - "sensor_tick": 0.05, - "gamma": 2.2, - "shutter_speed": 200.0, - "iso": 100.0, - "fstop": 8.0, - "min_fstop": 1.2, - "blade_count": 5, - "exposure_mode": "histogram", - "exposure_compensation": 0.0, - "exposure_min_bright": 7.0, - "exposure_max_bright": 9.0, - "exposure_speed_up": 3.0, - "exposure_speed_down": 1.0, - "calibration_constant": 16.0, - "focal_distance": 1000.0, - "blur_amount": 1.0, - "blur_radius": 0.0, - "motion_blur_intensity": 0.45, - "motion_blur_max_distortion": 0.35, - "motion_blur_min_object_screen_size": 0.1, - "slope": 0.88, - "toe": 0.55, - "shoulder": 0.26, - "black_clip": 0.0, - "white_clip": 0.04, - "temp": 6500.0, - "tint": 0.0, - "chromatic_aberration_intensity": 0.0, - "chromatic_aberration_offset": 0.0, - "enable_postprocess_effects": "True", - "lens_circle_falloff": 5.0, - "lens_circle_multiplier": 0.0, - "lens_k": -1.0, - "lens_kcube": 0.0, - "lens_x_size": 0.08, - "lens_y_size": 0.08, - "bloom_intensity": 0.675, - "lens_flare_intensity": 0.1 - }, - { - "type": "sensor.lidar.ray_cast", - "id": "lidar", - "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "range": 50, - "channels": 32, - "points_per_second": 320000, - "upper_fov": 2.0, - "lower_fov": -26.8, - "rotation_frequency": 20, - "sensor_tick": 0.05, - "noise_stddev": 0.0 - }, - { - "type": "sensor.lidar.ray_cast_semantic", - "id": "semantic_lidar", - "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "range": 50, - "channels": 32, - "points_per_second": 320000, - "upper_fov": 2.0, - "lower_fov": -26.8, - "rotation_frequency": 20, - "sensor_tick": 0.05 - }, - { - "type": "sensor.other.radar", - "id": "radar_front", - "spawn_point": {"x": 2.0, "y": 0.0, "z": 1.5, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "horizontal_fov": 30.0, - "vertical_fov": 10.0, - "points_per_second": 1500, - "range": 100.0 - }, - { - "type": "sensor.camera.semantic_segmentation", - "id": "semantic_segmentation_front", - "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "fov": 90.0, - "image_size_x": 400, - "image_size_y": 70, - "sensor_tick": 0.05, - "lens_circle_falloff": 5.0, - "lens_circle_multiplier": 0.0, - "lens_k": -1.0, - "lens_kcube": 0.0, - "lens_x_size": 0.08, - "lens_y_size": 0.08 - }, - { - "type": "sensor.camera.depth", - "id": "depth_front", - "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "fov": 90.0, - "image_size_x": 400, - "image_size_y": 70, - "sensor_tick": 0.05, - "lens_circle_falloff": 5.0, - "lens_circle_multiplier": 0.0, - "lens_k": -1.0, - "lens_kcube": 0.0, - "lens_x_size": 0.08, - "lens_y_size": 0.08 - }, - { - "type": "sensor.camera.dvs", - "id": "dvs_front", - "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "fov": 90.0, - "image_size_x": 400, - "image_size_y": 70, - "sensor_tick": 0.05, - "positive_threshold": 0.3, - "negative_threshold": 0.3, - "sigma_positive_threshold": 0.0, - "sigma_negative_threshold": 0.0, - "use_log": true, - "log_eps": 0.001 - }, - { - "type": "sensor.other.gnss", - "id": "gnss", - "spawn_point": {"x": 1.0, "y": 0.0, "z": 2.0}, - "noise_alt_stddev": 0.0, "noise_lat_stddev": 0.0, "noise_lon_stddev": 0.0, - "noise_alt_bias": 0.0, "noise_lat_bias": 0.0, "noise_lon_bias": 0.0 - }, - { - "type": "sensor.other.imu", - "id": "imu", - "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "noise_accel_stddev_x": 0.0, "noise_accel_stddev_y": 0.0, "noise_accel_stddev_z": 0.0, - "noise_gyro_stddev_x": 0.0, "noise_gyro_stddev_y": 0.0, "noise_gyro_stddev_z": 0.0, - "noise_gyro_bias_x": 0.0, "noise_gyro_bias_y": 0.0, "noise_gyro_bias_z": 0.0 - }, - { - "type": "sensor.other.collision", - "id": "collision", - "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0} - }, - { - "type": "sensor.other.lane_invasion", - "id": "lane_invasion", - "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0} - }, - { - "type": "sensor.pseudo.tf", - "id": "tf" - }, - { - "type": "sensor.pseudo.objects", - "id": "objects" - }, - { - "type": "sensor.pseudo.odom", - "id": "odometry" - }, - { - "type": "sensor.pseudo.speedometer", - "id": "speedometer" - }, - { - "type": "actor.pseudo.control", - "id": "control" - } - ] - } - ] -} diff --git a/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch.py b/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch.py index 21355532..948e711d 100644 --- a/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch.py +++ b/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch.py @@ -3,6 +3,7 @@ def generate_launch_description(): + ld = launch.LaunchDescription([ launch.actions.DeclareLaunchArgument( name='host', @@ -23,6 +24,10 @@ def generate_launch_description(): name='wait_for_ego', default_value='True' ), + launch.actions.DeclareLaunchArgument( + name='debug', + default_value='False' + ), launch_ros.actions.Node( package='carla_ros_scenario_runner', executable='carla_ros_scenario_runner', @@ -45,6 +50,9 @@ def generate_launch_description(): }, { 'wait_for_ego': launch.substitutions.LaunchConfiguration('wait_for_ego') + }, + { + 'debug': launch.substitutions.LaunchConfiguration('debug') } ] ) diff --git a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/carla_ros_scenario_runner_node.py b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/carla_ros_scenario_runner_node.py index ef8796e6..2253e332 100755 --- a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/carla_ros_scenario_runner_node.py +++ b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/carla_ros_scenario_runner_node.py @@ -64,6 +64,7 @@ def __init__(self): wait_for_ego = self.get_param("wait_for_ego", "True") host = self.get_param("host", "localhost") port = self.get_param("port", 2000) + debug = self.get_param("debug", "False") self._status_publisher = self.new_publisher( CarlaScenarioRunnerStatus, @@ -75,6 +76,7 @@ def __init__(self): host, port, wait_for_ego, + debug, self.scenario_runner_status_updated, self.scenario_runner_log) self._request_queue = queue.Queue() diff --git a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/ros_vehicle_control.py b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/ros_vehicle_control.py index 1256882f..ad824bfd 100644 --- a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/ros_vehicle_control.py +++ b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/ros_vehicle_control.py @@ -51,15 +51,15 @@ def __init__(self, actor, args=None): self._target_speed_publisher = self.node.new_publisher( Float64, - "/carla/{}/target_speed".format(self._role_name), + "/carla/vehicles/{}/target_speed".format(self._role_name), QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self.node.loginfo("Publishing target_speed on /carla/{}/target_speed".format(self._role_name)) + self.node.loginfo("Publishing target_speed on /carla/vehicles/{}/target_speed".format(self._role_name)) self._path_publisher = self.node.new_publisher( Path, - "/carla/{}/{}".format(self._role_name, self._path_topic_name), + "/carla/vehicles/{}/{}".format(self._role_name, self._path_topic_name), QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)) - self.node.loginfo("Publishing path on /carla/{}/{}".format(self._role_name, self._path_topic_name)) + self.node.loginfo("Publishing path on /carla/vehicles/{}/{}".format(self._role_name, self._path_topic_name)) if "launch" in args and "launch-package" in args: diff --git a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/scenario_runner_runner.py b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/scenario_runner_runner.py index 3f969530..1b985307 100755 --- a/carla_ros_scenario_runner/src/carla_ros_scenario_runner/scenario_runner_runner.py +++ b/carla_ros_scenario_runner/src/carla_ros_scenario_runner/scenario_runner_runner.py @@ -19,11 +19,12 @@ class ScenarioRunnerRunner(ApplicationRunner): Executes scenario runner """ - def __init__(self, path, host, port, wait_for_ego, status_updated_fct, log_fct): # pylint: disable=too-many-arguments + def __init__(self, path, host, port, wait_for_ego, debug, status_updated_fct, log_fct): # pylint: disable=too-many-arguments self._path = path self._host = host self._port = port self._wait_for_ego = wait_for_ego + self._debug = debug super(ScenarioRunnerRunner, self).__init__( status_updated_fct, log_fct, @@ -44,4 +45,6 @@ def execute_scenario(self, scenario_file): "--port", str(self._port)] if self._wait_for_ego: cmdline.append("--waitForEgo") + if self._debug: + cmdline.append("--debug") return self.execute(cmdline, env=os.environ) diff --git a/carla_spawn_objects/config/objects.json b/carla_spawn_objects/config/objects.json index daba209d..f800f4f1 100644 --- a/carla_spawn_objects/config/objects.json +++ b/carla_spawn_objects/config/objects.json @@ -1,26 +1,6 @@ { "objects": [ - { - "type": "sensor.pseudo.traffic_lights", - "id": "traffic_lights" - }, - { - "type": "sensor.pseudo.objects", - "id": "objects" - }, - { - "type": "sensor.pseudo.actor_list", - "id": "actor_list" - }, - { - "type": "sensor.pseudo.markers", - "id": "markers" - }, - { - "type": "sensor.pseudo.opendrive_map", - "id": "map" - }, { "type": "vehicle.tesla.model3", "id": "ego_vehicle", @@ -41,13 +21,6 @@ "image_size_x": 800, "image_size_y": 600, "fov": 90.0, - "attached_objects": - [ - { - "type": "actor.pseudo.control", - "id": "control" - } - ] }, { "type": "sensor.lidar.ray_cast", @@ -135,26 +108,6 @@ "type": "sensor.other.lane_invasion", "id": "lane_invasion", "spawn_point": {"x": 0.0, "y": 0.0, "z": 0.0} - }, - { - "type": "sensor.pseudo.tf", - "id": "tf" - }, - { - "type": "sensor.pseudo.objects", - "id": "objects" - }, - { - "type": "sensor.pseudo.odom", - "id": "odometry" - }, - { - "type": "sensor.pseudo.speedometer", - "id": "speedometer" - }, - { - "type": "actor.pseudo.control", - "id": "control" } ] } diff --git a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch index 3029d963..9df0719c 100644 --- a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch +++ b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch @@ -9,8 +9,6 @@ - - @@ -20,6 +18,5 @@ - diff --git a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py index ee24a672..6e38b153 100644 --- a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py +++ b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py @@ -23,10 +23,6 @@ def generate_launch_description(): name='spawn_sensors_only', default_value='False' ), - launch.actions.DeclareLaunchArgument( - name='control_id', - default_value='control' - ), launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(get_package_share_directory( @@ -44,8 +40,7 @@ def generate_launch_description(): 'carla_spawn_objects'), 'set_initial_pose.launch.py') ), launch_arguments={ - 'role_name': launch.substitutions.LaunchConfiguration('role_name'), - 'control_id': launch.substitutions.LaunchConfiguration('control_id') + 'role_name': launch.substitutions.LaunchConfiguration('role_name') }.items() ) ]) diff --git a/carla_spawn_objects/launch/set_initial_pose.launch b/carla_spawn_objects/launch/set_initial_pose.launch index ba4647b6..5324a922 100644 --- a/carla_spawn_objects/launch/set_initial_pose.launch +++ b/carla_spawn_objects/launch/set_initial_pose.launch @@ -1,11 +1,8 @@ - - - diff --git a/carla_spawn_objects/launch/set_initial_pose.launch.py b/carla_spawn_objects/launch/set_initial_pose.launch.py index 811ddee2..5fdb4fc2 100644 --- a/carla_spawn_objects/launch/set_initial_pose.launch.py +++ b/carla_spawn_objects/launch/set_initial_pose.launch.py @@ -13,8 +13,8 @@ def generate_launch_description(): default_value='ego_vehicle' ), launch.actions.DeclareLaunchArgument( - name='control_id', - default_value='control' + name='objects_definition_file', + default_value='' ), launch_ros.actions.Node( package='carla_spawn_objects', @@ -24,10 +24,10 @@ def generate_launch_description(): emulate_tty=True, parameters=[ { - 'role_name': launch.substitutions.LaunchConfiguration('role_name') + 'objects_definition_file': launch.substitutions.LaunchConfiguration('objects_definition_file') }, { - 'control_id': launch.substitutions.LaunchConfiguration('control_id') + 'role_name': launch.substitutions.LaunchConfiguration('role_name') } ] ) diff --git a/carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py b/carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py index 0160d6a3..763d69d3 100755 --- a/carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py +++ b/carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py @@ -5,7 +5,7 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . """ -base class for spawning objects (carla actors and pseudo_actors) in ROS +base class for spawning objects (carla actors) in ROS Gets config file from ros parameter ~objects_definition_file and spawns corresponding objects through ROS service /carla/spawn_object. @@ -53,19 +53,25 @@ def __init__(self): self.vehicles_sensors = [] self.global_sensors = [] - self.spawn_object_service = self.new_client(SpawnObject, "/carla/spawn_object") - self.destroy_object_service = self.new_client(DestroyObject, "/carla/destroy_object") + self.spawn_object_service = self.new_client(SpawnObject, "/carla/world/spawn_object") + self.destroy_object_service = self.new_client(DestroyObject, "/carla/world/destroy_object") def spawn_object(self, spawn_object_request): + role_name = "" + for attribute in spawn_object_request.blueprint.attributes: + if attribute.key == "role_name": + role_name = attribute.value + break + self.logwarn("Requesting to spawn object (type='{}', id='{}').".format(spawn_object_request.blueprint.id, role_name)) response_id = -1 response = self.call_service(self.spawn_object_service, spawn_object_request, spin_until_response_received=True) response_id = response.id if response_id != -1: - self.loginfo("Object (type='{}', id='{}') spawned successfully as {}.".format( - spawn_object_request.type, spawn_object_request.id, response_id)) + self.loginfo("Object (id='{}', role_name='{}') spawned successfully as {}.".format( + spawn_object_request.blueprint.id, role_name, response_id)) else: - self.logwarn("Error while spawning object (type='{}', id='{}').".format( - spawn_object_request.type, spawn_object_request.id)) + self.logwarn("Error while spawning object (id='{}', role_name='{}').".format( + spawn_object_request.blueprint.id, role_name)) raise RuntimeError(response.error_string) return response_id @@ -86,13 +92,13 @@ def spawn_objects(self): global_sensors = [] vehicles = [] - found_sensor_actor_list = False for actor in json_actors["objects"]: - actor_type = actor["type"].split('.')[0] - if actor["type"] == "sensor.pseudo.actor_list" and self.spawn_sensors_only: - global_sensors.append(actor) - found_sensor_actor_list = True + actor_type_split = actor["type"].split('.') + actor_type = actor_type_split[0] + if actor_type_split[1] == "pseudo": + self.logwarn( + "Object with type {} is not a valid sensor anymore, ignoring".format(actor["type"])) elif actor_type == "sensor": global_sensors.append(actor) elif actor_type == "vehicle" or actor_type == "walker": @@ -100,15 +106,12 @@ def spawn_objects(self): else: self.logwarn( "Object with type {} is not a vehicle, a walker or a sensor, ignoring".format(actor["type"])) - if self.spawn_sensors_only is True and found_sensor_actor_list is False: - raise RuntimeError("Parameter 'spawn_sensors_only' enabled, " + - "but 'sensor.pseudo.actor_list' is not instantiated, add it to your config file.") self.setup_sensors(global_sensors) if self.spawn_sensors_only is True: - # get vehicle id from topic /carla/actor_list for all vehicles listed in config file - actor_info_list = self.wait_for_message("/carla/actor_list", CarlaActorList) + # get vehicle id from topic /carla/world/actor_list for all vehicles listed in config file + actor_info_list = self.wait_for_message("/carla/world/actor_list", CarlaActorList) for vehicle in vehicles: for actor_info in actor_info_list.actors: if actor_info.type == vehicle["type"] and actor_info.rolename == vehicle["id"]: @@ -131,8 +134,8 @@ def setup_vehicles(self, vehicles): self.setup_sensors(vehicle["sensors"], carla_id) else: spawn_object_request = roscomp.get_service_request(SpawnObject) - spawn_object_request.type = vehicle["type"] - spawn_object_request.id = vehicle["id"] + spawn_object_request.blueprint.id = vehicle["type"] + spawn_object_request.blueprint.attributes.append(KeyValue(key="role_name", value=vehicle["id"])) spawn_object_request.attach_to = 0 spawn_object_request.random_pose = False @@ -188,6 +191,7 @@ def setup_vehicles(self, vehicles): self.logwarn( "Object (type='{}', id='{}') has no 'sensors' field in his config file, none will be spawned.".format(spawn_object_request.type, spawn_object_request.id)) + def setup_sensors(self, sensors, attached_vehicle_id=None): """ Create the sensors defined by the user and attach them to the vehicle @@ -204,12 +208,17 @@ def setup_sensors(self, sensors, attached_vehicle_id=None): sensor_type = str(sensor_spec.pop("type")) sensor_id = str(sensor_spec.pop("id")) + if "pseudo" in sensor_type: + self.logwarn( + "Sensor of type {} is not a valid sensor anymore, ignoring".format(sensor_type)) + continue + sensor_name = sensor_type + "/" + sensor_id if sensor_name in sensor_names: raise NameError sensor_names.append(sensor_name) - if attached_vehicle_id is None and "pseudo" not in sensor_type: + if attached_vehicle_id is None: spawn_point = sensor_spec.pop("spawn_point") sensor_transform = self.create_spawn_point( spawn_point.pop("x"), @@ -219,7 +228,7 @@ def setup_sensors(self, sensors, attached_vehicle_id=None): spawn_point.pop("pitch", 0.0), spawn_point.pop("yaw", 0.0)) else: - # if sensor attached to a vehicle, or is a 'pseudo_actor', allow default pose + # if sensor attached to a vehicle allow default pose spawn_point = sensor_spec.pop("spawn_point", 0) if spawn_point == 0: sensor_transform = self.create_spawn_point(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) @@ -233,8 +242,8 @@ def setup_sensors(self, sensors, attached_vehicle_id=None): spawn_point.pop("yaw", 0.0)) spawn_object_request = roscomp.get_service_request(SpawnObject) - spawn_object_request.type = sensor_type - spawn_object_request.id = sensor_id + spawn_object_request.blueprint.id = sensor_type + spawn_object_request.blueprint.attributes.append(KeyValue(key="role_name", value=sensor_id)) spawn_object_request.attach_to = attached_vehicle_id if attached_vehicle_id is not None else 0 spawn_object_request.transform = sensor_transform spawn_object_request.random_pose = False # never set a random pose for a sensor @@ -245,7 +254,7 @@ def setup_sensors(self, sensors, attached_vehicle_id=None): for attached_object in sensor_spec["attached_objects"]: attached_objects.append(attached_object) continue - spawn_object_request.attributes.append( + spawn_object_request.blueprint.attributes.append( KeyValue(key=str(attribute), value=str(value))) response_id = self.spawn_object(spawn_object_request) @@ -316,7 +325,7 @@ def destroy(self): destroy_object_request = roscomp.get_service_request(DestroyObject) destroy_object_request.id = actor_id self.call_service(self.destroy_object_service, - destroy_object_request, timeout=0.5, spin_until_response_received=True) + destroy_object_request, timeout=0.5, spin_until_response_received=True, ignore_future_done=True) self.loginfo("Object {} successfully destroyed.".format(actor_id)) self.vehicles_sensors = [] @@ -325,7 +334,7 @@ def destroy(self): destroy_object_request = roscomp.get_service_request(DestroyObject) destroy_object_request.id = actor_id self.call_service(self.destroy_object_service, - destroy_object_request, timeout=0.5, spin_until_response_received=True) + destroy_object_request, timeout=0.5, spin_until_response_received=True, ignore_future_done=True) self.loginfo("Object {} successfully destroyed.".format(actor_id)) self.global_sensors = [] @@ -334,12 +343,13 @@ def destroy(self): destroy_object_request = roscomp.get_service_request(DestroyObject) destroy_object_request.id = player_id self.call_service(self.destroy_object_service, - destroy_object_request, timeout=0.5, spin_until_response_received=True) + destroy_object_request, timeout=0.5, spin_until_response_received=True, ignore_future_done=True) self.loginfo("Object {} successfully destroyed.".format(player_id)) self.players = [] - except ServiceException: + + except ServiceException as e: self.logwarn( - 'Could not call destroy service on objects, the ros bridge is probably already shutdown') + 'Could not call destroy service on objects, the ros bridge is probably already shutdown: {}'.format(e)) # ============================================================================== # -- main() -------------------------------------------------------------------- diff --git a/carla_spawn_objects/src/carla_spawn_objects/set_initial_pose.py b/carla_spawn_objects/src/carla_spawn_objects/set_initial_pose.py index c70981d7..8ecbce9c 100755 --- a/carla_spawn_objects/src/carla_spawn_objects/set_initial_pose.py +++ b/carla_spawn_objects/src/carla_spawn_objects/set_initial_pose.py @@ -17,25 +17,45 @@ /initialpose might be published via RVIZ '2D Pose Estimate" button. """ +import json +import os + import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from geometry_msgs.msg import PoseWithCovarianceStamped, Pose - class SetInitialPose(CompatibleNode): def __init__(self): super(SetInitialPose, self).__init__("set_initial_pose") - self.role_name = self.get_param("role_name", "ego_vehicle") - # control_id should correspond to the id of the actor.pseudo.control - # actor that is set in the config file used to spawn it - self.control_id = self.get_param("control_id", "control") + self.vehicle_role_names = [] + + # Read vehicle role_name from file if provided + self.objects_definition_file = self.get_param('objects_definition_file', '') + if self.objects_definition_file: + if not os.path.exists(self.objects_definition_file): + raise RuntimeError( + "Could not read object definitions from {}".format(self.objects_definition_file)) + with open(self.objects_definition_file) as handle: + json_actors = json.loads(handle.read()) + + for actor in json_actors["objects"]: + actor_type_split = actor["type"].split('.') + actor_type = actor_type_split[0] + if actor_type == "vehicle": + self.vehicle_role_names.append(actor["id"]) + + if len(self.vehicle_role_names) == 0: + self.vehicle_role_names = self.get_param("role_name", "ego_vehicle") + + if len(self.vehicle_role_names) > 1: + self.logwarn("Set initial pose only supports one single vehicle. Using {} from [{}]".format(self.vehicle_role_names[0], self.vehicle_role_names)) self.transform_publisher = self.new_publisher( Pose, - "/carla/{}/{}/set_transform".format(self.role_name, self.control_id), + "/carla/vehicles/{}/control/set_transform".format(self.vehicle_role_names[0]), qos_profile=10) self.initial_pose_subscriber = self.new_subscription( @@ -62,7 +82,8 @@ def main(): except KeyboardInterrupt: roscomp.loginfo("Cancelled by user.") finally: - roscomp.shutdown() + if roscomp.ok(): + roscomp.shutdown() if __name__ == '__main__': main() diff --git a/carla_twist_to_control/launch/carla_twist_to_control.launch b/carla_twist_to_control/launch/carla_twist_to_control.launch index d3694776..8a13e8ce 100644 --- a/carla_twist_to_control/launch/carla_twist_to_control.launch +++ b/carla_twist_to_control/launch/carla_twist_to_control.launch @@ -4,6 +4,7 @@ + diff --git a/carla_twist_to_control/launch/carla_twist_to_control.launch.py b/carla_twist_to_control/launch/carla_twist_to_control.launch.py index 3977a0b5..cf9d3a82 100644 --- a/carla_twist_to_control/launch/carla_twist_to_control.launch.py +++ b/carla_twist_to_control/launch/carla_twist_to_control.launch.py @@ -11,6 +11,10 @@ def generate_launch_description(): name='role_name', default_value='ego_vehicle' ), + launch.actions.DeclareLaunchArgument( + name='control_priority', + default_value='6' + ), launch_ros.actions.Node( package='carla_twist_to_control', executable='carla_twist_to_control', @@ -20,6 +24,7 @@ def generate_launch_description(): parameters=[ { 'role_name': launch.substitutions.LaunchConfiguration('role_name') + 'control_priority': launch.substitutions.LaunchConfiguration('control_priority') } ] ) diff --git a/carla_twist_to_control/src/carla_twist_to_control/carla_twist_to_control.py b/carla_twist_to_control/src/carla_twist_to_control/carla_twist_to_control.py index ab640ea8..d8529484 100755 --- a/carla_twist_to_control/src/carla_twist_to_control/carla_twist_to_control.py +++ b/carla_twist_to_control/src/carla_twist_to_control/carla_twist_to_control.py @@ -5,7 +5,7 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . """ -receive geometry_nav_msgs::Twist and publish carla_msgs::CarlaEgoVehicleControl +receive geometry_nav_msgs::Twist and publish carla_msgs::CarlaVehicleControl use max wheel steer angle """ @@ -17,13 +17,13 @@ from ros_compatibility.node import CompatibleNode from ros_compatibility.qos import QoSProfile, DurabilityPolicy -from carla_msgs.msg import CarlaEgoVehicleControl, CarlaEgoVehicleInfo # pylint: disable=import-error +from carla_msgs.msg import CarlaVehicleControl, CarlaVehicleInfo # pylint: disable=import-error from geometry_msgs.msg import Twist # pylint: disable=import-error class TwistToVehicleControl(CompatibleNode): # pylint: disable=too-few-public-methods """ - receive geometry_nav_msgs::Twist and publish carla_msgs::CarlaEgoVehicleControl + receive geometry_nav_msgs::Twist and publish carla_msgs::CarlaVehicleControl use max wheel steer angle """ @@ -37,23 +37,24 @@ def __init__(self): super(TwistToVehicleControl, self).__init__("twist_to_control") self.role_name = self.get_param("role_name", "ego_vehicle") + self.control_priority = self.get_param("control_priority", "6") self.max_steering_angle = None self.new_subscription( - CarlaEgoVehicleInfo, - "/carla/{}/vehicle_info".format(self.role_name), + CarlaVehicleInfo, + "/carla/vehicles/{}/vehicle_info".format(self.role_name), self.update_vehicle_info, qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) self.new_subscription( Twist, - "/carla/{}/twist".format(self.role_name), + "/carla/vehicles/{}/twist".format(self.role_name), self.twist_received, qos_profile=10) self.pub = self.new_publisher( - CarlaEgoVehicleControl, - "/carla/{}/vehicle_control_cmd".format(self.role_name), + CarlaVehicleControl, + "/carla/vehicles/{}/control/vehicle_control_cmd".format(self.role_name), qos_profile=10) def update_vehicle_info(self, vehicle_info): @@ -79,7 +80,8 @@ def twist_received(self, twist): self.logwarn("Did not yet receive vehicle info.") return - control = CarlaEgoVehicleControl() + control = CarlaVehicleControl() + control.control_priority = self.control_priority if twist == Twist(): # stop control.throttle = 0. diff --git a/carla_walker_agent/README.md b/carla_walker_agent/README.md index a9307df7..2088d16d 100644 --- a/carla_walker_agent/README.md +++ b/carla_walker_agent/README.md @@ -6,4 +6,4 @@ An simple walker agent that can follow a given route. | Topic | Type | Description | | ---------------------------------- | ------------------- | --------------------------- | -| `/carla//walker_control_cmd` | [carla_msgs.CarlaWalkerControl](https://github.com/carla-simulator/ros-carla-msgs/tree/master/msg/CarlaWalkerControl.msg) | Walker control command | +| `/carla/walkers//control/walker_control_cmd` | [carla_msgs.CarlaWalkerControl](https://github.com/carla-simulator/ros-carla-msgs/tree/master/msg/CarlaWalkerControl.msg) | Walker control command | diff --git a/carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py b/carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py index 7fd3329e..ddbcefff 100755 --- a/carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py +++ b/carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py @@ -44,31 +44,31 @@ def __init__(self): # wait for ros bridge to create relevant topics try: - self.wait_for_message("/carla/{}/odometry".format(role_name), Odometry, qos_profile=10) + self.wait_for_message("/carla/walkers/{}/object".format(role_name), Odometry, qos_profile=10) except ROSInterruptException as e: if not roscomp.ok: raise e - self._odometry_subscriber = self.new_subscription( + self._object_subscriber = self.new_subscription( Odometry, - "/carla/{}/odometry".format(role_name), - self.odometry_updated, + "/carla/walkers/{}/object".format(role_name), + self.object_updated, qos_profile=10) self.control_publisher = self.new_publisher( CarlaWalkerControl, - "/carla/{}/walker_control_cmd".format(role_name), + "/carla/walkers/{}/control/walker_control_cmd".format(role_name), qos_profile=1) self._route_subscriber = self.new_subscription( Path, - "/carla/{}/waypoints".format(role_name), + "/carla/walkers/{}/waypoints".format(role_name), self.path_updated, qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) self._target_speed_subscriber = self.new_subscription( Float64, - "/carla/{}/target_speed".format(role_name), + "/carla/walkers/{}/target_speed".format(role_name), self.target_speed_updated, qos_profile=10) @@ -97,9 +97,9 @@ def path_updated(self, path): for elem in path.poses: self._waypoints.append(elem.pose) - def odometry_updated(self, odo): + def object_updated(self, odo): """ - callback on new odometry + callback on new object """ self._current_pose = odo.pose.pose diff --git a/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py b/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py index a3005786..56aa242d 100755 --- a/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py +++ b/carla_waypoint_publisher/src/carla_waypoint_publisher/carla_waypoint_publisher.py @@ -10,10 +10,10 @@ It uses the current pose of the ego vehicle as starting point. If the vehicle is respawned or move, the route is newly calculated. -The goal is either read from the ROS topic `/carla//move_base_simple/goal`, if available +The goal is either read from the ROS topic `/carla/vehicles//move_base_simple/goal`, if available (e.g. published by RVIZ via '2D Nav Goal') or a fixed point is used. -The calculated route is published on '/carla//waypoints' +The calculated route is published on '/carla/vehicles//waypoints' Additionally, services are provided to interface CARLA waypoints. """ @@ -60,7 +60,7 @@ def __init__(self): self.role_name = self.get_param("role_name", 'ego_vehicle') self.waypoint_publisher = self.new_publisher( Path, - '/carla/{}/waypoints'.format(self.role_name), + '/carla/vehicles/{}/waypoints'.format(self.role_name), QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)) # initialize ros services @@ -79,7 +79,7 @@ def __init__(self): self.current_route = None self.goal_subscriber = self.new_subscription( PoseStamped, - "/carla/{}/goal".format(self.role_name), + "/carla/vehicles/{}/goal".format(self.role_name), self.on_goal, qos_profile=10) diff --git a/carla_ackermann_control/src/carla_ackermann_control/__init__.py b/derived_objects_visualizer/derived_objects_visualizer/__init__.py similarity index 100% rename from carla_ackermann_control/src/carla_ackermann_control/__init__.py rename to derived_objects_visualizer/derived_objects_visualizer/__init__.py diff --git a/derived_objects_visualizer/derived_objects_visualizer/derived_objects_visualizer.py b/derived_objects_visualizer/derived_objects_visualizer/derived_objects_visualizer.py new file mode 100644 index 00000000..096a83d4 --- /dev/null +++ b/derived_objects_visualizer/derived_objects_visualizer/derived_objects_visualizer.py @@ -0,0 +1,187 @@ +import math +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy +from message_filters import Subscriber +from derived_object_msgs.msg import ObjectWithCovarianceArray, ObjectWithCovariance +from std_msgs.msg import ColorRGBA +from geometry_msgs.msg import Point +from visualization_msgs.msg import Marker, MarkerArray +import tf2_ros + + +class DerivedObjectsVisualizer(Node): + def __init__(self): + super().__init__('derived_objects_visualizer') + + self.declare_parameter("objects_topic", value="/carla/world/objects_with_covariance") + + self.object_subscription = Subscriber(self, ObjectWithCovarianceArray, self.get_parameter( + "objects_topic").get_parameter_value().string_value, qos_profile=self.get_qos_objects_lidar()) + self.object_subscription.registerCallback(self.object_callback) + + self.object_publisher = self.create_publisher(MarkerArray, self.get_parameter( + "objects_topic").get_parameter_value().string_value + '/marker_objects', qos_profile=self.get_qos_marker_objects()) + + def get_qos_objects_lidar(self): + # volatile, best_effort should be ok for sensor data input + qos = QoSProfile(depth=1) + qos.durability = QoSDurabilityPolicy.VOLATILE + qos.reliability = QoSReliabilityPolicy.BEST_EFFORT + return qos + + def get_qos_marker_objects(self): + # most compatible publisher durability: transient local + # most compatible publisher reliabilty: reliable + # queue size per default: 1 + qos = QoSProfile(depth=1) + qos.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL + qos.reliability = QoSReliabilityPolicy.RELIABLE + return qos + + def get_object_color(self, object): + color = ColorRGBA() + color.a = 1.0 + # white + color.r = 1.0 + color.g = 1.0 + color.b = 1.0 + if object.classification == ObjectWithCovariance.CLASSIFICATION_PEDESTRIAN: + # yellow + color.r = 1.0 + color.g = 1.0 + color.b = 0.0 + elif object.classification == ObjectWithCovariance.CLASSIFICATION_BIKE: + # orange + color.r = 1.0 + color.g = 0.65 + color.b = 0.0 + elif (object.classification == ObjectWithCovariance.CLASSIFICATION_CAR) or (object.classification == ObjectWithCovariance.CLASSIFICATION_TRUCK) or (object.classification == ObjectWithCovariance.CLASSIFICATION_MOTORCYCLE) or (object.classification == ObjectWithCovariance.CLASSIFICATION_OTHER_VEHICLE): + # blue + color.r = 0.0 + color.g = 0.0 + color.b = 1.0 + + return color + + def object_callback(self, msg): + marker_array = MarkerArray() + + for object in msg.objects: + # print(object) + marker = Marker() + marker.header = object.header + marker.ns = "Objects" + marker.type = Marker.CUBE + marker.lifetime = rclpy.duration.Duration(seconds=1.).to_msg() + + marker.id = object.id + marker.pose.position.x = object.pose.pose.position.x + marker.pose.position.y = object.pose.pose.position.y + marker.pose.position.z = object.pose.pose.position.z + marker.pose.orientation.x = object.pose.pose.orientation.x + marker.pose.orientation.y = object.pose.pose.orientation.y + marker.pose.orientation.z = object.pose.pose.orientation.z + marker.pose.orientation.w = object.pose.pose.orientation.w + + marker.scale.x = object.shape.dimensions[0] + marker.scale.y = object.shape.dimensions[1] + marker.scale.z = object.shape.dimensions[2] + + marker.color = self.get_object_color(object) + + marker.action = Marker.ADD + + if len(object.polygon.points) > 0: + print('polygon') + + marker_array.markers.append(marker) + + # Heading + marker = Marker() + marker.header = object.header + marker.ns = "Heading" + marker.type = Marker.ARROW + marker.lifetime = rclpy.duration.Duration(seconds=1.).to_msg() + + marker.id = object.id + marker.pose.position.x = object.pose.pose.position.x + marker.pose.position.y = object.pose.pose.position.y + marker.pose.position.z = object.pose.pose.position.z + marker.pose.orientation.x = object.pose.pose.orientation.x + marker.pose.orientation.y = object.pose.pose.orientation.y + marker.pose.orientation.z = object.pose.pose.orientation.z + marker.pose.orientation.w = object.pose.pose.orientation.w + + marker.scale.x = max(object.shape.dimensions[0], 1.0) + marker.scale.y = 0.2 + marker.scale.z = 0.2 + + marker.color = self.get_object_color(object) + + marker.action = Marker.ADD + + marker_array.markers.append(marker) + + # Speed + marker = Marker() + marker.header = object.header + marker.ns = "speed" + + marker.type = Marker.LINE_STRIP + marker.lifetime = rclpy.duration.Duration(seconds=1.).to_msg() + marker.pose.position.x = 0. + marker.pose.position.y = 0. + marker.pose.position.z = 0. + marker.pose.orientation.x = 0. + marker.pose.orientation.y = 0. + marker.pose.orientation.z = 0. + marker.pose.orientation.w = 1. + + marker.color.g = 1.0 + marker.color.a = 1.0 + marker.action = Marker.ADD + + marker.id = object.id + + marker.scale.x = 0.2 + marker.scale.y = 0. + marker.scale.z = 0. + + point = Point() + point = object.pose.pose.position + marker.points.append(point) + + point2 = Point() + point2.x = point.x + object.twist.twist.linear.x*10.0 + point2.y = point.y + object.twist.twist.linear.y*10.0 + point2.z = point.z + + marker.points.append(point2) + marker_array.markers.append(marker) + + marker = Marker() + marker.header = object.header + marker.ns = "IDs" + marker.lifetime = rclpy.duration.Duration(seconds=1.).to_msg() + marker.type = Marker.TEXT_VIEW_FACING + + marker.id = object.id + marker.pose.position.x = object.pose.pose.position.x + marker.pose.position.y = object.pose.pose.position.y + marker.pose.position.z = object.pose.pose.position.z + 2.0 + + marker.scale.z = 1.0 + marker.color.g = 1.0 + marker.color.a = 1.0 + + speed = round(math.sqrt(object.twist.twist.linear.x**2 + object.twist.twist.linear.y**2),2) + marker.text = str(object.id) + " - " + str(speed) + "m/s" + marker.action = Marker.ADD + + marker_array.markers.append(marker) + + self.object_publisher.publish(marker_array) + + def before_shutdown(self): + pass diff --git a/derived_objects_visualizer/derived_objects_visualizer/derived_objects_visualizer_main.py b/derived_objects_visualizer/derived_objects_visualizer/derived_objects_visualizer_main.py new file mode 100644 index 00000000..2aedda10 --- /dev/null +++ b/derived_objects_visualizer/derived_objects_visualizer/derived_objects_visualizer_main.py @@ -0,0 +1,29 @@ +import rclpy + +from derived_objects_visualizer.derived_objects_visualizer import DerivedObjectsVisualizer + + +def main(args=None): + rclpy.init(args=args) + + # create nodes + derived_objects_visualizer = DerivedObjectsVisualizer() + + # create executor and add nodes + executor = rclpy.executors.MultiThreadedExecutor() + if(not executor.add_node(derived_objects_visualizer)): + derived_objects_visualizer.get_logger().error( + "Can't add derived_objects_visualizer to executor") + + try: + executor.spin() + except KeyboardInterrupt: + print("Keyboard interrupt. Shutting down.") + + derived_objects_visualizer.before_shutdown() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/derived_objects_visualizer/package.xml b/derived_objects_visualizer/package.xml new file mode 100644 index 00000000..0a2a6454 --- /dev/null +++ b/derived_objects_visualizer/package.xml @@ -0,0 +1,27 @@ + + + + derived_objects_visualizer + 0.0.0 + DerivedObjects Visualizer + CARLA Simulator Team + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + rclpy + tf2_ros + sensor_msgs_py + std_msgs + geometry_msgs + nav_msgs + derived_object_msgs + visualization_msgs + + + ament_python + + diff --git a/carla_ackermann_control/resource/carla_ackermann_control b/derived_objects_visualizer/resource/derived_objects_visualizer similarity index 100% rename from carla_ackermann_control/resource/carla_ackermann_control rename to derived_objects_visualizer/resource/derived_objects_visualizer diff --git a/derived_objects_visualizer/setup.cfg b/derived_objects_visualizer/setup.cfg new file mode 100644 index 00000000..d6a178a1 --- /dev/null +++ b/derived_objects_visualizer/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/derived_objects_visualizer +[install] +install_scripts=$base/lib/derived_objects_visualizer diff --git a/derived_objects_visualizer/setup.py b/derived_objects_visualizer/setup.py new file mode 100644 index 00000000..54e22fa0 --- /dev/null +++ b/derived_objects_visualizer/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup, find_packages + +package_name = 'derived_objects_visualizer' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='CARLA Simulator Team', + maintainer_email='carla.simulator@gmail.com', + description='DerivedObjects Visualizer', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + f'derived_objects_visualizer = {package_name}.derived_objects_visualizer_main:main', + ], + }, +) diff --git a/docs/carla_ackermann_control.md b/docs/carla_ackermann_control.md index c2cbd696..1d2f9846 100644 --- a/docs/carla_ackermann_control.md +++ b/docs/carla_ackermann_control.md @@ -1,9 +1,6 @@ # Carla Ackermann Control -The [`carla_ackermann_control` package](https://github.com/carla-simulator/ros-bridge/tree/master/carla_ackermann_control) is used to control a CARLA vehicle with [Ackermann messages][ackermanncontrolmsg]. The package converts the Ackermann messages into [CarlaEgoVehicleControl][carlaegovehiclecontrolmsg] messages. It reads vehicle information from CARLA and passes that information to a Python based PID controller called `simple-pid` to control the acceleration and velocity. - -[ackermanncontrolmsg]: https://docs.ros.org/en/api/ackermann_msgs/html/msg/AckermannDrive.html -[carlaegovehiclecontrolmsg]: https://carla.readthedocs.io/en/latest/ros_msgs/#carlaegovehiclecontrolmsg +[carlavehiclecontrolmsg]: https://carla.readthedocs.io/en/latest/ros_msgs/#carlavehiclecontrolmsg - [__Configuration__](#configuration) - [__Testing control messages__](#testing-control-messages) @@ -24,7 +21,7 @@ Parameters can be set both initially in a [configuration file][ackermanconfig] w ### Testing control messages -Test the setup by sending commands to the car via the topic `/carla//ackermann_cmd`. For example, move an ego vehicle with the role name of `ego_vehicle` forward at a speed of 10 meters/sec by running this command: +Test the setup by sending commands to the car via the topic `/carla/vehicles//ackermann_cmd`. For example, move an ego vehicle with the role name of `ego_vehicle` forward at a speed of 10 meters/sec by running this command: ```bash @@ -56,7 +53,7 @@ ros2 topic pub /carla/ego_vehicle/ackermann_cmd ackermann_msgs/AckermannDrive "{ |Topic|Type|Description| |--|--|--| -|`/carla//ackermann_cmd` | [ackermann_msgs.AckermannDrive][ackermanncontrolmsg] | __Subscriber__ for steering commands | +|`/carla/vehicles//ackermann_cmd` | [ackermann_msgs.AckermannDrive][ackermanncontrolmsg] | __Subscriber__ for steering commands |
@@ -64,7 +61,7 @@ ros2 topic pub /carla/ego_vehicle/ackermann_cmd ackermann_msgs/AckermannDrive "{ |Topic|Type|Description| |--|--|--| -| `/carla//ackermann_control/control_info` | [carla_ackermann_control.EgoVehicleControlInfo][egovehiclecontrolmsg] | The current values used within the controller (useful for debugging) | +| `/carla/vehicles//ackermann_control/control_info` | [carla_ackermann_control.EgoVehicleControlInfo][egovehiclecontrolmsg] | The current values used within the controller (useful for debugging) | [egovehiclecontrolmsg]: https://carla.readthedocs.io/en/latest/ros_msgs/#egovehiclecontrolinfomsg diff --git a/docs/carla_ad_agent.md b/docs/carla_ad_agent.md index 62a24903..a399d1f5 100644 --- a/docs/carla_ad_agent.md +++ b/docs/carla_ad_agent.md @@ -19,15 +19,6 @@ The PID parameters were gathered by [Ziegler-Nichols method](https://en.wikipedi --- -## Requirements - -To be able to use the `carla_ad_agent`, a minimal set of sensors need to be spawned (see [Carla Spawn Objects](carla_spawn_objects.md) for information on how to spawn sensors): - -- An odometry pseudo sensor (`sensor.pseudo.odom`) with role-name `odometry` attached to the vehicle. -- An object pseudo sensor (`sensor.pseudo.objects`) with role-name `objects` attached to the vehicle. -- A traffic light pseudo sensor (`sensor.pseudo.traffic_lights`) with role-name `traffic_lights`. - ---- ## ROS API @@ -46,10 +37,10 @@ To be able to use the `carla_ad_agent`, a minimal set of sensors need to be spaw | Topic | Type | Description | |-------|------|-------------| -| `/carla//target_speed` | [std_msgs/Float64](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html) | Target speed of the ego vehicle | -| `/carla//odometry` | [nav_msgs/Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) | Odometry of the ego vehicle | -| `/carla//vehicle_info` | [carla_msgs/CarlaEgoVehicleInfo](ros_msgs.md#carlaegovehicleinfomsg) | Identify the CARLA actor id of the ego vehicle | -| `/carla//objects` | [derived_object_msgs/ObjectArray](https://docs.ros.org/en/melodic/api/derived_object_msgs/html/msg/ObjectArray.html) | Information about other actors | +| `/carla/vehicles//target_speed` | [std_msgs/Float64](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html) | Target speed of the ego vehicle | +| `/carla/vehicles//odometry` | [nav_msgs/Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) | Odometry of the ego vehicle | +| `/carla/vehicles//vehicle_info` | [carla_msgs/CarlaVehicleInfo](ros_msgs.md#carlavehicleinfomsg) | Identify the CARLA actor id of the ego vehicle | +| `/carla/vehicles//objects` | [derived_object_msgs/ObjectArray](https://docs.ros.org/en/melodic/api/derived_object_msgs/html/msg/ObjectArray.html) | Information about other actors | | `/carla/traffic_lights/status` | [carla_msgs/CarlaTrafficLightStatusList](ros_msgs.md#carlatrafficlightstatuslistmsg) | Get the current state of the traffic lights | | `/carla/traffic_lights/info` | [carla_msgs/CarlaTrafficLightInfoList](ros_msgs.md#carlatrafficlightinfolistmsg) | Get info about traffic lights | @@ -59,7 +50,7 @@ To be able to use the `carla_ad_agent`, a minimal set of sensors need to be spaw | Topic | Type | Description | |-------|------|-------------| -| `/carla//speed_command` | [std_msgs/Float64](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html) | Target speed | +| `/carla/vehicles//speed_command` | [std_msgs/Float64](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html) | Target speed |
@@ -84,9 +75,9 @@ To be able to use the `carla_ad_agent`, a minimal set of sensors need to be spaw | Topic | Type | Description | |-------|------|-------------| -| `/carla//waypoints` | [nav_msgs/Path](https://docs.ros.org/en/api/nav_msgs/html/msg/Path.html) | Route to follow | -| `/carla//odometry` | [nav_msgs/Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) | Odometry of the ego vehicle | -| `/carla//speed_command` | [std_msgs/Float64](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html) | Target speed | +| `/carla/vehicles//waypoints` | [nav_msgs/Path](https://docs.ros.org/en/api/nav_msgs/html/msg/Path.html) | Route to follow | +| `/carla/vehicles//odometry` | [nav_msgs/Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) | Odometry of the ego vehicle | +| `/carla/vehicles//speed_command` | [std_msgs/Float64](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html) | Target speed |
@@ -94,7 +85,7 @@ To be able to use the `carla_ad_agent`, a minimal set of sensors need to be spaw | Topic | Type | Description | |-------|------|-------------| -| `/carla//next_target` | [visualization_msgs/Marker](http://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html) | Next target pose marker | -| `/carla//vehicle_control_cmd` | [carla_msgs/CarlaEgoVehicleControl](ros_msgs.md#carlaegovehiclecontrolmsg) | Vehicle control command | +| `/carla/vehicles//next_target` | [visualization_msgs/Marker](http://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html) | Next target pose marker | +| `/carla/vehicles//control/vehicle_control_cmd` | [carla_msgs/CarlaVehicleControl](ros_msgs.md#carlavehiclecontrolmsg) | Vehicle control command |
diff --git a/docs/carla_manual_control.md b/docs/carla_manual_control.md index 2be53b17..e50af850 100644 --- a/docs/carla_manual_control.md +++ b/docs/carla_manual_control.md @@ -13,7 +13,7 @@ The [CARLA manual control package](https://github.com/carla-simulator/ros-bridge To be able to use `carla_manual_control`, some specific sensors need to be attached to the ego vehicle (see [Carla Spawn Objects](carla_spawn_objects.md) for information on how to attach sensors to vehicles): - __to display an image__: a camera with role-name `rgb_view` and resolution 800x600. -- __to display the current position__: a GNSS sensor with role-name `gnss` and an odometry pseudo-sensor with role-name `odometry`. +- __to display the current position__: a GNSS sensor with role-name `gnss`. - __to get a notification on lane invasions__: a lane invasion sensor with role-name `lane_invasion`. - __to get a notification on collisons__: a collision sensor with role-name `collision`. diff --git a/docs/carla_ros_scenario_runner.md b/docs/carla_ros_scenario_runner.md index bee2097a..81c8caa5 100644 --- a/docs/carla_ros_scenario_runner.md +++ b/docs/carla_ros_scenario_runner.md @@ -89,7 +89,7 @@ ros2 service call /scenario_runner/execute_scenario carla_ros_scenario_runner_ty The controller `ros_vehicle_control` provides the following topics: | Topic | Type | Description | |-------|------|-------------| -| `/carla//waypoints` | [`nav_msgs.Path`](https://docs.ros.org/en/api/nav_msgs/html/msg/Path.html) | the path defined within the scenario. Note: The topic name can be changed by modifying the parameter `path_topic_name` | -| `/carla//target_speed` | [`std_msgs.Float64`](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html) | the target speed as defined within the scenario | +| `/carla/vehicles//waypoints` | [`nav_msgs.Path`](https://docs.ros.org/en/api/nav_msgs/html/msg/Path.html) | the path defined within the scenario. Note: The topic name can be changed by modifying the parameter `path_topic_name` | +| `/carla/vehicles//target_speed` | [`std_msgs.Float64`](https://docs.ros.org/en/api/std_msgs/html/msg/Float64.html) | the target speed as defined within the scenario |
diff --git a/docs/carla_spawn_objects.md b/docs/carla_spawn_objects.md index d6399f3b..e75b64c8 100644 --- a/docs/carla_spawn_objects.md +++ b/docs/carla_spawn_objects.md @@ -85,7 +85,7 @@ All sensor attributes are defined as described in the [blueprint library](https: ### Respawning vehicles -A vehicle can be respawned to a different location during a simulation by publishing to the topic `/carla///initialpose`. To use this functionality: +A vehicle can be respawned to a different location during a simulation by publishing to the topic `/carla/vehicles///initialpose`. To use this functionality: 1. Attach an `actor.pseudo.control` pseudo-actor to the vehicle in the `.json` file. It should have the same `id` value as the value passed as `` used to publish to the topic: diff --git a/docs/carla_twist_to_control.md b/docs/carla_twist_to_control.md index 1bc8c097..d21b1f54 100644 --- a/docs/carla_twist_to_control.md +++ b/docs/carla_twist_to_control.md @@ -1,6 +1,6 @@ # Carla Twist to Control -The [`carla_twist_to_control` package](https://github.com/carla-simulator/ros-bridge/tree/master/carla_twist_to_control) converts a [geometry_msgs.Twist](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) to [carla_msgs.CarlaEgoVehicleControl](ros_msgs.md#carlaegovehiclecontrolmsg). +The [`carla_twist_to_control` package](https://github.com/carla-simulator/ros-bridge/tree/master/carla_twist_to_control) converts a [geometry_msgs.Twist](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) to [carla_msgs.CarlaVehicleControl](ros_msgs.md#carlavehiclecontrolmsg). --- ## ROS API @@ -9,8 +9,8 @@ The [`carla_twist_to_control` package](https://github.com/carla-simulator/ros-br | Topic | Type | Description | |-------|------|-------------| -| `/carla//vehicle_info` | [`carla_msgs.CarlaEgoVehicleInfo`](ros_msgs.md#carlaegovehicleinfomsg) | Ego vehicle info, to receive max steering angle. | -| `/carla//twist` | [`geometry_msgs.Twist`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) | Twist to convert. | +| `/carla/vehicles//vehicle_info` | [`carla_msgs.CarlaVehicleInfo`](ros_msgs.md#carlavehicleinfomsg) | Ego vehicle info, to receive max steering angle. | +| `/carla/vehicles//twist` | [`geometry_msgs.Twist`](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) | Twist to convert. |
@@ -18,6 +18,6 @@ The [`carla_twist_to_control` package](https://github.com/carla-simulator/ros-br | Topic | Type | Description | |-------|------|-------------| -| `/carla//vehicle_control_cmd` | [`carla_msgs.CarlaEgoVehicleControl`](ros_msgs.md#carlaegovehiclecontrolmsg) | Converted vehicle control command. | +| `/carla/vehicles//control/vehicle_control_cmd` | [`carla_msgs.CarlaVehicleControl`](ros_msgs.md#carlavehiclecontrolmsg) | Converted vehicle control command. |
diff --git a/docs/carla_waypoint.md b/docs/carla_waypoint.md index fc0d5dbe..4debcc87 100644 --- a/docs/carla_waypoint.md +++ b/docs/carla_waypoint.md @@ -25,7 +25,7 @@ ros2 launch carla_waypoint_publisher carla_waypoint_publisher.launch.py ### Set a goal -If available a goal is read from the topic `/carla//goal`, otherwise a fixed spawn point is used. +If available a goal is read from the topic `/carla/vehicles//goal`, otherwise a fixed spawn point is used. The preferred way of setting a goal is to click '2D Nav Goal` in RVIZ. diff --git a/docs/images/ad_demo.dot b/docs/images/ad_demo.dot index cd97b1ac..12b363c7 100644 --- a/docs/images/ad_demo.dot +++ b/docs/images/ad_demo.dot @@ -39,7 +39,7 @@ digraph graphname { tooltip="/carla_waypoint_publisher", width=2.9067]; n___rviz -> n___carla_waypoint_publisher [URL=topic_3A__carla__ego_vehicle__goal, - label="/carla/ego_vehicle/goal", + label="/carla/vehicles/ego_vehicle/goal", lp="188.49,609.5", penwidth=1, pos="e,322.49,631.48 92.426,541.3 145.96,562.28 250.36,603.21 313.16,627.82"]; @@ -58,7 +58,7 @@ digraph graphname { tooltip="/rviz", width=0.75]; n___rviz -> n___carla_ego_vehicle_ego_vehicle [URL=topic_3A__initialpose, - label="/carla/ego_vehicle/initialpose", + label="/carla/vehicles/ego_vehicle/initialpose", lp="1169.4,526.5", penwidth=1, pos="e,1545.9,484.96 686.93,420.12 692.63,424.89 699.37,429.75 706.27,433 842.05,496.96 888.71,477.92 1036.9,502 1129.3,517.02 1380.6,503.05 1454.1,561 1461,566.42 1454.8,574.23 1462.1,579 1476.3,588.2 1487.6,590.27 1500.1,579 1512.3,568.11 1497.2,518.1 1508.1,506 1515.8,497.57 1525.6,491.81 1536.3,487.92"]; @@ -70,7 +70,7 @@ digraph graphname { tooltip="/carla_twist_to_control", width=2.5637]; n___rviz -> n___carla_twist_to_control [URL=topic_3A__carla__ego_vehicle__twist, - label="/carla/ego_vehicle/twist", + label="/carla/vehicles/ego_vehicle/twist", lp="771.27,421.5", penwidth=1, pos="e,847.79,414.98 698.66,406.55 731.36,408.4 787.87,411.59 837.61,414.41"]; @@ -101,17 +101,17 @@ digraph graphname { fontcolor=blue, pos="e,1536.6,304.89 675.79,387.15 680.68,371.53 690.05,349.45 706.27,337 756.06,298.78 782.37,317.4 844.27,307 1045.1,273.26 1098.5,274.03 1301.9,284 1377.7,287.72 1463.4,296.44 1526.3,303.69"]; n___rviz -> n___carla_spectator_camera [URL=topic_3A__carla__ego_vehicle__spectator_pose, - label="/carla/ego_vehicle/spectator_pose", + label="/carla/vehicles/ego_vehicle/spectator_pose", lp="1169.4,291.5", penwidth=1, pos="e,1536.6,304.89 675.79,387.15 680.68,371.53 690.05,349.45 706.27,337 756.06,298.78 782.37,317.4 844.27,307 1045.1,273.26 1098.5,274.03 1301.9,284 1377.7,287.72 1463.4,296.44 1526.3,303.69"]; n___carla_twist_to_control -> n___carla_ros_bridge [URL=topic_3A__carla__ego_vehicle__vehicle_control_cmd_manual, - label="/carla/ego_vehicle/vehicle_control_cmd_manual", + label="/carla/vehicles/ego_vehicle/vehicle_control_cmd_manual", lp="1169.4,457.5", penwidth=1, pos="e,1309.8,450.27 1005.5,432.06 1016,433.59 1026.7,434.97 1036.9,436 1126.4,445.06 1229.6,448.65 1299.7,450.08"]; n___carla_waypoint_publisher -> n___rviz [URL=topic_3A__carla__ego_vehicle__waypoints, - label="/carla/ego_vehicle/waypoints", + label="/carla/vehicles/ego_vehicle/waypoints", lp="556.77,575.5", penwidth=1, pos="e,662.37,422.27 391.18,630.38 455.5,587.57 615.9,480.06 636.27,459 644.28,450.73 651.51,440.36 657.29,430.93"]; @@ -123,7 +123,7 @@ digraph graphname { tooltip="/carla_ad_agent_ego_vehicle", width=3.1414]; n___carla_waypoint_publisher -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__ego_vehicle__waypoints, - label="/carla/ego_vehicle/waypoints", + label="/carla/vehicles/ego_vehicle/waypoints", lp="1169.4,676.5", penwidth=1, pos="e,2017.5,594.46 446.66,659.26 456.95,660.36 467.34,661.32 477.27,662 640.01,673.19 681.15,665.89 844.27,667 1104.4,668.77 1620,671.24 1726.1,666 1840.4,660.36 1895.2,715.05 1982.4,641 1992.9,632.07 1980.2,620.31 1990.4,611 1995.8,606.05 2001.9,601.96 2008.4,598.6"]; @@ -164,17 +164,17 @@ digraph graphname { penwidth=1, pos="e,676.56,422.87 1537.5,626.95 1396.9,605.25 1095.9,556.81 844.27,503 782.5,489.79 756.33,507.53 706.27,469 694.28,459.77 685.85,445.2 680.28,432.3"]; n___carla_ros_scenario_runner -> n___carla_waypoint_publisher [URL=topic_3A__carla__ego_vehicle__goal, - label="/carla/ego_vehicle/goal", + label="/carla/vehicles/ego_vehicle/goal", lp="936.56,655.5", penwidth=1, pos="e,469.3,648 1515.6,644.58 1474.2,646.42 1425.8,648 1382,648 671.27,648 671.27,648 671.27,648 607.87,648 537.09,648 479.52,648"]; n___carla_ros_scenario_runner -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__ego_vehicle__target_speed, - label="/carla/ego_vehicle/target_speed", + label="/carla/vehicles/ego_vehicle/target_speed", lp="1858.3,629.5", penwidth=1, pos="e,2022.7,596.15 1696.4,626.86 1709,625.11 1721.9,623.42 1734.1,622 1786.2,615.96 1917,604.85 2012.7,596.97"]; - n___carla_ros_bridge -> n___rviz [URL=topic_3A__carla__ego_vehicle__vehicle_status, - label="/carla/ego_vehicle/vehicle_status", + n___carla_ros_bridge -> n___rviz [URL=topic_3A__carla__ego_vehicle__vehicle_control_status, + label="/carla/vehicles/ego_vehicle/vehicle_control_status", lp="936.56,385.5", penwidth=1, pos="e,697.21,399.67 1361.4,433.48 1346.1,421.51 1324,406.49 1301.9,399 1109,333.77 1047.2,362.26 844.27,378 796.62,381.7 742.24,391.04 707.37,397.7"]; @@ -184,12 +184,12 @@ digraph graphname { penwidth=1, pos="e,2016.2,593.77 1402.9,468.38 1418.9,481.63 1441.7,500.22 1462.1,516 1478.7,528.83 1485.7,528.82 1500.1,544 1504.6,548.64 1502.6,552.83 1508.1,556 1590.2,602.71 1851.7,600.22 2006.2,594.17"]; n___carla_ros_bridge -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__odometry, - label="/carla/ego_vehicle/odometry", + label="/carla/vehicles/ego_vehicle/odometry", lp="1617.1,601.5", penwidth=1, pos="e,2016.2,593.77 1402.9,468.38 1418.9,481.63 1441.7,500.22 1462.1,516 1478.7,528.83 1485.7,528.82 1500.1,544 1504.6,548.64 1502.6,552.83 1508.1,556 1590.2,602.71 1851.7,600.22 2006.2,594.17"]; n___carla_ros_bridge -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__objects, - label="/carla/ego_vehicle/objects", + label="/carla/vehicles/ego_vehicle/objects", lp="1617.1,601.5", penwidth=1, pos="e,2016.2,593.77 1402.9,468.38 1418.9,481.63 1441.7,500.22 1462.1,516 1478.7,528.83 1485.7,528.82 1500.1,544 1504.6,548.64 1502.6,552.83 1508.1,556 1590.2,602.71 1851.7,600.22 2006.2,594.17"]; @@ -213,12 +213,12 @@ digraph graphname { fontcolor=blue, pos="e,1410.5,467.54 2035.6,576.6 1955.1,566.19 1832.6,550.42 1726.1,537 1629.3,524.79 1602,537.01 1508.1,510 1477,501.04 1444,485.32 1419.5,472.35"]; n___carla_ad_agent_ego_vehicle -> n___carla_ros_bridge [URL=topic_3A__carla__ego_vehicle__vehicle_control_cmd, - label="/carla/ego_vehicle/vehicle_control_cmd", + label="/carla/vehicles/ego_vehicle/vehicle_control_cmd", lp="1617.1,544.5", penwidth=1, pos="e,1410.5,467.54 2035.6,576.6 1955.1,566.19 1832.6,550.42 1726.1,537 1629.3,524.79 1602,537.01 1508.1,510 1477,501.04 1444,485.32 1419.5,472.35"]; n___carla_ros_bridge -> n___carla_ad_agent_ego_vehicle [URL=topic_3A__carla__ros_bridge_ad_agent_ego_vehicle, - label="/carla/ego_vehicle/vehicle_info", + label="/carla/vehicles/ego_vehicle/vehicle_info", lp="1617.1,544.5", penwidth=1, pos="e,1410.5,467.54 2035.6,576.6 1955.1,566.19 1832.6,550.42 1726.1,537 1629.3,524.79 1602,537.01 1508.1,510 1477,501.04 1444,485.32 1419.5,472.35"]; diff --git a/docs/ros_msgs.md b/docs/ros_msgs.md index aaf694c4..7fdaa82d 100644 --- a/docs/ros_msgs.md +++ b/docs/ros_msgs.md @@ -63,7 +63,7 @@ These messages control the simulation while in synchronous, non-passive mode. Th --- -## CarlaEgoVehicleControl.msg +## CarlaVehicleControl.msg Messages sent to apply a control to a vehicle in both modes, autopilot and manual. These are published in a stack. @@ -77,10 +77,11 @@ Messages sent to apply a control to a vehicle in both modes, autopilot and manua | `reverse` | bool | If **True**, the vehicle will move reverse. | | `gear` | int32 | Changes between the available gears in a vehicle. | | `manual_gear_shift` | bool | If **True**, the gears will be shifted using `gear`. | +| `control_priority` | uint8 | This provides the priority of the control command input. 4 <= control_priority <= 255. Higher numbers of control input override lower ones | --- -## CarlaEgoVehicleInfo.msg +## CarlaVehicleInfo.msg Static information regarding a vehicle, mostly the attributes used to define the vehicle's physics. @@ -90,7 +91,7 @@ Static information regarding a vehicle, mostly the attributes used to define the | `type` | string | The identifier of the blueprint this vehicle was based on. | | `type` | string | The identifier of the blueprint this vehicle was based on. | | `rolename` | string | Role assigned to the vehicle. | -| `wheels` | [CarlaEgoVehicleInfoWheel](<#carlaegovehicleinfowheelmsg>) | List of messages with information regarding wheels. | +| `wheels` | [CarlaVehicleInfoWheel](<#carlavehicleinfowheelmsg>) | List of messages with information regarding wheels. | | `max_rpm` | float32 | Maximum RPM of the vehicle's engine. | | `moi` | float32 | Moment of inertia of the vehicle's engine. | | `damping_rate_full_throttle` | float32 | Damping rate when the throttle is at maximum. | @@ -105,9 +106,9 @@ Static information regarding a vehicle, mostly the attributes used to define the --- -## CarlaEgoVehicleInfoWheel.msg +## CarlaVehicleInfoWheel.msg -Static information regarding a wheel that will be part of a [CarlaEgoVehicleInfo.msg](#carlaegovehicleinfomsg) message. +Static information regarding a wheel that will be part of a [CarlaVehicleInfo.msg](#carlavehicleinfomsg) message. | Field | Type | Description | | -------------------------------------------------------- | -------------------------------------------------------- | -------------------------------------------------------- | @@ -121,17 +122,16 @@ Static information regarding a wheel that will be part of a [CarlaEgoVehicleInfo --- -## CarlaEgoVehicleStatus.msg +## CarlaVehicleControlStatus.msg -Current status of the vehicle as an object in the world. +Current status of the vehicle control settings. | Field | Type | Description | | ------------------------------------------------------------------------- | ------------------------------------------------------------------------- | ------------------------------------------------------------------------- | -| `header` | [Header](https://docs.ros.org/en/melodic/api/std_msgs/html/msg/Header.html) | Time stamp and frame ID when the message is published. | -| `velocity` | float32 | Current speed of the vehicle. | -| `acceleration` | geometry\_msgs/Accel | Current acceleration of the vehicle. | -| `orientation` | geometry\_msgs/Quaternion | Current orientation of the vehicle. | -| `control` | [CarlaEgoVehicleControl](<#carlaegovehiclecontrolmsg>) | Current control values as reported by CARLA. | +| `active_control_type` | {VEHICLE_CONTROL, ACKERMANN_CONTROL} | Current active control type as reported by CARLA. | +| `last_applied_vehicle_control` | [CarlaVehicleControl](<#carlavehiclecontrolmsg>) | Current control values as reported by CARLA. | +| `last_applied_ackermann_control` | AckermannDriveStamped | Current ackermann control values as reported by CARLA. | + --- @@ -280,7 +280,7 @@ Current values within an Ackermann controller. These messages are useful for deb | `target` | [EgoVehicleControlTarget](<#egovehiclecontroltargetmsg>) | Limits to the controller values. | | `current` | [EgoVehicleControlCurrent](<#egovehiclecontrolcurrentmsg>) | Limits to the controller values. | | `status` | [EgoVehicleControlStatus](<#egovehiclecontrolstatusmsg>) | Limits to the controller values. | -| `output` | [CarlaEgoVehicleControl](<#carlaegovehiclecontrolmsg>) | Limits to the controller values. | +| `output` | [CarlaVehicleControl](<#carlavehiclecontrolmsg>) | Limits to the controller values. | --- diff --git a/docs/run_ros.md b/docs/run_ros.md index cc9c5934..777e79a5 100644 --- a/docs/run_ros.md +++ b/docs/run_ros.md @@ -42,10 +42,6 @@ The command to run depends on whether you installed the ROS bridge via the Debia Once you have set your ROS environment and have a CARLA server running, you will need to start the `carla_ros_bridge` package before being able to use any of the other packages. To do that, run the following command: ```sh - # ROS 1 - roslaunch carla_ros_bridge carla_ros_bridge.launch - - # ROS 2 ros2 launch carla_ros_bridge carla_ros_bridge.launch.py ``` @@ -106,39 +102,31 @@ If the ROS bridge is not in passive mode (ROS bridge is the one ticking the worl There are two modes to control the ego vehicle: -1. Normal mode - reading commands from `/carla//vehicle_control_cmd` -2. Manual mode - reading commands from `/carla//vehicle_control_cmd_manual`. This allows to manually override Vehicle Control Commands published by a software stack. - -You can toggle between the two modes by publishing to `/carla//vehicle_control_manual_override`. For an example of this being used see [Carla Manual Control](carla_manual_control.md). +1. Normal mode - reading commands from `/carla/vehicles//control/vehicle_control_cmd`. The To test steering from the command line: __1.__ Launch the ROS Bridge with an ego vehicle: ```sh - # ROS 1 - roslaunch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch - - # ROS 2 ros2 launch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch.py ``` -__2.__ In another terminal, publish to the topic `/carla//vehicle_control_cmd` +__2.__ In another terminal, publish to the topic `/carla/vehicles//control/vehicle_control_cmd` ```sh # Max forward throttle with max steering to the right - - # for ros1 - rostopic pub /carla/ego_vehicle/vehicle_control_cmd carla_msgs/CarlaEgoVehicleControl "{throttle: 1.0, steer: 1.0}" -r 10 - - # for ros2 - ros2 topic pub /carla/ego_vehicle/vehicle_control_cmd carla_msgs/CarlaEgoVehicleControl "{throttle: 1.0, steer: 1.0}" -r 10 + ros2 topic pub /carla/ego_vehicle/control/vehicle_control_cmd carla_msgs/CarlaVehicleControl "{throttle: 1.0, steer: 1.0}" -r 10 ``` -The current status of the vehicle can be received via topic `/carla//vehicle_status`. Static information about the vehicle can be received via `/carla//vehicle_info`. +The current control status of the vehicle can be received via topic `/carla/vehicles//vehicle_constrol_status`. Static information about the vehicle can be received via `/carla/vehicles//vehicle_info`. + +It is possible to use [AckermannDrive](https://docs.ros.org/en/api/ackermann_msgs/html/msg/AckermannDrive.html) messages to control the ego vehicles. This can be achieved by publishing to the topic +```sh + ros2 topic pub /carla/ego_vehicle/control/ackermann_control_cmd ackermann_msgs/AckermannDrive "{speed: 10., acceleration: 1., jerk: 1., steering_angle: 0.1, steering_angle_velocity: 0.1}" -r 10 -It is possible to use [AckermannDrive](https://docs.ros.org/en/api/ackermann_msgs/html/msg/AckermannDrive.html) messages to control the ego vehicles. This can be achieved through the use of the [CARLA Ackermann Control](carla_ackermann_control.md) package. +``` --- diff --git a/docs/rviz_plugin.md b/docs/rviz_plugin.md index 0910e3aa..ba53759e 100644 --- a/docs/rviz_plugin.md +++ b/docs/rviz_plugin.md @@ -76,9 +76,9 @@ ros2 launch carla_manual_control carla_manual_control.launch.py | Topic | Type | Description | |-------|------|-------------| -| `/carla/status` | [carla_msgs/CarlaStatus](ros_msgs.md#carlastatusmsg) | Read the current status of CARLA | -| `/carla/ego_vehicle/vehicle_status` | [carla_msgs/CarlaEgoVehicleStatus](ros_msgs.md#carlaegovehiclestatusmsg) | Display the current state of the ego vehicle | -| `/carla/ego_vehicle/odometry` | [nav_msgs/Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) | Display the current pose of the ego vehicle | +| `/carla/world/status` | [carla_msgs/CarlaStatus](ros_msgs.md#carlastatusmsg) | Read the current status of CARLA | +| `/carla/vehicles/ego_vehicle/vehicle_control_status` | [carla_msgs/CarlaVehicleControlStatus](ros_msgs.md#carlavehiclecontrolstatusmsg) | Display the current state of the ego vehicle | +| `/carla/vehicles/ego_vehicle/odometry` | [nav_msgs/Odometry](https://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html) | Display the current pose of the ego vehicle | | `/scenario_runner/status` | [carla_ros_scenario_runner_types/CarlaScenarioRunnerStatus](ros_msgs.md#carlascenariorunnerstatusmsg) | Visualize the scenario runner status | | `/carla/available_scenarios` | [carla_ros_scenario_runner_types/CarlaScenarioList](ros_msgs.md#carlascenariolistmsg) | Provides a list of scenarios to execute (disabled in combo box)| @@ -88,10 +88,10 @@ ros2 launch carla_manual_control carla_manual_control.launch.py | Topic | Type | Description | |-------|------|-------------| -| `/carla/control` | [carla_msgs/CarlaControl](ros_msgs.md#carlacontrolmsg) | Play/pause/step CARLA | -| `/carla/ego_vehicle/spectator_pose` | [geometry_msgs/PoseStamped](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html) | Publish the current pose of the RVIZ camera view | -| `/carla/ego_vehicle/vehicle_control_manual_override` | [std_msgs/Bool](https://docs.ros.org/en/api/std_msgs/html/msg/Bool.html) | Enable/disable vehicle control override | -| `/carla/ego_vehicle/twist` | [geometry_msgs/Twist](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) | The twist command, created via mouse | +| `/carla/world/control` | [carla_msgs/CarlaControl](ros_msgs.md#carlacontrolmsg) | Play/pause/step CARLA | +| `/carla/vehicles/ego_vehicle/spectator_pose` | [geometry_msgs/PoseStamped](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html) | Publish the current pose of the RVIZ camera view | +| `/carla/vehicles/ego_vehicle/vehicle_control_manual_override` | [std_msgs/Bool](https://docs.ros.org/en/api/std_msgs/html/msg/Bool.html) | Enable/disable vehicle control override | +| `/carla/vehicles/ego_vehicle/twist` | [geometry_msgs/Twist](https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html) | The twist command, created via mouse |
diff --git a/pcl_recorder/launch/pcl_recorder.launch b/pcl_recorder/launch/pcl_recorder.launch index 9df6d76c..bb03b287 100644 --- a/pcl_recorder/launch/pcl_recorder.launch +++ b/pcl_recorder/launch/pcl_recorder.launch @@ -6,7 +6,7 @@ + args="pub -l /carla/vehicles/$(arg role_name)/enable_autopilot std_msgs/Bool '{ data: true}' " /> diff --git a/ros_compatibility/src/ros_compatibility/__init__.py b/ros_compatibility/src/ros_compatibility/__init__.py index 34272fe7..fefacb2e 100644 --- a/ros_compatibility/src/ros_compatibility/__init__.py +++ b/ros_compatibility/src/ros_compatibility/__init__.py @@ -72,7 +72,8 @@ def get_package_share_directory(package_name): _shutdown_hooks = [] def init(name, args=None): - rclpy.init(args=args) + if not rclpy.ok(): + rclpy.init(args=args) def ok(): return rclpy.ok() @@ -81,7 +82,8 @@ def shutdown(): global _shutdown_hooks for h in _shutdown_hooks: h() - rclpy.shutdown() + if ok(): + rclpy.shutdown() def _add_shutdown_hook(hook): if not callable(hook): diff --git a/ros_compatibility/src/ros_compatibility/node.py b/ros_compatibility/src/ros_compatibility/node.py index f80e8536..b753729f 100644 --- a/ros_compatibility/src/ros_compatibility/node.py +++ b/ros_compatibility/src/ros_compatibility/node.py @@ -11,7 +11,7 @@ from ros_compatibility.core import get_ros_version from ros_compatibility.exceptions import * -ROS_VERSION = get_ros_version() +ROS_VERSION = get_ros_version() if ROS_VERSION == 1: @@ -19,11 +19,10 @@ from ros_compatibility.logging import logdebug, loginfo, logwarn, logerr, logfatal - class CompatibleNode(object): def __init__(self, name, **kwargs): - pass + pass def get_param(self, name, alternative_value=None): if name.startswith('/'): @@ -50,13 +49,15 @@ def logfatal(self, msg): def new_publisher(self, msg_type, topic, qos_profile, callback_group=None): if isinstance(qos_profile, int): - qos_profile = ros_compatibility.qos.QoSProfile(depth=qos_profile) + qos_profile = ros_compatibility.qos.QoSProfile( + depth=qos_profile) return rospy.Publisher(topic, msg_type, queue_size=qos_profile.depth, latch=qos_profile.is_latched()) def new_subscription(self, msg_type, topic, callback, qos_profile, callback_group=None): if isinstance(qos_profile, int): - qos_profile = ros_compatibility.qos.QoSProfile(depth=qos_profile) + qos_profile = ros_compatibility.qos.QoSProfile( + depth=qos_profile) return rospy.Subscriber(topic, msg_type, callback, queue_size=qos_profile.depth) @@ -122,17 +123,22 @@ def destroy(self): ros_compatibility.qos.DurabilityPolicy.TRANSIENT_LOCAL: rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, ros_compatibility.qos.DurabilityPolicy.VOLATILE: rclpy.qos.DurabilityPolicy.VOLATILE } - - + _RELIABILITY_POLICY_MAP = { + ros_compatibility.qos.ReliabilityPolicy.BEST_EFFORT: rclpy.qos.ReliabilityPolicy.BEST_EFFORT, + ros_compatibility.qos.ReliabilityPolicy.RELIABLE: rclpy.qos.ReliabilityPolicy.RELIABLE + } + def _get_rclpy_qos_profile(qos_profile): return rclpy.qos.QoSProfile( depth=qos_profile.depth, - durability=_DURABILITY_POLICY_MAP[qos_profile.durability] + durability=_DURABILITY_POLICY_MAP[qos_profile.durability], + reliability=_RELIABILITY_POLICY_MAP[qos_profile.reliability] ) - class CompatibleNode(Node): + created_publisher = {} + def __init__(self, name, **kwargs): param = Parameter("use_sim_time", Parameter.Type.BOOL, True) super(CompatibleNode, self).__init__( @@ -167,18 +173,58 @@ def logerr(self, text): def logfatal(self, text): self.get_logger().fatal(text) + def publisher_topic_conflict_resolver(self, msg_type, topic): + index = 1 + test_topic = topic + while test_topic in CompatibleNode.created_publisher: + test_topic = topic + "_{}".format(index) + index = index + 1 + return test_topic + def new_publisher(self, msg_type, topic, qos_profile, callback_group=None): if isinstance(qos_profile, int): - qos_profile = ros_compatibility.qos.QoSProfile(depth=qos_profile) + qos_profile = ros_compatibility.qos.QoSProfile( + depth=qos_profile) rclpy_qos_profile = _get_rclpy_qos_profile(qos_profile) + publish_counter = 0 + if topic in CompatibleNode.created_publisher: + published_msg_type, publish_counter = CompatibleNode.created_publisher[topic] + if published_msg_type != msg_type: + resolved_topic_name = self.publisher_topic_conflict_resolver( + msg_type=msg_type, topic=topic) + if resolved_topic_name == topic: + raise ROSException("Unable to create publisher with type {} for topic {} because another publisher already was created for type {}. Reimplement publisher_topic_conflict_resolver() function to be able to provide an alternative topic name in case of a conflict.".format( + msg_type, topic, published_msg_type)) + else: + self.logwarn("Unable to create publisher with type {} for topic {} because another publisher already was created for type {}. Using alternative topic name {} instead.".format( + msg_type, topic, published_msg_type, resolved_topic_name)) + topic = resolved_topic_name + publish_counter = 0 + publish_counter = publish_counter + 1 + CompatibleNode.created_publisher[topic] = ( + msg_type, publish_counter) + return self.create_publisher( msg_type, topic, rclpy_qos_profile, callback_group=callback_group) + def destroy_publisher(self, publisher): + if publisher.topic in CompatibleNode.created_publisher: + published_msg_type, publish_counter = CompatibleNode.created_publisher[ + publisher.topic] + if publish_counter > 1: + publish_counter = publish_counter - 1 + CompatibleNode.created_publisher[publisher.topic] = ( + published_msg_type, publish_counter) + else: + CompatibleNode.created_publisher.pop(publisher.topic, None) + super(CompatibleNode, self).destroy_publisher(publisher) + def new_subscription(self, msg_type, topic, callback, qos_profile, callback_group=None): if isinstance(qos_profile, int): - qos_profile = ros_compatibility.qos.QoSProfile(depth=qos_profile) + qos_profile = ros_compatibility.qos.QoSProfile( + depth=qos_profile) rclpy_qos_profile = _get_rclpy_qos_profile(qos_profile) return self.create_subscription( @@ -196,7 +242,7 @@ def wait_for_message(self, topic, msg_type, timeout=None, qos_profile=1): """ Wait for one message from topic. This will create a new subcription to the topic, receive one message, then unsubscribe. - + Do not call this method in a callback or a deadlock may occur. """ s = None @@ -207,7 +253,8 @@ def wait_for_message(self, topic, msg_type, timeout=None, qos_profile=1): topic, lambda msg: future.set_result(msg), qos_profile=qos_profile) - rclpy.spin_until_future_complete(self, future, self.executor, timeout) + rclpy.spin_until_future_complete( + self, future, self.executor, timeout) finally: if s is not None: self.destroy_subscription(s) @@ -223,25 +270,28 @@ def new_client(self, srv_type, srv_name, timeout_sec=None, callback_group=None): srv_type, srv_name, callback_group=callback_group) ready = client.wait_for_service(timeout_sec=timeout_sec) if not ready: - raise ROSException("Timeout of {}sec while waiting for service".format(timeout_sec)) + raise ROSException( + "Timeout of {}sec while waiting for service".format(timeout_sec)) return client - def call_service(self, client, req, timeout=None, spin_until_response_received=False): + def call_service(self, client, req, timeout=None, spin_until_response_received=False, ignore_future_done=False): if not spin_until_response_received: response = client.call(req) return response else: future = client.call_async(req) - rclpy.spin_until_future_complete(self, future, self.executor, timeout) + rclpy.spin_until_future_complete( + self, future, self.executor, timeout) - if future.done(): + if ignore_future_done or future.done(): return future.result() else: if timeout is not None: raise ServiceException( 'Service did not return a response before timeout {}'.format(timeout)) else: - raise ServiceException('Service did not return a response') + raise ServiceException( + 'Service did not return a response') def spin(self): rclpy.spin(self, self.executor) diff --git a/ros_compatibility/src/ros_compatibility/qos.py b/ros_compatibility/src/ros_compatibility/qos.py index 1dfbd977..9f7dbb1f 100644 --- a/ros_compatibility/src/ros_compatibility/qos.py +++ b/ros_compatibility/src/ros_compatibility/qos.py @@ -15,11 +15,26 @@ class DurabilityPolicy(enum.Enum): TRANSIENT_LOCAL = 1 VOLATILE = 2 +class ReliabilityPolicy(enum.Enum): + + RELIABLE = 1 + BEST_EFFORT = 2 + class QoSProfile(object): - def __init__(self, depth, durability=DurabilityPolicy.VOLATILE): + def __init__(self, depth, durability=DurabilityPolicy.VOLATILE, reliability=ReliabilityPolicy.RELIABLE): self.depth = depth self.durability = durability + self.reliability = reliability def is_latched(self): return self.durability == DurabilityPolicy.TRANSIENT_LOCAL + +class QoSProfilePublisher(QoSProfile): + def __init__(self, depth, durability=DurabilityPolicy.TRANSIENT_LOCAL, reliability=ReliabilityPolicy.RELIABLE): + super(QoSProfilePublisher, self).__init__(depth, durability, reliability) + +class QoSProfileSubscriber(QoSProfile): + def __init__(self, depth, durability=DurabilityPolicy.VOLATILE, reliability=ReliabilityPolicy.BEST_EFFORT): + super(QoSProfileSubscriber, self).__init__(depth, durability, reliability) + diff --git a/rviz_carla_plugin/CMakeLists.txt b/rviz_carla_plugin/CMakeLists.txt index 83904edb..57455f8d 100644 --- a/rviz_carla_plugin/CMakeLists.txt +++ b/rviz_carla_plugin/CMakeLists.txt @@ -49,6 +49,9 @@ elseif(${ROS_VERSION} EQUAL 2) find_package(rviz_common REQUIRED) find_package(carla_msgs REQUIRED) find_package(nav_msgs COMPONENTS) + find_package(geometry_msgs REQUIRED) + find_package(tf2 REQUIRED) + find_package(tf2_geometry_msgs REQUIRED) find_package(carla_ros_scenario_runner_types COMPONENTS) find_package(pluginlib REQUIRED) find_package(rviz_ogre_vendor REQUIRED) @@ -73,7 +76,7 @@ elseif(${ROS_VERSION} EQUAL 2) pluginlib_export_plugin_description_file(rviz_common plugin_description_ros2.xml) - ament_target_dependencies(rviz_carla_plugin rclcpp carla_msgs nav_msgs + ament_target_dependencies(rviz_carla_plugin rclcpp carla_msgs nav_msgs geometry_msgs tf2_geometry_msgs tf2 carla_ros_scenario_runner_types rviz_common) ament_export_libraries(${PROJECT_NAME}) diff --git a/rviz_carla_plugin/package.xml b/rviz_carla_plugin/package.xml index 32f993ac..f6109055 100644 --- a/rviz_carla_plugin/package.xml +++ b/rviz_carla_plugin/package.xml @@ -18,12 +18,11 @@ qtbase5-dev carla_msgs - nav_msgs - geometry_msgs - carla_ros_scenario_runner_types - nav_msgs - geometry_msgs - carla_ros_scenario_runner_types + nav_msgs + geometry_msgs + tf2 + tf2_geometry_msgs + carla_ros_scenario_runner_types libqt5-core libqt5-gui libqt5-widgets diff --git a/rviz_carla_plugin/src/carla_control_panel.cpp b/rviz_carla_plugin/src/carla_control_panel.cpp index 8dea95e4..50c192b9 100644 --- a/rviz_carla_plugin/src/carla_control_panel.cpp +++ b/rviz_carla_plugin/src/carla_control_panel.cpp @@ -132,15 +132,15 @@ CarlaControlPanel::CarlaControlPanel(QWidget *parent) mCarlaStatusSubscriber = mNodeHandle.subscribe("/carla/status", 1000, &CarlaControlPanel::carlaStatusChanged, this); mCarlaControlPublisher = mNodeHandle.advertise("/carla/control", 10); - mEgoVehicleStatusSubscriber = mNodeHandle.subscribe( - "/carla/ego_vehicle/vehicle_status", 1000, &CarlaControlPanel::egoVehicleStatusChanged, this); - mEgoVehicleOdometrySubscriber - = mNodeHandle.subscribe("/carla/ego_vehicle/odometry", 1000, &CarlaControlPanel::egoVehicleOdometryChanged, this); + mVehicleStatusSubscriber = mNodeHandle.subscribe( + "/carla/ego_vehicle/vehicle_status", 1000, &CarlaControlPanel::vehicleStatusChanged, this); + mVehicleOdometrySubscriber + = mNodeHandle.subscribe("/carla/ego_vehicle/odometry", 1000, &CarlaControlPanel::vehicleOdometryChanged, this); mCameraPosePublisher = mNodeHandle.advertise("carla/ego_vehicle/spectator_pose", 10, true); - mEgoVehicleControlManualOverridePublisher + mVehicleControlManualOverridePublisher = mNodeHandle.advertise("/carla/ego_vehicle/vehicle_control_manual_override", 1, true); mExecuteScenarioClient @@ -243,7 +243,7 @@ void CarlaControlPanel::overrideVehicleControl(int state) boolMsg.data = false; mDriveWidget->setEnabled(false); } - mEgoVehicleControlManualOverridePublisher.publish(boolMsg); + mVehicleControlManualOverridePublisher.publish(boolMsg); } void CarlaControlPanel::scenarioRunnerStatusChanged( @@ -297,7 +297,7 @@ void CarlaControlPanel::carlaScenariosChanged(const carla_ros_scenario_runner_ty setScenarioRunnerStatus(mScenarioSelection->count() > 0); } -void CarlaControlPanel::egoVehicleStatusChanged(const carla_msgs::CarlaEgoVehicleStatus::ConstPtr &msg) +void CarlaControlPanel::vehicleStatusChanged(const carla_msgs::CarlaVehicleStatus::ConstPtr &msg) { mOverrideVehicleControl->setEnabled(true); mSteerBar->setValue(msg->control.steer * 100); @@ -309,7 +309,7 @@ void CarlaControlPanel::egoVehicleStatusChanged(const carla_msgs::CarlaEgoVehicl mSpeedLabel->setText(speedStream.str().c_str()); } -void CarlaControlPanel::egoVehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg) +void CarlaControlPanel::vehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg) { std::stringstream posStream; posStream << std::fixed << std::setprecision(2) << msg->pose.pose.position.x << ", " << msg->pose.pose.position.y; diff --git a/rviz_carla_plugin/src/carla_control_panel.h b/rviz_carla_plugin/src/carla_control_panel.h index 646b96dd..7cffc863 100644 --- a/rviz_carla_plugin/src/carla_control_panel.h +++ b/rviz_carla_plugin/src/carla_control_panel.h @@ -7,7 +7,7 @@ #pragma once #include -#include +#include #include #include #include @@ -60,8 +60,8 @@ protected Q_SLOTS: void scenarioRunnerStatusChanged(const carla_ros_scenario_runner_types::CarlaScenarioRunnerStatus::ConstPtr &msg); void carlaStatusChanged(const carla_msgs::CarlaStatus::ConstPtr &msg); - void egoVehicleStatusChanged(const carla_msgs::CarlaEgoVehicleStatus::ConstPtr &msg); - void egoVehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg); + void vehicleStatusChanged(const carla_msgs::CarlaVehicleStatus::ConstPtr &msg); + void vehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg); void carlaScenariosChanged(const carla_ros_scenario_runner_types::CarlaScenarioList::ConstPtr &msg); carla_msgs::CarlaStatus::ConstPtr mCarlaStatus{nullptr}; @@ -80,10 +80,10 @@ protected Q_SLOTS: IndicatorWidget *mIndicatorWidget; ros::Publisher mTwistPublisher; ros::Publisher mCarlaControlPublisher; - ros::Publisher mEgoVehicleControlManualOverridePublisher; + ros::Publisher mVehicleControlManualOverridePublisher; ros::Subscriber mCarlaStatusSubscriber; - ros::Subscriber mEgoVehicleStatusSubscriber; - ros::Subscriber mEgoVehicleOdometrySubscriber; + ros::Subscriber mVehicleStatusSubscriber; + ros::Subscriber mVehicleOdometrySubscriber; ros::ServiceClient mExecuteScenarioClient; ros::Subscriber mScenarioSubscriber; ros::Subscriber mScenarioRunnerStatusSubscriber; diff --git a/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp b/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp index a9dc0b98..24063090 100644 --- a/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp +++ b/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp @@ -43,6 +43,11 @@ CarlaControlPanel::CarlaControlPanel(QWidget *parent) QVBoxLayout *layout = new QVBoxLayout; QHBoxLayout *vehicleLayout = new QHBoxLayout; + QVBoxLayout *egoSelectionLayout = new QVBoxLayout; + mEgoVehileSelection = new QComboBox(); + egoSelectionLayout->addWidget(mEgoVehileSelection, 10); + layout->addLayout(egoSelectionLayout); + QFormLayout *egoCtrlStatusLayout = new QFormLayout; mThrottleBar = new QProgressBar(); @@ -174,35 +179,64 @@ void CarlaControlPanel::onInitialize() connect(getDisplayContext()->getViewManager(), SIGNAL(currentChanged()), this, SLOT(currentViewControllerChanged())); _node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - // set up ros subscriber and publishers - mCarlaStatusSubscriber = _node->create_subscription("/carla/status", 1000, std::bind(&CarlaControlPanel::carlaStatusChanged, this, _1)); - mCarlaControlPublisher = _node->create_publisher("/carla/control", 10); - mEgoVehicleStatusSubscriber - = _node->create_subscription("/carla/ego_vehicle/vehicle_status", 1000, std::bind(&CarlaControlPanel::egoVehicleStatusChanged, this, _1)); - mEgoVehicleOdometrySubscriber - = _node->create_subscription("/carla/ego_vehicle/odometry", 1000, std::bind(&CarlaControlPanel::egoVehicleOdometryChanged, this, _1)); - - auto qos_latch_10 = rclcpp::QoS( rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10)); - qos_latch_10.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - mCameraPosePublisher - = _node->create_publisher("/carla/ego_vehicle/spectator_pose", qos_latch_10); + connect(mEgoVehileSelection, SIGNAL(currentTextChanged(const QString &)), this, SLOT( onConnectToVehicle(const QString &) )); - auto qos_latch_1 = rclcpp::QoS( rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1)); - qos_latch_1.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - mEgoVehicleControlManualOverridePublisher - = _node->create_publisher("/carla/ego_vehicle/vehicle_control_manual_override", qos_latch_1); + auto empty_actor_list = std::make_shared(); + carlaActorListChanged(empty_actor_list); + mCarlaStatusSubscriber = _node->create_subscription("/carla/world/status", rclcpp::SensorDataQoS().keep_last(10).transient_local(), std::bind(&CarlaControlPanel::carlaStatusChanged, this, _1)); + mCarlaActorListSubscriber = _node->create_subscription("/carla/world/actor_list", rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1)).transient_local(), std::bind(&CarlaControlPanel::carlaActorListChanged, this, _1)); + mCarlaControlPublisher = _node->create_publisher("/carla/world/control/carla_control", 10); mExecuteScenarioClient = _node->create_client("/scenario_runner/execute_scenario"); mScenarioRunnerStatusSubscriber = _node->create_subscription("/scenario_runner/status", 10, std::bind(&CarlaControlPanel::scenarioRunnerStatusChanged, this, _1)); - mTwistPublisher = _node->create_publisher("/carla/ego_vehicle/twist", 1); - mScenarioSubscriber = _node->create_subscription("/carla/available_scenarios", 1, std::bind(&CarlaControlPanel::carlaScenariosChanged, this, _1)); } +void CarlaControlPanel::onConnectToVehicle(const QString &vehicleTopicName) +{ + std::string vehicleTopicPrefix = "/carla/vehicles/"; + if ( vehicleTopicName.length()>0u ) + { + vehicleTopicPrefix += vehicleTopicName.toStdString(); + } + else + { + vehicleTopicPrefix += "hero"; + } + + if ( mVehicleTopicPrefix != vehicleTopicPrefix ) + { + mVehicleTopicPrefix = vehicleTopicPrefix; + auto sensorDataQos = rclcpp::SensorDataQoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10)); + mVehicleControlStatusSubscriber + = _node->create_subscription( + vehicleTopicPrefix + "/vehicle_control_status", sensorDataQos, + std::bind(&CarlaControlPanel::vehicleControlStatusChanged, this, _1)); + mVehicleOdometrySubscriber = _node->create_subscription( + vehicleTopicPrefix + "/odometry", sensorDataQos, + std::bind(&CarlaControlPanel::vehicleOdometryChanged, this, _1)); + mVehicleSpeedSubscriber = _node->create_subscription( + vehicleTopicPrefix + "/speed", sensorDataQos, + std::bind(&CarlaControlPanel::vehicleSpeedChanged, this, _1)); + + auto qos_latch_10 = rclcpp::QoS( rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10)); + qos_latch_10.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + mCameraPosePublisher + = _node->create_publisher(vehicleTopicPrefix + "/spectator_pose", qos_latch_10); + + auto qos_latch_1 = rclcpp::QoS( rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1)); + qos_latch_1.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + mVehicleControlManualOverridePublisher + = _node->create_publisher(vehicleTopicPrefix + "/vehicle_control_manual_override", qos_latch_1); + + mTwistPublisher = _node->create_publisher(vehicleTopicPrefix + "/twist", 1); + } +} + void CarlaControlPanel::currentViewControllerChanged() { mViewController @@ -249,7 +283,7 @@ void CarlaControlPanel::overrideVehicleControl(int state) boolMsg.data = false; mDriveWidget->setEnabled(false); } - mEgoVehicleControlManualOverridePublisher->publish(boolMsg); + mVehicleControlManualOverridePublisher->publish(boolMsg); } void CarlaControlPanel::scenarioRunnerStatusChanged( @@ -303,32 +337,66 @@ void CarlaControlPanel::carlaScenariosChanged(const carla_ros_scenario_runner_ty setScenarioRunnerStatus(mScenarioSelection->count() > 0); } -void CarlaControlPanel::egoVehicleStatusChanged(const carla_msgs::msg::CarlaEgoVehicleStatus::SharedPtr msg) +void CarlaControlPanel::carlaActorListChanged(const carla_msgs::msg::CarlaActorList::SharedPtr msg) +{ + auto currentSelection = mEgoVehileSelection->currentText(); + mEgoVehileSelection->clear(); + int idx = 0; + for (auto actorInfo: msg->actors) + { + if ( actorInfo.topic_prefix.find("carla/vehicles/") == 0u ) + { + QString const vehicleTopicName(actorInfo.topic_prefix.substr(15).c_str()); + mEgoVehileSelection->addItem(vehicleTopicName); + if (currentSelection == vehicleTopicName) + { + mEgoVehileSelection->setCurrentIndex(idx); + } + ++idx; + } + } + if (mEgoVehileSelection->count() == 0) + { + mEgoVehileSelection->addItem("hero"); + mEgoVehileSelection->setCurrentIndex(0); + } +} + + +void CarlaControlPanel::vehicleControlStatusChanged(const carla_msgs::msg::CarlaVehicleControlStatus::SharedPtr msg) { mOverrideVehicleControl->setEnabled(true); - mSteerBar->setValue(msg->control.steer * 100); - mThrottleBar->setValue(msg->control.throttle * 100); - mBrakeBar->setValue(msg->control.brake * 100); + mSteerBar->setValue(msg->last_applied_vehicle_control.steer * 100); + mThrottleBar->setValue(msg->last_applied_vehicle_control.throttle * 100); + mBrakeBar->setValue(msg->last_applied_vehicle_control.brake * 100); +} +void CarlaControlPanel::vehicleSpeedChanged(const std_msgs::msg::Float32::SharedPtr msg) +{ std::stringstream speedStream; - speedStream << std::fixed << std::setprecision(2) << msg->velocity * 3.6; + speedStream << std::fixed << std::setprecision(2) << msg->data * 3.6; mSpeedLabel->setText(speedStream.str().c_str()); } -void CarlaControlPanel::egoVehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg) +void CarlaControlPanel::vehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg) { std::stringstream posStream; posStream << std::fixed << std::setprecision(2) << msg->pose.pose.position.x << ", " << msg->pose.pose.position.y; mPosLabel->setText(posStream.str().c_str()); std::stringstream headingStream; - headingStream << std::fixed << std::setprecision(2) << tf2::getYaw(msg->pose.pose.orientation); + auto quaternion = tf2::Quaternion( + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w); + headingStream << std::fixed << std::setprecision(2) << tf2::getYaw(quaternion); mHeadingLabel->setText(headingStream.str().c_str()); } void CarlaControlPanel::setSimulationButtonStatus(bool active) { - if (active) + if (true) { mPlayPauseButton->setDisabled(false); mPlayPauseButton->setToolTip("Play/Pause the CARLA world."); @@ -347,11 +415,11 @@ void CarlaControlPanel::setSimulationButtonStatus(bool active) void CarlaControlPanel::carlaStatusChanged(const carla_msgs::msg::CarlaStatus::SharedPtr msg) { mCarlaStatus = msg; - setSimulationButtonStatus(mCarlaStatus->synchronous_mode); + setSimulationButtonStatus(mCarlaStatus->episode_settings.synchronous_mode); - if (mCarlaStatus->synchronous_mode) + if (mCarlaStatus->episode_settings.synchronous_mode) { - if (mCarlaStatus->synchronous_mode_running) + if (mCarlaStatus->game_running) { QPixmap pixmap(":/icons/pause.png"); QIcon iconPause(pixmap); @@ -378,7 +446,7 @@ void CarlaControlPanel::carlaTogglePlayPause() if (mCarlaStatus) { carla_msgs::msg::CarlaControl ctrl; - if (mCarlaStatus->synchronous_mode_running) + if (mCarlaStatus->game_running) { ctrl.command = carla_msgs::msg::CarlaControl::PAUSE; } diff --git a/rviz_carla_plugin/src/carla_control_panel_ROS2.h b/rviz_carla_plugin/src/carla_control_panel_ROS2.h index 38fd1954..d2da9457 100644 --- a/rviz_carla_plugin/src/carla_control_panel_ROS2.h +++ b/rviz_carla_plugin/src/carla_control_panel_ROS2.h @@ -6,21 +6,22 @@ */ #pragma once -#include -#include -#include +#include #include +#include +#include #include #include +#include #include -#include "rclcpp/rclcpp.hpp" -#include +#include +#include #include -#include -#include -#include +#include + +#include "rviz_common/panel.hpp" #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" -#include +#include "rviz_common/frame_position_tracking_view_controller.hpp" class QLineEdit; class QPushButton; @@ -49,6 +50,7 @@ public Q_SLOTS: protected Q_SLOTS: void sendVel(); + void onConnectToVehicle(const QString &vehicleTopicPrefix); void carlaStepOnce(); void carlaTogglePlayPause(); @@ -62,18 +64,22 @@ protected Q_SLOTS: virtual void cameraPreRenderScene(Ogre::Camera *cam) override; virtual void onInitialize() override; + void setSimulationButtonStatus(bool active); void setScenarioRunnerStatus(bool active); void scenarioRunnerStatusChanged(const carla_ros_scenario_runner_types::msg::CarlaScenarioRunnerStatus::SharedPtr msg); void carlaStatusChanged(const carla_msgs::msg::CarlaStatus::SharedPtr msg); - void egoVehicleStatusChanged(const carla_msgs::msg::CarlaEgoVehicleStatus::SharedPtr msg); - void egoVehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg); + void carlaActorListChanged(const carla_msgs::msg::CarlaActorList::SharedPtr msg); + void vehicleControlStatusChanged(const carla_msgs::msg::CarlaVehicleControlStatus::SharedPtr msg); + void vehicleSpeedChanged(const std_msgs::msg::Float32::SharedPtr msg); + void vehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg); void carlaScenariosChanged(const carla_ros_scenario_runner_types::msg::CarlaScenarioList::SharedPtr msg); carla_msgs::msg::CarlaStatus::SharedPtr mCarlaStatus{nullptr}; rclcpp::Node::SharedPtr _node; + QComboBox *mEgoVehileSelection; DriveWidget *mDriveWidget; QPushButton *mTriggerScenarioButton; QPushButton *mPlayPauseButton; @@ -89,10 +95,12 @@ protected Q_SLOTS: IndicatorWidget *mIndicatorWidget; rclcpp::Publisher::SharedPtr mTwistPublisher; rclcpp::Publisher::SharedPtr mCarlaControlPublisher; - rclcpp::Publisher::SharedPtr mEgoVehicleControlManualOverridePublisher; + rclcpp::Publisher::SharedPtr mVehicleControlManualOverridePublisher; rclcpp::Subscription::SharedPtr mCarlaStatusSubscriber; - rclcpp::Subscription::SharedPtr mEgoVehicleStatusSubscriber; - rclcpp::Subscription::SharedPtr mEgoVehicleOdometrySubscriber; + rclcpp::Subscription::SharedPtr mCarlaActorListSubscriber; + rclcpp::Subscription::SharedPtr mVehicleControlStatusSubscriber; + rclcpp::Subscription::SharedPtr mVehicleOdometrySubscriber; + rclcpp::Subscription::SharedPtr mVehicleSpeedSubscriber; rclcpp::Client::SharedPtr mExecuteScenarioClient; rclcpp::Subscription::SharedPtr mScenarioSubscriber; rclcpp::Subscription::SharedPtr mScenarioRunnerStatusSubscriber; @@ -100,10 +108,10 @@ protected Q_SLOTS: carla_ros_scenario_runner_types::msg::CarlaScenarioList::SharedPtr mCarlaScenarios; - float mLinearVelocity{0.0}; float mAngularVelocity{0.0}; bool mVehicleControlManualOverride{false}; + std::string mVehicleTopicPrefix; rviz_common::FramePositionTrackingViewController *mViewController{nullptr}; Ogre::Vector3 mCameraCurrentPosition; Ogre::Quaternion mCameraCurrentOrientation;