diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d80121b..0337a19 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,11 @@ Changelog ========= +Version 0.6 +=========== +- **New sensors** Added support for new sensors : GPS, Roads sensor, Radar, Ideal radar, Mesh, Powertrain senso +- **New scenarios** + Version 0.5 ========================= - **LIDAR Modes:** This automated LiDAR sensor operates in three modes: Full 360 Degrees Mode for comprehensive, rapid updates around an 'up' vector; LFO Mode for low-frequency, directional readings; and Static Mode for fixed, angle-specific readings, with 360 mode and other settings customizable via flags. diff --git a/LICENSE.txt b/LICENSE.txt index b1e6aa0..5cfacf8 100644 --- a/LICENSE.txt +++ b/LICENSE.txt @@ -1,6 +1,6 @@ The MIT License (MIT) -Copyright (c) 2022 BeamNG GmbH +Copyright (c) 2024, BeamNG GmbH. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/README.md b/README.md index 8ff291d..58f825e 100644 --- a/README.md +++ b/README.md @@ -99,6 +99,7 @@ Running the BeamNG ROS integration requires three individual software components | BeamNG.tech | BeamNGpy | BeamNG ROS Integration | |-------------|----------|------------------------| +| 0.33.3 |1.30 | 0.6 | | 0.32 |1.29 | 0.5 | | 0.31 |1.28 | 0.4.1 | | 0.30 |1.27.1 | 0.4 | @@ -124,4 +125,4 @@ We always welcome user contributions, be sure to check out our [contribution gui [1]: https://github.com/BeamNG/BeamNGpy [8]: https://documentation.beamng.com/ -[9]: https://github.com/BeamNG/BeamNG-ROS-Integration/blob/master/contributing.md +[9]: https://github.com/BeamNG/BeamNG-ROS-Integration/blob/master/contributing.md \ No newline at end of file diff --git a/beamng_agent/launch/agent.launch b/beamng_agent/launch/agent.launch index 3916d10..dcd32ea 100644 --- a/beamng_agent/launch/agent.launch +++ b/beamng_agent/launch/agent.launch @@ -1,6 +1,6 @@ - + diff --git a/beamng_agent/package.xml b/beamng_agent/package.xml index ef193cc..24558d9 100644 --- a/beamng_agent/package.xml +++ b/beamng_agent/package.xml @@ -1,10 +1,10 @@ beamng_agent - 0.0.0 + 0.6.0 Vehicle input control. - Abdul Rahman Saeed + BeamNG GmbH BeamNG GmbH MIT diff --git a/beamng_control/config/scenarios/GridMap_all.json b/beamng_control/config/scenarios/GridMap_all.json new file mode 100644 index 0000000..92e11b5 --- /dev/null +++ b/beamng_control/config/scenarios/GridMap_all.json @@ -0,0 +1,70 @@ +{ + "version": 0.1, + "level": "GridMap", + "name": "road_network", + "mode": "None", + "network_vizualization": "off", + "vehicles": [ + { + "name": "ego_vehicle", + "model": "etk800", + "position": [0,0,0], + "rotation": [0, 0, 0, 1], + "sensors_classical": [ + { + "name": "electrics0", + "type": "Electrics" + }, + { + "name":"damage0", + "type":"Damage" + }, + { + "name":"gforce0", + "type":"GForces" + }, + { + "name":"time0", + "type":"Timer" + } + ], + "sensors_automation": [ + { + "name": "lidar0", + "type": "Lidar.default", + "position": [0, 0, 1.7], + "rotation": [0, -1, 0], + "up": [0, 0, 1] + }, + { + "name": "front_cam", + "type": "Camera.default", + "position": [-0.3, -2.1, 1], + "rotation": [0, 0, 0] + }, + { + "name": "ultrasonic_right0", + "type":"Ultrasonic.smallrange", + "position":[-1.2, -2.1, 0.0], + "rotation":[-1.0, -1.0, 0.1] + }, + { + "name": "ultrasonic_left0", + "type":"Ultrasonic.smallrange", + "position":[1.2, -2.1, 0.0], + "rotation":[1.0, -1.0, 0.1] + }, + { + "name": "advance_imu", + "type": "AdvancedIMU", + "gfx_update_time": 0.0, + "physics_update_time": 0.01, + "position": [0.5, 0.0, 1.0], + "rotation": [0, -1, 0], + "is_visualised": true + } + ] + } + ] +} + diff --git a/beamng_control/config/scenarios/simple_scenario_pickup.json b/beamng_control/config/scenarios/simple_scenario_pickup.json index 1efb97d..6c88133 100644 --- a/beamng_control/config/scenarios/simple_scenario_pickup.json +++ b/beamng_control/config/scenarios/simple_scenario_pickup.json @@ -10,11 +10,6 @@ "position": [0, 0, 0], "rotation": [0, 0, 0, 1], "sensors_classical": [ - { - "name": "imu0", - "type": "IMU", - "position": [0.73,0.51,0.8] - }, { "name":"damage0", "type":"Damage" @@ -30,16 +25,6 @@ { "name": "electrics0", "type": "Electrics" - }, - { - "name": "node_imu", - "type": "IMU", - "node": 0 - }, - { - "name": "position_imu", - "type": "IMU", - "position": [0.73, 0.51, 0.8] } ], "sensors_automation": [ diff --git a/beamng_control/config/scenarios/smallgrid_all.json b/beamng_control/config/scenarios/smallgrid_all.json index 153f9d7..a87993c 100644 --- a/beamng_control/config/scenarios/smallgrid_all.json +++ b/beamng_control/config/scenarios/smallgrid_all.json @@ -55,13 +55,10 @@ "rotation":[1.0, -1.0, 0.1] }, { - "name": "advance_imu", - "type": "AdvancedIMU", - "gfx_update_time": 0.0, - "physics_update_time": 0.01, + "name": "advanced_imu0", + "type": "AdvancedIMU.default", "position": [0.5, 0.0, 1.0], - "rotation": [0, -1, 0], - "is_visualised": true + "rotation": [0, -1, 0] } ] } diff --git a/beamng_control/config/scenarios/west_coast.json b/beamng_control/config/scenarios/west_coast.json deleted file mode 100644 index d0a390e..0000000 --- a/beamng_control/config/scenarios/west_coast.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "version": 0.1, - "level": "west_coast_usa", - "name": "road_network", - "mode": "None", - "network_vizualization": "on", - "vehicles": [ - { - "name": "ego_vehicle", - "model": "etk800", - "position": [568.908, 13.422, 148.565], - "rotation": [0, 0, 0, 1] - } - ] -} - diff --git a/beamng_control/config/scenarios/west_coast_with_imu.json b/beamng_control/config/scenarios/west_coast_GPS.json similarity index 58% rename from beamng_control/config/scenarios/west_coast_with_imu.json rename to beamng_control/config/scenarios/west_coast_GPS.json index 370994d..9650349 100644 --- a/beamng_control/config/scenarios/west_coast_with_imu.json +++ b/beamng_control/config/scenarios/west_coast_GPS.json @@ -8,13 +8,14 @@ { "name": "ego_vehicle", "model": "etk800", - "position": [568.908, 13.422, 148.565], + "position": [56.977, 49.188, 119.608], "rotation": [0, 0, 0, 1], - "sensors_classical": [ + "sensors_automation": [ { - "name": "position_imu", - "type": "IMU", - "position": [0.73, 0.51, 0.8] + "name": "GPS0", + "type":"GPS", + "rotation": [0, -1, 0], + "position":[0, 0, 1.7] } ] } diff --git a/beamng_control/config/scenarios/west_coast_with_advance_imu.json b/beamng_control/config/scenarios/west_coast_with_advance_imu.json index fd980a9..035102d 100644 --- a/beamng_control/config/scenarios/west_coast_with_advance_imu.json +++ b/beamng_control/config/scenarios/west_coast_with_advance_imu.json @@ -12,13 +12,10 @@ "rotation": [0, 0, 0, 1], "sensors_automation": [ { - "name": "advance_imu", - "type": "AdvancedIMU", - "gfx_update_time": 0.0, - "physics_update_time": 0.01, + "name": "advanced_imu0", + "type": "AdvancedIMU.default", "position": [0.5, 0.0, 1.0], - "rotation": [0, -1, 0], - "is_visualised": true + "rotation": [0, -1, 0] } ] } diff --git a/beamng_control/config/scenarios/west_coast_with_idealradar.json b/beamng_control/config/scenarios/west_coast_with_idealradar.json new file mode 100644 index 0000000..3ad0b88 --- /dev/null +++ b/beamng_control/config/scenarios/west_coast_with_idealradar.json @@ -0,0 +1,24 @@ +{ + "version": 0.1, + "level": "west_coast_usa", + "name": "IdealRadar", + "mode": "None", + "network_vizualization": "on", + "vehicles": [ + { + "name": "ego_vehicle", + "model": "etk800", + "position": [56.977, 49.188, 119.608], + "rotation": [0, 0, 0, 1], + "sensors_automation": [ + { + "name": "ideal_radar0", + "type":"IdealRadar.default", + "position":[0, 0, 0], + "rotation": [0, -1, 0] + } + ] + } + ] +} + diff --git a/beamng_control/config/scenarios/west_coast_with_imu_node.json b/beamng_control/config/scenarios/west_coast_with_imu_node.json deleted file mode 100644 index f63e00b..0000000 --- a/beamng_control/config/scenarios/west_coast_with_imu_node.json +++ /dev/null @@ -1,23 +0,0 @@ -{ - "version": 0.1, - "level": "west_coast_usa", - "name": "road_network", - "mode": "None", - "network_vizualization": "on", - "vehicles": [ - { - "name": "ego_vehicle", - "model": "etk800", - "position": [568.908, 13.422, 148.565], - "rotation": [0, 0, 0, 1], - "sensors_classical": [ - { - "name": "node_imu", - "type": "IMU", - "node": 0 - } - ] - } - ] -} - diff --git a/beamng_control/config/scenarios/west_coast_with_lidar.json b/beamng_control/config/scenarios/west_coast_with_lidar.json index bc031e9..13b3ac2 100644 --- a/beamng_control/config/scenarios/west_coast_with_lidar.json +++ b/beamng_control/config/scenarios/west_coast_with_lidar.json @@ -8,11 +8,11 @@ { "name": "ego_vehicle", "model": "etk800", - "position": [568.908, 13.422, 148.565], + "position": [56.977, 49.188, 119.608], "rotation": [0, 0, 0, 1], "sensors_automation": [ { - "name": "lidar0", + "name": "Lidar0", "type": "Lidar.default", "position": [0, 0, 2.0], "rotation": [0, 0] diff --git a/beamng_control/config/scenarios/west_coast_with_no_sensors.json b/beamng_control/config/scenarios/west_coast_with_no_sensors.json deleted file mode 100644 index d0a390e..0000000 --- a/beamng_control/config/scenarios/west_coast_with_no_sensors.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "version": 0.1, - "level": "west_coast_usa", - "name": "road_network", - "mode": "None", - "network_vizualization": "on", - "vehicles": [ - { - "name": "ego_vehicle", - "model": "etk800", - "position": [568.908, 13.422, 148.565], - "rotation": [0, 0, 0, 1] - } - ] -} - diff --git a/beamng_control/config/scenarios/west_coast_with_radar.json b/beamng_control/config/scenarios/west_coast_with_radar.json new file mode 100644 index 0000000..f05b3d8 --- /dev/null +++ b/beamng_control/config/scenarios/west_coast_with_radar.json @@ -0,0 +1,24 @@ +{ + "version": 0.1, + "level": "west_coast_usa", + "name": "Radar", + "mode": "None", + "network_vizualization": "on", + "vehicles": [ + { + "name": "ego_vehicle", + "model": "etk800", + "position": [56.977, 49.188, 119.608], + "rotation": [0, 0, 0, 1], + "sensors_automation": [ + { + "name": "radar0", + "type":"Radar.default", + "position":[0, 0, 0], + "rotation": [0, -1, 0] + } + ] + } + ] +} + diff --git a/beamng_control/config/scenarios/west_coast_with_two_ultrasonic.json b/beamng_control/config/scenarios/west_coast_with_two_ultrasonic.json new file mode 100644 index 0000000..7bbbaf9 --- /dev/null +++ b/beamng_control/config/scenarios/west_coast_with_two_ultrasonic.json @@ -0,0 +1,30 @@ +{ + "version": 0.1, + "level": "west_coast_usa", + "name": "road_network", + "mode": "None", + "network_vizualization": "on", + "vehicles": [ + { + "name": "ego_vehicle", + "model": "etk800", + "position": [214.908, -211.422, 148.565], + "rotation": [0, 0, 0, 1], + "sensors_automation": [ + { + "name": "ultrasonic_right0", + "type":"Ultrasonic.smallrange", + "position":[-1.2, -2.1, 1.0], + "rotation":[-1.0, -1.0, 0.1] + }, + { + "name": "ultrasonic_left0", + "type":"Ultrasonic.smallrange", + "position":[1.2, -2.1, 1.0], + "rotation":[1.0, -1.0, 0.1] + } + ] + } + ] +} + diff --git a/beamng_control/config/sensors.json b/beamng_control/config/sensors.json index 603b55a..c5445ad 100644 --- a/beamng_control/config/sensors.json +++ b/beamng_control/config/sensors.json @@ -42,21 +42,95 @@ "is_visualised": false, "is_streaming": false, "range_min_cutoff": 0.15, - "range_direct_max_cutoff": 5.0 + "range_direct_max_cutoff": 1000.0 } }, "AdvancedIMU": { - "gfx_update_time": 0.0, - "physics_update_time": 0.01, - "accel_window_width": null, - "gyro_window_width": null, - "accel_frequency_cutoff": null, - "gyro_frequency_cutoff": null, - "is_send_immediately": false, - "is_using_gravity": false, - "is_visualised": true, - "is_snapping_desired": false, - "is_force_inside_triangle": false - } + "default": { + "gfx_update_time": 0.0, + "physics_update_time": 0.01, + "accel_window_width": null, + "gyro_window_width": null, + "accel_frequency_cutoff": null, + "gyro_frequency_cutoff": null, + "is_send_immediately": false, + "is_using_gravity": false, + "is_visualised": true, + "is_snapping_desired": false, + "is_force_inside_triangle": false + } + }, + "GPS": { + "default": { + "gfx_update_time": 0.0, + "physics_update_time": 0.01, + "ref_lon": 0.0, + "ref_lat": 0.0, + "is_send_immediately": false, + "is_visualised": true, + "is_snapping_desired": false, + "is_force_inside_triangle": false, + "is_dir_world_space": false + } + }, + "Mesh": { + "default": { + "gfx_update_time": 0.0, + "physics_update_time": 0.015, + "groups_list":[], + "is_track_beams": true + } + }, + "IdealRadar": { + "default": { + "gfx_update_time": 0.0, + "physics_update_time": 0.01, + "is_send_immediately": false + } + }, + "PowertrainSensor": { + "default": { + "gfx_update_time": 0.0, + "physics_update_time": 0.01, + "is_send_immediately": false + } + }, + "Radar": { + "default": { + "requested_update_time": 0.1, + "update_priority": 0.0, + "range_bins": 200, + "azimuth_bins": 200, + "vel_bins": 200, + "range_min": 0.1, + "range_max": 100.0, + "vel_min": -50.0, + "vel_max": 50.0, + "half_angle_deg": 30.0, + "resolution": [200, 200], + "field_of_view_y": 70, + "near_far_planes": [0.1, 150.0], + "range_roundness": -2.0, + "range_cutoff_sensitivity": 0.0, + "range_shape": 0.23, + "range_focus": 0.12, + "range_min_cutoff": 0.5, + "range_direct_max_cutoff": 150.0, + "is_visualised": true, + "is_streaming": false, + "is_static": false, + "is_snapping_desired": false, + "is_force_inside_triangle": false, + "is_dir_world_space": false + } + }, + "RoadsSensor": { + "default": { + "gfx_update_time": 0.0, + "physics_update_time": 0.01, + "is_send_immediately": false + } + } + } diff --git a/beamng_control/launch/control.launch b/beamng_control/launch/control.launch index 688a8d6..7ca460b 100644 --- a/beamng_control/launch/control.launch +++ b/beamng_control/launch/control.launch @@ -1,6 +1,6 @@ - diff --git a/beamng_control/launch/control_with_teleop.launch b/beamng_control/launch/control_with_teleop.launch index cb6866c..75b3f8c 100644 --- a/beamng_control/launch/control_with_teleop.launch +++ b/beamng_control/launch/control_with_teleop.launch @@ -1,5 +1,6 @@ + diff --git a/beamng_control/launch/control_with_teleop_new.launch b/beamng_control/launch/control_with_teleop_new.launch new file mode 100644 index 0000000..ea333a3 --- /dev/null +++ b/beamng_control/launch/control_with_teleop_new.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/beamng_control/package.xml b/beamng_control/package.xml index 65001ad..5c44f13 100644 --- a/beamng_control/package.xml +++ b/beamng_control/package.xml @@ -1,10 +1,10 @@ beamng_control - 0.0.0 + 0.6.0 Package for BeamNG.tech integration via the BeamNGpy library. - Abdul Rahman Saeed + BeamNG GmbH BeamNG GmbH MIT diff --git a/beamng_control/scripts/beamng_control/bridge.py b/beamng_control/scripts/beamng_control/bridge.py index 58c0914..d1088cb 100644 --- a/beamng_control/scripts/beamng_control/bridge.py +++ b/beamng_control/scripts/beamng_control/bridge.py @@ -241,8 +241,7 @@ def decode_scenario(self, scenario_spec): for v_spec in scenario_spec['vehicles']: # NOT USED IN NEW CODE vehicle = self.get_sensor_classical_from_dict(v_spec) - - # self._publishers.append(VehiclePublisher(vehicle, NODE_NAME)) # todo markers need to be added somwhere else + # self._publishers.append(VehiclePublisher(vehicle, NODE_NAME)) # todo markers need to be added somewhere else # USED IN NEW CODE # vehicle = self.get_sensor_classical_from_dict(v_spec, vehicle) @@ -391,10 +390,7 @@ def get_current_vehicles(self, req): rospy.loginfo(f'vehicles "{vehicles}".') response=True return response - -# # TODO: add vehicle.vid - - + def teleport_vehicle(self, req): diff --git a/beamng_control/scripts/beamng_control/publishers.py b/beamng_control/scripts/beamng_control/publishers.py index b5bdaba..9824fe1 100644 --- a/beamng_control/scripts/beamng_control/publishers.py +++ b/beamng_control/scripts/beamng_control/publishers.py @@ -1,6 +1,7 @@ from abc import ABC, abstractmethod import rospy +from rospy import Time import numpy as np np.float = np.float64 # temp fix for following import @@ -9,26 +10,28 @@ import tf2_ros from cv_bridge import CvBridge, CvBridgeError import numpy as np -from cv_bridge import CvBridge - +from typing import Dict, List, Any, Type import threading -import std_msgs.msg -import sensor_msgs -from sensor_msgs.msg import Range, Imu import geometry_msgs.msg as geom_msgs from geometry_msgs.msg import Point as geom_msgs_Point from visualization_msgs.msg import Marker, MarkerArray -from sensor_msgs.point_cloud2 import PointCloud2 import tf.transformations as tf_transformations from tf.transformations import quaternion_from_euler, quaternion_multiply -from tf import transformations as tf_trans - +from tf import transformations as tf_trans import beamng_msgs.msg as bng_msgs import beamngpy.sensors as bng_sensors +import std_msgs.msg +from sensor_msgs.msg import Range, Imu, NavSatFix, NavSatStatus, Image +from sensor_msgs.point_cloud2 import PointCloud2 +try: + import radar_msgs.msg as radar_msgs + RADAR_MSGS_FOUND = True +except ImportError as e: + RADAR_MSGS_FOUND = False def get_sensor_publisher(sensor): sensor_mapping = { @@ -36,8 +39,16 @@ def get_sensor_publisher(sensor): bng_sensors.Timer: TimerPublisher, bng_sensors.Damage: DamagePublisher, bng_sensors.GForces: GForcePublisher, - bng_sensors.IMU: IMUPublisher, - bng_sensors.Electrics: ElectricsPublisher + bng_sensors.Electrics: ElectricsPublisher, + bng_sensors.IdealRadar: IdealRadarPublisher, + bng_sensors.Radar: RadarPublisher, + bng_sensors.RoadsSensor: RoadsSensorPublisher, + bng_sensors.AdvancedIMU: AdvancedIMUPublisher, + bng_sensors.Mesh: MeshPublisher, + bng_sensors.PowertrainSensor: PowertrainSensorPublisher, + bng_sensors.GPS: GPSPublisher + + # bng_sensors.imu: IMUPublisher, } for k, v in sensor_mapping.items(): if isinstance(sensor, k): @@ -73,6 +84,458 @@ def publish(self, current_time): self._pub.publish(msg) + +class RadarPublisher(SensorDataPublisher): + """ + Radar sensor publisher publishing radar_msgs :radar_msgs:`RadarReturn` messages + if the library is found, custom :external+beamng_msgs:doc:`interfaces/msg/RadarReturn` messages if not. + + You can force the publisher to always send custom BeamNG message type by setting the + ``use_beamng_msg_type=True`` argument in the sensor .json definition. + + Args: + name: Name of the sensor. + config: Arguments of the sensor passed to the BeamNGpy class. + """ + + def __init__(self, sensor, topic_id, vehicle): + """ + Initializes the RadarPublisher class. + + Args: + sensor: The sensor instance being used to poll radar data. + topic_id: The ROS topic on which the radar data will be published. + vehicle: The vehicle object that the sensor is attached to. + """ + super().__init__(sensor, topic_id, bng_msgs.RadarScan) + self.listener = tf.TransformListener() + sensor_name = topic_id.split("/")[-1] + self.frame_Radar_sensor = f'{vehicle.vid}_{sensor_name}' + self.current_time = rospy.get_rostime() + + def _convert_array_to_dict(self, data_array): + """ + Convert the NumPy array from the radar sensor into a dictionary. + Adjust the indices according to your sensor data structure. + """ + # Example: Adjust these indices according to the actual data format + return { + "range": data_array[0], + "doppler_velocity": data_array[1], + "azimuth": data_array[2], + "elevation": data_array[3], + "radar_cross_section": data_array[4], + "signal_to_noise_ratio": data_array[5], + "facing_factor": data_array[6], + } + + def _make_msg(self): + data = self._sensor.poll() + + # Ensure data exists + if data is None: + rospy.logwarn("No data received from Radar sensor.") + return None + + # Handle the case where data is a NumPy array + if isinstance(data, np.ndarray): + # Convert the ndarray to a dictionary format. + data = self._convert_array_to_dict(data) + + # Check if the polled data is a dictionary + if not isinstance(data, dict): + rospy.logwarn(f"Unexpected data type received: {type(data)}") + return None + + # Log raw data for debugging + # rospy.loginfo(f"Raw data from Radar sensor: {data}") + + # Extract data fields, ensuring they are floats + try: + range_msg = float(data.get("range", 0.0)[0]) # Get the first value + doppler_velocity_msg = float(data.get("doppler_velocity", 0.0)[0]) + azimuth_msg = float(data.get("azimuth", 0.0)[0]) + elevation_msg = float(data.get("elevation", 0.0)[0]) + radar_cross_section_msg = float(data.get("radar_cross_section", 0.0)[0]) + signal_to_noise_ratio_msg = float(data.get("signal_to_noise_ratio", 0.0)[0]) + facing_factor_msg = float(data.get("facing_factor", 0.0)[0]) + except Exception as e: + rospy.logwarn(f"Error extracting data fields: {str(e)}") + return None + + # Create a RadarReturn message + radar_return_msg = bng_msgs.RadarReturn( + range=range_msg, + doppler_velocity=doppler_velocity_msg, + azimuth=azimuth_msg, + elevation=elevation_msg, + radar_cross_section=radar_cross_section_msg, + signal_to_noise_ratio=signal_to_noise_ratio_msg, + facing_factor=facing_factor_msg, + ) + + # Ensure the frame ID is correctly set + header = std_msgs.msg.Header( + stamp=self.current_time, + frame_id=self.frame_Radar_sensor # corrected variable name + ) + + # Build and return the RadarScan message + msg = bng_msgs.RadarScan( + header=header, + returns=[radar_return_msg] # wrap in a list + ) + + return msg + + + + + +class RoadsSensorPublisher(SensorDataPublisher): + """ + Roads sensor publisher publishing :external+beamng_msgs:doc:`interfaces/msg/RoadsSensor` messages. + + Args: + name: Name of the sensor. + config: Arguments of the sensor passed to the BeamNGpy class. + """ + def __init__(self, sensor, topic_id, vehicle): + super().__init__(sensor, topic_id, bng_msgs.RoadsSensor) + self.listener = tf.TransformListener() + sensor_name = topic_id.split("/")[-1] + self.frame_Roads_sensor = f'{vehicle.vid}_{sensor_name}' + self.current_time = rospy.get_rostime() + + + + @staticmethod + def _make_cubic_polynomial( + a: float, b: float, c: float, d: float + ) -> bng_msgs.CubicPolynomial: + return bng_msgs.CubicPolynomial(a=a, b=b, c=c, d=d) + + + def xyz_to_point(self, x: float, y: float, z: float) -> geom_msgs.Point: + return geom_msgs.Point(x=x, y=y, z=z) + + def _make_msg(self): + data = self._sensor.poll() + + # Ensure data exists and is in the expected format + if not data: + rospy.logwarn("No data received from RoadsSensor.") + return None + + # Check if the polled data is a dictionary + if not isinstance(data, dict): + rospy.logwarn(f"Unexpected data type received: {type(data)}") + return None + + # Log raw data for debugging + # rospy.loginfo(f"Raw data from RoadsSensor: {data}") + + # Iterate through the data dictionary and handle invalid entries (like floats) + valid_data = {} + for key, value in data.items(): + if isinstance(value, dict): + valid_data[key] = value + else: + rospy.logwarn(f"Invalid data entry for key {key}: Expected dict, got {type(value)}") + + # If no valid data is found, log and return + if not valid_data: + rospy.logwarn("No valid data entries found in RoadsSensor data.") + return None + + # Process the first valid entry (if data is valid) + first_key = next(iter(valid_data)) + data = valid_data[first_key] + + # Extract data fields + dist2_cl = data.get("dist2CL", 0.0) + dist2_left = data.get("dist2Left", 0.0) + dist2_right = data.get("dist2Right", 0.0) + half_width = data.get("halfWidth", 0.0) + road_radius = data.get("roadRadius", 0.0) + heading_angle = data.get("headingAngle", 0.0) + + p0_on_cl = self.xyz_to_point( + data.get("xP0onCL", 0.0), data.get("yP0onCL", 0.0), data.get("zP0onCL", 0.0) + ) + p1_on_cl = self.xyz_to_point( + data.get("xP1onCL", 0.0), data.get("yP1onCL", 0.0), data.get("zP1onCL", 0.0) + ) + p2_on_cl = self.xyz_to_point( + data.get("xP2onCL", 0.0), data.get("yP2onCL", 0.0), data.get("zP2onCL", 0.0) + ) + p3_on_cl = self.xyz_to_point( + data.get("xP3onCL", 0.0), data.get("yP3onCL", 0.0), data.get("zP3onCL", 0.0) + ) + + u_cl = self._make_cubic_polynomial( + data.get("uAofCL", 0.0), data.get("uBofCL", 0.0), data.get("uCofCL", 0.0), data.get("uDofCL", 0.0) + ) + v_cl = self._make_cubic_polynomial( + data.get("vAofCL", 0.0), data.get("vBofCL", 0.0), data.get("vCofCL", 0.0), data.get("vDofCL", 0.0) + ) + + u_left_re = self._make_cubic_polynomial( + data.get("uAofLeftRE", 0.0), data.get("uBofLeftRE", 0.0), data.get("uCofLeftRE", 0.0), data.get("uDofLeftRE", 0.0) + ) + v_left_re = self._make_cubic_polynomial( + data.get("vAofLeftRE", 0.0), data.get("vBofLeftRE", 0.0), data.get("vCofLeftRE", 0.0), data.get("vDofLeftRE", 0.0) + ) + + u_right_re = self._make_cubic_polynomial( + data.get("uAofRightRE", 0.0), data.get("uBofRightRE", 0.0), data.get("uCofRightRE", 0.0), data.get("uDofRightRE", 0.0) + ) + v_right_re = self._make_cubic_polynomial( + data.get("vAofRightRE", 0.0), data.get("vBofRightRE", 0.0), data.get("vCofRightRE", 0.0), data.get("vDofRightRE", 0.0) + ) + + start_cl = self.xyz_to_point( + data.get("xStartCL", 0.0), data.get("yStartCL", 0.0), data.get("zStartCL", 0.0) + ) + start_l = self.xyz_to_point( + data.get("xStartL", 0.0), data.get("yStartL", 0.0), data.get("zStartL", 0.0) + ) + start_r = self.xyz_to_point( + data.get("xStartR", 0.0), data.get("yStartR", 0.0), data.get("zStartR", 0.0) + ) + + drivability = data.get("drivability", 0.0) + speed_limit = data.get("speedLimit", 0.0) + flag1way = data.get("flag1way", 0.0) + + # Log critical fields + # rospy.loginfo(f"dist2_cl: {dist2_cl}, dist2_left: {dist2_left}, dist2_right: {dist2_right}") + # rospy.loginfo(f"half_width: {half_width}, road_radius: {road_radius}, heading_angle: {heading_angle}") + + # Build and return the message + msg = bng_msgs.RoadsSensor( + header=std_msgs.msg.Header( + stamp=self.current_time, + frame_id=self.frame_Roads_sensor + ), + dist2_cl=dist2_cl, + dist2_left=dist2_left, + dist2_right=dist2_right, + half_width=half_width, + road_radius=road_radius, + heading_angle=heading_angle, + p0_on_cl=p0_on_cl, + p1_on_cl=p1_on_cl, + p2_on_cl=p2_on_cl, + p3_on_cl=p3_on_cl, + u_cl=u_cl, + v_cl=v_cl, + u_left_re=u_left_re, + v_left_re=v_left_re, + u_right_re=u_right_re, + v_right_re=v_right_re, + start_cl=start_cl, + start_l=start_l, + start_r=start_r, + drivability=drivability, + speed_limit=speed_limit, + flag1way=flag1way + ) + + return msg + + + + +class IdealRadarPublisher(SensorDataPublisher): + def __init__(self, sensor, topic_id, vehicle): + super().__init__(sensor, topic_id, bng_msgs.IdealRadarSensor) + self.listener = tf.TransformListener() + sensor_name = topic_id.split("/")[-1] + self.frame_IdealRadar_sensor = f'{vehicle.vid}_{sensor_name}' + self.current_time = rospy.get_rostime() + + def xyz_to_vec3(self, x: float, y: float, z: float) -> geom_msgs.Vector3: + return geom_msgs.Vector3(x=x, y=y, z=z) + + def _vehicle_to_msg(self, veh: Dict[str, Any]) -> bng_msgs.IdealRadarSensorVehicle: + # Use default value (Vector3 with zero velocity) if 'vel' key is missing + vel = self.xyz_to_vec3(**veh.get("vel", {"x": 0, "y": 0, "z": 0})) + + return bng_msgs.IdealRadarSensorVehicle( + vehicle_id=int(veh["vehicleID"]), + dist_to_player_vehicle_sq=veh["distToPlayerVehicleSq"], + width=veh["width"], + length=veh["length"], + acc=self.xyz_to_vec3(**veh.get("acc", {"x": 0, "y": 0, "z": 0})), + vel=vel, + rel_acc_x=veh.get("relAccX", 0), + rel_acc_y=veh.get("relAccY", 0), + rel_dist_x=veh.get("relDistX", 0), + rel_dist_y=veh.get("relDistY", 0), + rel_vel_x=veh.get("relVelX", 0), + rel_vel_y=veh.get("relVelY", 0), + ) + + + def _make_msg(self): + data = self._sensor.poll() + if 0.0 in data: # bulk data + data = data[0.0] + msg = bng_msgs.IdealRadarSensor( + header=std_msgs.msg.Header( + stamp=self.current_time, + frame_id=self.frame_IdealRadar_sensor + ), + closest_vehicles1=self._vehicle_to_msg(data["closestVehicles1"]), + closest_vehicles2=self._vehicle_to_msg(data["closestVehicles2"]), + closest_vehicles3=self._vehicle_to_msg(data["closestVehicles3"]), + closest_vehicles4=self._vehicle_to_msg(data["closestVehicles4"]), + ) + return msg + + + +class PowertrainSensorPublisher(SensorDataPublisher): + """ + Powertrain sensor publisher publishing :external+beamng_msgs:doc:`interfaces/msg/PowertrainSensor` messages. + + Args: + name: Name of the sensor. + config: Arguments of the sensor passed to the BeamNGpy class. + """ + + def __init__(self, sensor, topic_id, vehicle): + super().__init__(sensor, topic_id, bng_msgs.PowertrainSensor) + self.listener = tf.TransformListener() + sensor_name = topic_id.split("/")[-1] + self.frame_Powertrain_sensor = f'{vehicle.vid}_{sensor_name}' + self.current_time = rospy.get_rostime() + + + @staticmethod + def _device_to_msg( + name: str, device: Dict[str, Any]) -> bng_msgs.PowertrainSensorDevice: + if isinstance(device, dict): + return bng_msgs.PowertrainSensorDevice( + name=name, + input_av=device["inputAV"], + gear_ratio=device.get("gearRatio", float("nan")), + is_broken=device.get("isBroken", False), + mode=device.get("mode", ""), + parent_name=device.get("parentName", ""), + parent_output_index=int(device.get("parentOutputIndex", -1)), + output_torque_1=device.get("outputTorque1", float("nan")), + output_av_1=device.get("outputAV1", float("nan")), + output_torque_2=device.get("outputTorque2", float("nan")), + output_av_2=device.get("outputAV2", float("nan")), + ) + else: + rospy.logwarn(f"Expected dict for device, got {type(device)}: {device}") + return bng_msgs.PowertrainSensorDevice( + name=name, + input_av=float("nan"), + gear_ratio=float("nan"), + is_broken=False, + mode="", + parent_name="", + parent_output_index=-1, + output_torque_1=float("nan"), + output_av_1=float("nan"), + output_torque_2=float("nan"), + output_av_2=float("nan"), + ) + + + def _make_msg(self): + data = self._sensor.poll() + if 0.0 in data: # bulk data + data = data[0.0] + msg = bng_msgs.PowertrainSensor( + header=std_msgs.msg.Header( + stamp=self.current_time, + frame_id=self.frame_Powertrain_sensor + ), + devices=[ + self._device_to_msg(name, device) for name, device in data.items() + ], + ) + return msg + +class MeshPublisher(SensorDataPublisher): + def __init__(self, sensor, topic_id, vehicle): + super().__init__(sensor, topic_id, bng_msgs.MeshSensor) # Correct message type + self.listener = tf.TransformListener() + sensor_name = topic_id.split("/")[-1] + self.frame_Mesh_sensor = f'{vehicle.vid}_{sensor_name}' + self.current_time = rospy.get_rostime() + + def xyz_to_point(self, x: float, y: float, z: float) -> geom_msgs.Point: + return geom_msgs.Point(x=x, y=y, z=z) + + def xyz_to_vec3(self, x: float, y: float, z: float) -> geom_msgs.Vector3: + return geom_msgs.Vector3(x=x, y=y, z=z) + + @staticmethod + def _beam_to_msg(beam: Dict[str, Any]) -> bng_msgs.MeshSensorBeam: + return bng_msgs.MeshSensorBeam(stress=beam["stress"]) + + def _node_to_msg(self, node: Dict[str, Any]) -> bng_msgs.MeshSensorNode: + return bng_msgs.MeshSensorNode( + part_origin=node.get("partOrigin", ""), + mass=node["mass"], + pos=self.xyz_to_point(**node["pos"]), + vel=self.xyz_to_vec3(**node["vel"]), + force=self.xyz_to_vec3(**node["force"]), + ) + + def _make_msg(self): + data = self._sensor.poll() + msg = bng_msgs.MeshSensor( + header=std_msgs.msg.Header( + stamp=self.current_time, # Correct header with timestamp + frame_id=self.frame_Mesh_sensor + ), + beams=[ + self._beam_to_msg(data["beams"][i]) + for i in range(len(data["beams"])) + ], + nodes=[ + self._node_to_msg(data["nodes"][i]) + for i in range(len(data["nodes"])) + ], + ) + return msg + +class GPSPublisher(SensorDataPublisher): + def __init__(self, sensor, topic_id, vehicle): + super().__init__(sensor, topic_id, NavSatFix) + self.listener = tf.TransformListener() + sensor_name = topic_id.split("/")[-1] + self.frame_Gps_sensor = f'{vehicle.vid}_{sensor_name}' + self.current_time = rospy.get_rostime() + + def _make_msg(self): + data = self._sensor.poll() + if 0.0 in data: + data = data[0.0] + # Ensure the message type matches what the publisher expects + msg = NavSatFix( + header=std_msgs.msg.Header( + stamp=self.current_time, # Correct header with timestamp + frame_id=self.frame_Gps_sensor + ), + status=NavSatStatus( + status=NavSatStatus.STATUS_FIX, + service=NavSatStatus.SERVICE_GPS + ), + latitude=data["lat"], + longitude=data["lon"], + position_covariance_type=NavSatFix.COVARIANCE_TYPE_UNKNOWN, + ) + return msg + + class StatePublisher(SensorDataPublisher): def __init__(self, sensor, topic_id): @@ -150,26 +613,6 @@ def _make_msg(self): return msg -class IMUPublisher(SensorDataPublisher): - - def __init__(self, sensor, topic_id): - super().__init__(sensor, - topic_id, - sensor_msgs.msg.Imu) - - def _make_msg(self): - data = self._sensor.data - msg = sensor_msgs.msg.Imu() - msg.orientation = geom_msgs.Quaternion(0, 0, 0, 0) - msg.orientation_covariance = [-1, ] * 9 - msg.angular_velocity = geom_msgs.Vector3(*[data[x] for x in ['aX', 'aY', 'aZ']]) - msg.angular_velocity_covariance = [-1, ] * 9 - msg.linear_acceleration = geom_msgs.Vector3(*[data[x] for x in ['gX', 'gY', 'gZ']]) - msg.linear_acceleration_covariance = [-1, ] * 9 - return msg - - - class ElectricsPublisher(SensorDataPublisher): def __init__(self, sensor, topic_id): @@ -323,7 +766,7 @@ class ColorImgPublisher(CameraDataPublisher): def __init__(self, sensor, topic_id, cv_helper, data_descriptor): super().__init__(sensor, topic_id, - sensor_msgs.msg.Image) + Image) self._cv_helper = cv_helper self._data_descriptor = data_descriptor @@ -346,7 +789,7 @@ class DepthImgPublisher(CameraDataPublisher): def __init__(self, sensor, topic_id, cv_helper): super().__init__(sensor, topic_id, - sensor_msgs.msg.Image) + Image) self._cv_helper = cv_helper def _make_msg(self, data): @@ -366,7 +809,7 @@ class BBoxImgPublisher(CameraDataPublisher): def __init__(self, sensor, topic_id, cv_helper, vehicle): super().__init__(sensor, topic_id, - sensor_msgs.msg.Image) + Image) self._cv_helper = cv_helper self._vehicle = vehicle self._classes = None @@ -487,36 +930,64 @@ def __init__(self, sensor, topic_id, vehicle): self.frame_ImuSensor_sensor = f'{vehicle.vid}_{sensor_name}' self.current_time = rospy.get_rostime() - def _make_msg(self): - data = self._sensor.poll() - imu_msg = Imu() - imu_msg.header.stamp = self.current_time - imu_msg.header.frame_id = 'advanced_imu_sensor' - imu_msg.orientation.x = data[1]['dirX'][0] - imu_msg.orientation.y = data[1]['dirY'][1] - imu_msg.orientation.z = data[1]['dirZ'][2] - # imu_msg.orientation.w = 1 # Assuming orientation in the format (x, y, z, w) + def _make_msg(self): + data = self._sensor.poll() - imu_msg.angular_velocity.x = data[1]['angVel'][0] - imu_msg.angular_velocity.y = data[1]['angVel'][1] - imu_msg.angular_velocity.z = data[1]['angVel'][2] + # Debugging to check the returned data + # rospy.logdebug(f"Sensor data: {data}") - imu_msg.linear_acceleration.x = data[1]['accRaw'][0] - imu_msg.linear_acceleration.y = data[1]['accRaw'][1] - imu_msg.linear_acceleration.z = data[1]['accRaw'][2] + # Ensure data is not None and is in the expected format + if data is None or len(data) < 2: + rospy.logerr("Invalid sensor data. Skipping IMU message publication.") + return None - return imu_msg + try: + imu_msg = Imu() + imu_msg.header.stamp = self.current_time + imu_msg.header.frame_id = 'advanced_imu_sensor' # Can change to desired frame id + + # Orientation quaternion (make sure it's a full quaternion [x, y, z, w]) + imu_msg.orientation.x = data[1]['dirX'][0] + imu_msg.orientation.y = data[1]['dirY'][1] + imu_msg.orientation.z = data[1]['dirZ'][2] + imu_msg.orientation.w = 1.0 # Set `w` component (assumed to be normalized) + + # Angular velocity + imu_msg.angular_velocity.x = data[1]['angVel'][0] + imu_msg.angular_velocity.y = data[1]['angVel'][1] + imu_msg.angular_velocity.z = data[1]['angVel'][2] + + # Linear acceleration + imu_msg.linear_acceleration.x = data[1]['accRaw'][0] + imu_msg.linear_acceleration.y = data[1]['accRaw'][1] + imu_msg.linear_acceleration.z = data[1]['accRaw'][2] + + # Optionally, you can also assign covariance matrices if applicable + imu_msg.orientation_covariance = np.zeros(9) + imu_msg.angular_velocity_covariance = np.zeros(9) + imu_msg.linear_acceleration_covariance = np.zeros(9) + + return imu_msg + + except KeyError as e: + rospy.logerr(f"KeyError encountered: {e}") + return None + except IndexError as e: + rospy.logerr(f"IndexError encountered: {e}") + return None class LidarPublisher(SensorDataPublisher): def __init__(self, sensor, topic_id, vehicle): - super().__init__(sensor, topic_id, sensor_msgs.msg.PointCloud2) + super().__init__(sensor, topic_id, PointCloud2) self.listener = tf.TransformListener() - # self.frame_lidar_sensor = 'lidar_link' sensor_name = topic_id.split("/")[-1] self.frame_lidar_sensor = f'{vehicle.vid}_{sensor_name}' + + + def _make_msg(self): header = std_msgs.msg.Header() header.frame_id = self.frame_lidar_sensor @@ -528,19 +999,21 @@ def _make_msg(self): num_points = points.shape[0] colours = colours[:num_points] + # Extract intensity + intensities = colours[:, 0] if colours.size > 0 else np.zeros((num_points,)) pointcloud_fields = [('x', np.float32), - ('y', np.float32), - ('z', np.float32), - ('intensity', np.float32)] + ('y', np.float32), + ('z', np.float32), + ('intensity', np.float32)] try: (trans_map, rot_map) = self.listener.lookupTransform(self.frame_map, self.frame_lidar_sensor, header.stamp) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: rospy.logwarn(f'No transform between {self.frame_map} and ' - f'{self.frame_lidar_sensor} available with exception: {e}') + f'{self.frame_lidar_sensor} available with exception: {e}') points = np.zeros((0, 3)) - colours = np.zeros((0,)) + intensities = np.zeros((0,)) # Change here trans_map = np.zeros(3) rot_map = tf_transformations.quaternion_from_euler(0, 0, 0) # Identity rotation @@ -552,7 +1025,8 @@ def _make_msg(self): pointcloud_data['x'] = rotated_points[:, 0] pointcloud_data['y'] = rotated_points[:, 1] pointcloud_data['z'] = rotated_points[:, 2] - pointcloud_data['intensity'] = np.array(colours) + pointcloud_data['intensity'] = intensities + msg = ros_numpy.msgify(PointCloud2, pointcloud_data) msg.header = header return msg @@ -928,3 +1402,23 @@ def publish(self, current_time): self._pub.publish(self._road_network.markers) +#Deprecated +# class IMUPublisher(SensorDataPublisher): + +# def __init__(self, sensor, topic_id): +# super().__init__(sensor, +# topic_id, +# sensor_msgs.msg.Imu) + +# def _make_msg(self): +# data = self._sensor.data +# msg = sensor_msgs.msg.Imu() +# msg.orientation = geom_msgs.Quaternion(0, 0, 0, 0) +# msg.orientation_covariance = [-1, ] * 9 +# msg.angular_velocity = geom_msgs.Vector3(*[data[x] for x in ['aX', 'aY', 'aZ']]) +# msg.angular_velocity_covariance = [-1, ] * 9 +# msg.linear_acceleration = geom_msgs.Vector3(*[data[x] for x in ['gX', 'gY', 'gZ']]) +# msg.linear_acceleration_covariance = [-1, ] * 9 +# return msg + + diff --git a/beamng_control/scripts/beamng_control/sensorHelper.py b/beamng_control/scripts/beamng_control/sensorHelper.py index 4811a9d..3f8d541 100644 --- a/beamng_control/scripts/beamng_control/sensorHelper.py +++ b/beamng_control/scripts/beamng_control/sensorHelper.py @@ -2,7 +2,7 @@ import copy import beamngpy.sensors as bng_sensors -from beamng_control.publishers import LidarPublisher, CameraPublisher, UltrasonicPublisher, AdvancedIMUPublisher +from beamng_control.publishers import LidarPublisher, CameraPublisher, UltrasonicPublisher, AdvancedIMUPublisher, GPSPublisher, MeshPublisher, IdealRadarPublisher, PowertrainSensorPublisher, RadarPublisher, RoadsSensorPublisher # from beamngpy.noise import RandomImageNoise, RandomLIDARNoise @@ -103,6 +103,120 @@ def get_advanced_imu(bng, +def get_gps(bng, + name, + vehicle, + position, + rotation, + **spec): + if 'rotation' in spec: + spec=spec.pop('rotation') + rospy.loginfo('GPS sensor has no rotation') + try: + Gps = bng_sensors.GPS(bng=bng, + name=name, + vehicle=vehicle, + pos=position) + except TypeError as e: + raise SensorSpecificationError(f'Could not get GPS instance. ' + f'Original error message:\n{e}') + return Gps + +def get_ideal_radar(bng, + name, + vehicle, + position, + rotation, + **spec): + # if 'rotation' or 'position' in spec: + # spec=spec.pop('rotation') + # spec=spec.pop('position') + # rospy.loginfo('IdealRadar sensor has no position nor rotation') + try: + ideal_radar = bng_sensors.IdealRadar(bng=bng, + name=name, + vehicle=vehicle) + except TypeError as e: + raise SensorSpecificationError(f'Could not get Idealradar instance. ' + f'Original error message:\n{e}') + return ideal_radar + + +def get_radar(bng, + name, + vehicle, + position, + rotation, + **spec): + # if 'rotation' or 'position' in spec: + # spec=spec.pop('rotation') + # spec=spec.pop('position') + # rospy.loginfo('IdealRadar sensor has no position nor rotation') + try: + radar = bng_sensors.Radar(bng=bng, + name=name, + vehicle=vehicle) + except TypeError as e: + raise SensorSpecificationError(f'Could not get Radar instance. ' + f'Original error message:\n{e}') + return radar + +def get_roads_sensor(bng, name, vehicle, position, **spec): + if 'rotation' in spec: + spec.pop('rotation') + rospy.loginfo('RoadsSensor sensor has no rotation') + if 'position' in spec: + spec.pop('position') + rospy.loginfo('RoadsSensor sensor has no position') + try: + roads_sensor = bng_sensors.RoadsSensor(bng=bng, name=name, vehicle=vehicle) + except TypeError as e: + raise SensorSpecificationError(f'Could not get Roads Sensor instance. ' + f'Original error message:\n{e}') + return roads_sensor + + +def get_power_train(bng, + name, + vehicle, + position, + rotation, + **spec): + # if 'rotation' or 'position' in spec: + # spec=spec.pop('rotation') + # spec=spec.pop('position') + # rospy.loginfo('IdealRadar sensor has no position nor rotation') + try: + Power_train_sensor = bng_sensors.PowertrainSensor(bng=bng, + name=name, + vehicle=vehicle) + except TypeError as e: + raise SensorSpecificationError(f'Could not get Powertrain Sensor instance. ' + f'Original error message:\n{e}') + return Power_train_sensor + +def get_mesh(bng, + name, + vehicle, + position, + rotation, + **spec): + # if 'rotation' or 'position' in spec: + # spec=spec.pop('rotation') + # spec=spec.pop('position') + # rospy.loginfo('mesh sensor has no position nor rotation') + try: + mesh = bng_sensors.Mesh(bng=bng, + name=name, + vehicle=vehicle, + **spec) + except TypeError as e: + raise SensorSpecificationError(f'Could not get GPS instance. ' + f'Original error message:\n{e}') + return mesh + + + def get_camera(name, bng, vehicle, position, rotation, field_of_view_y, resolution, **spec): # if 'bounding_box' in spec: # if spec['bounding_box']: @@ -265,20 +379,29 @@ def get_sensors_automation(sensor_type, all_sensor_defs, bng=None, vehicle=None, return sensor, sensor_publisher _automation_sensors = ['Camera', - 'Lidar', - 'CameraNoise', - 'LidarNoise', - 'AdvancedIMU', - 'Ultrasonic', - 'PowerTrain', # unsure about this one - 'Radar', # unsure about this one - 'meshsensor', # unsure about this one - ] + 'Lidar', + # 'CameraNoise', + # 'LidarNoise', + 'AdvancedIMU', + 'GPS', + 'Ultrasonic', + 'IdealRadar', + 'Radar', + 'RoadsSensor', + 'PowertrainSensor', + 'Mesh' + ] _sensor_automation_type_publisher_getter = { 'Lidar': LidarPublisher, 'Camera': CameraPublisher, 'Ultrasonic': UltrasonicPublisher, - 'AdvancedIMU': AdvancedIMUPublisher + 'AdvancedIMU': AdvancedIMUPublisher, + 'GPS': GPSPublisher, + 'IdealRadar': IdealRadarPublisher, + 'Radar': RadarPublisher, + 'RoadsSensor': RoadsSensorPublisher, + 'PowertrainSensor': PowertrainSensorPublisher, + 'Mesh': MeshPublisher } _sensor_getters = { @@ -286,14 +409,18 @@ def get_sensors_automation(sensor_type, all_sensor_defs, bng=None, vehicle=None, 'Timer': bng_sensors.Timer, 'GForces': bng_sensors.GForces, 'Electrics': bng_sensors.Electrics, - 'IMU': get_imu, - + 'Lidar': get_lidar, 'Camera': get_camera, 'Ultrasonic': get_ultrasonic, - 'Lidar': get_lidar, - 'AdvancedIMU': get_advanced_imu + 'AdvancedIMU': get_advanced_imu, + 'GPS': get_gps, + 'IdealRadar': get_ideal_radar, + 'Radar': get_radar, + 'RoadsSensor': get_roads_sensor, + 'PowertrainSensor': get_power_train, + 'Mesh': get_mesh - # 'Ultrasonic': bng_sensors.Ultrasonic, + # 'IMU': bng_sensors.imu, # 'CameraNoise': get_camera_noise_sensor, # 'LidarNoise': get_lidar_noise_sensor } \ No newline at end of file diff --git a/beamng_msgs/CMakeLists.txt b/beamng_msgs/CMakeLists.txt index 220e14a..ff8aefd 100644 --- a/beamng_msgs/CMakeLists.txt +++ b/beamng_msgs/CMakeLists.txt @@ -1,10 +1,12 @@ cmake_minimum_required(VERSION 3.0.2) project(beamng_msgs) find_package(catkin REQUIRED COMPONENTS -message_generation -std_msgs -actionlib_msgs + std_msgs + geometry_msgs + actionlib_msgs + message_generation ) + add_message_files( FILES DamageSensor.msg @@ -17,6 +19,17 @@ add_message_files( TimeSensor.msg VehicleControl.msg VehicleInfo.msg + MeshSensor.msg + MeshSensorBeam.msg + MeshSensorNode.msg + IdealRadarSensor.msg + IdealRadarSensorVehicle.msg + PowertrainSensor.msg + PowertrainSensorDevice.msg + RadarScan.msg + RadarReturn.msg + RoadsSensor.msg + CubicPolynomial.msg ) add_service_files( DIRECTORY srv @@ -40,13 +53,18 @@ add_action_files( generate_messages( DEPENDENCIES std_msgs + geometry_msgs actionlib_msgs ) + catkin_package( - CATKIN_DEPENDS - std_msgs - actionlib_msgs + CATKIN_DEPENDS + message_runtime + std_msgs + geometry_msgs + actionlib_msgs ) + include_directories( ${catkin_INCLUDE_DIRS} ) diff --git a/beamng_msgs/msg/CubicPolynomial.msg b/beamng_msgs/msg/CubicPolynomial.msg new file mode 100644 index 0000000..981b2ce --- /dev/null +++ b/beamng_msgs/msg/CubicPolynomial.msg @@ -0,0 +1,4 @@ +float64 a +float64 b +float64 c +float64 d \ No newline at end of file diff --git a/beamng_msgs/msg/IdealRadarSensor.msg b/beamng_msgs/msg/IdealRadarSensor.msg new file mode 100644 index 0000000..3c73e92 --- /dev/null +++ b/beamng_msgs/msg/IdealRadarSensor.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +IdealRadarSensorVehicle closest_vehicles1 +IdealRadarSensorVehicle closest_vehicles2 +IdealRadarSensorVehicle closest_vehicles3 +IdealRadarSensorVehicle closest_vehicles4 \ No newline at end of file diff --git a/beamng_msgs/msg/IdealRadarSensorVehicle.msg b/beamng_msgs/msg/IdealRadarSensorVehicle.msg new file mode 100644 index 0000000..5a9b013 --- /dev/null +++ b/beamng_msgs/msg/IdealRadarSensorVehicle.msg @@ -0,0 +1,12 @@ +int32 vehicle_id +float64 dist_to_player_vehicle_sq +float64 width +float64 length +geometry_msgs/Vector3 acc +geometry_msgs/Vector3 vel +float64 rel_acc_x +float64 rel_acc_y +float64 rel_dist_x +float64 rel_dist_y +float64 rel_vel_x +float64 rel_vel_y diff --git a/beamng_msgs/msg/MeshSensor.msg b/beamng_msgs/msg/MeshSensor.msg new file mode 100644 index 0000000..7b991b3 --- /dev/null +++ b/beamng_msgs/msg/MeshSensor.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +MeshSensorBeam[] beams +MeshSensorNode[] nodes \ No newline at end of file diff --git a/beamng_msgs/msg/MeshSensorBeam.msg b/beamng_msgs/msg/MeshSensorBeam.msg new file mode 100644 index 0000000..26ace1a --- /dev/null +++ b/beamng_msgs/msg/MeshSensorBeam.msg @@ -0,0 +1 @@ +float64 stress \ No newline at end of file diff --git a/beamng_msgs/msg/MeshSensorNode.msg b/beamng_msgs/msg/MeshSensorNode.msg new file mode 100644 index 0000000..ba89dcf --- /dev/null +++ b/beamng_msgs/msg/MeshSensorNode.msg @@ -0,0 +1,5 @@ +string part_origin +float64 mass +geometry_msgs/Point pos +geometry_msgs/Vector3 vel +geometry_msgs/Vector3 force \ No newline at end of file diff --git a/beamng_msgs/msg/PowertrainSensor.msg b/beamng_msgs/msg/PowertrainSensor.msg new file mode 100644 index 0000000..5b61c3f --- /dev/null +++ b/beamng_msgs/msg/PowertrainSensor.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +PowertrainSensorDevice[] devices \ No newline at end of file diff --git a/beamng_msgs/msg/PowertrainSensorDevice.msg b/beamng_msgs/msg/PowertrainSensorDevice.msg new file mode 100644 index 0000000..b28d6c0 --- /dev/null +++ b/beamng_msgs/msg/PowertrainSensorDevice.msg @@ -0,0 +1,11 @@ +string name +float64 input_av +float64 gear_ratio +bool is_broken +string mode +string parent_name +int32 parent_output_index +float64 output_torque_1 +float64 output_av_1 +float64 output_torque_2 +float64 output_av_2 \ No newline at end of file diff --git a/beamng_msgs/msg/RadarReturn.msg b/beamng_msgs/msg/RadarReturn.msg new file mode 100644 index 0000000..fa572ae --- /dev/null +++ b/beamng_msgs/msg/RadarReturn.msg @@ -0,0 +1,7 @@ +float32 range +float32 doppler_velocity +float32 azimuth +float32 elevation +float32 radar_cross_section +float32 signal_to_noise_ratio +float32 facing_factor \ No newline at end of file diff --git a/beamng_msgs/msg/RadarScan.msg b/beamng_msgs/msg/RadarScan.msg new file mode 100644 index 0000000..287e32d --- /dev/null +++ b/beamng_msgs/msg/RadarScan.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +RadarReturn[] returns \ No newline at end of file diff --git a/beamng_msgs/msg/RoadsSensor.msg b/beamng_msgs/msg/RoadsSensor.msg new file mode 100644 index 0000000..3ca3026 --- /dev/null +++ b/beamng_msgs/msg/RoadsSensor.msg @@ -0,0 +1,23 @@ +std_msgs/Header header +float64 dist2_cl +float64 dist2_left +float64 dist2_right +float64 half_width +float64 road_radius +float64 heading_angle +geometry_msgs/Point p0_on_cl +geometry_msgs/Point p1_on_cl +geometry_msgs/Point p2_on_cl +geometry_msgs/Point p3_on_cl +CubicPolynomial u_cl +CubicPolynomial v_cl +CubicPolynomial u_left_re +CubicPolynomial v_left_re +CubicPolynomial u_right_re +CubicPolynomial v_right_re +geometry_msgs/Point start_cl +geometry_msgs/Point start_l +geometry_msgs/Point start_r +float64 drivability +float64 speed_limit +float64 flag1way \ No newline at end of file diff --git a/beamng_msgs/package.xml b/beamng_msgs/package.xml index 5c47bb6..7351ed0 100644 --- a/beamng_msgs/package.xml +++ b/beamng_msgs/package.xml @@ -1,25 +1,27 @@ beamng_msgs - 0.0.0 + 0.6.0 The beamng_msgs package. - Abdul Rahman Saeed + BeamNG GmbH BeamNG GmbH - + MIT catkin std_msgs actionlib_msgs + geometry_msgs message_generation + std_msgs + geometry_msgs std_msgs - message_runtime + geometry_msgs actionlib_msgs + message_runtime - - diff --git a/beamng_teleop_keyboard/nodes/converter b/beamng_teleop_keyboard/nodes/converter index d6bf613..302a2c1 100644 --- a/beamng_teleop_keyboard/nodes/converter +++ b/beamng_teleop_keyboard/nodes/converter @@ -7,9 +7,11 @@ from std_msgs.msg import String import subprocess import os import beamngpy as bngpy +import sys from beamng_msgs.msg import VehicleControl + class twist2bng(): def __init__(self): self.BNG_pub = rospy.Publisher('/control', VehicleControl ,queue_size=10) diff --git a/beamng_teleop_keyboard/nodes/teleop_key b/beamng_teleop_keyboard/nodes/teleop_key index 230e23e..1cfa165 100644 --- a/beamng_teleop_keyboard/nodes/teleop_key +++ b/beamng_teleop_keyboard/nodes/teleop_key @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright (c) 2011, Abdul Rahman , Inc. +# Copyright (c) 2024, BeamNG GmbH. # All rights reserved. import rospy diff --git a/beamng_teleop_keyboard/nodes/twist_converter.py b/beamng_teleop_keyboard/nodes/twist_converter.py deleted file mode 100644 index f83bd4c..0000000 --- a/beamng_teleop_keyboard/nodes/twist_converter.py +++ /dev/null @@ -1,86 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -from geometry_msgs.msg import Twist -import sys -import rospy -import json -import beamngpy as bngpy -import beamng_msgs.msg as bng_msgs - -from pathlib import Path -from distutils.version import LooseVersion - -MIN_BNG_VERSION_REQUIRED = '0.31.0' -NODE_NAME = 'beamng_agent' - -class VehicleControl(object): - - def __init__(self, vehicle_id): - params = rospy.get_param("beamng") - self.game_client = bngpy.BeamNGpy(params['host'], params['port']) - # self.game_client = bngpy.BeamNGpy(params['host'], params['port'], remote=True) - - try: - # self.game_client.open(launch=False, deploy=False) - self.game_client.open(listen_ip='*',launch=False, deploy=False) - rospy.loginfo("Successfully connected to BeamNG.tech.") - except TimeoutError: - rospy.logerr("Could not establish game connection, check whether BeamNG.tech is running.") - sys.exit(1) - - current_vehicles = self.game_client.get_current_vehicles() - - ass_msg = f"no vehicle with id {vehicle_id} exists" - assert vehicle_id in current_vehicles.keys(), ass_msg - self.vehicle_client = current_vehicles[vehicle_id] - try: - self.vehicle_client.connect(self.game_client) - vid = self.vehicle_client.vid - rospy.loginfo(f'Successfully connected to vehicle client with id {vid}') - except TimeoutError: - rospy.logfatal("Could not establish vehicle connection, system exit.") - sys.exit(1) - - control_topic = 'control' - rospy.Subscriber(control_topic, bng_msgs.VehicleControl, lambda x: self.send_control_signal(x)) - rospy.logdebug(f'subscribing to "{control_topic}" for vehicle control') - rospy.on_shutdown(lambda: self.on_shutdown()) - - def on_shutdown(self): - self.vehicle_client.disconnect() - self.game_client.disconnect() - node_name = rospy.get_name() - rospy.logdebug(f'Shutting down node "{node_name}"') - - def send_control_signal(self, signal): - self.vehicle_client.control(steering=signal.steering, - throttle=signal.throttle, - brake=signal.brake, - parkingbrake=signal.parkingbrake, - clutch=signal.clutch, - gear=signal.gear - ) - - -def main(): - rospy.init_node(NODE_NAME, anonymous=True, log_level=rospy.DEBUG) - node_name = rospy.get_name() - rospy.logdebug(f'started node "{node_name}"') - - available_version = bngpy.__version__ - if LooseVersion(MIN_BNG_VERSION_REQUIRED) > LooseVersion(available_version): - rospy.logfatal(f'This package requires the beamngpy lib to have version {MIN_BNG_VERSION_REQUIRED}, but the available version is {available_version}, aborting process.') - sys.exit(1) - - argv = rospy.myargv(argv=sys.argv) - if len(argv)==2: - VehicleControl(argv[1]) - - else: - rospy.logerr("No Vehicle ID given, shutting down node.") - - rospy.spin() - -if __name__ == "__main__": - main() diff --git a/beamng_teleop_keyboard/package.xml b/beamng_teleop_keyboard/package.xml index eaa17fa..30823ca 100644 --- a/beamng_teleop_keyboard/package.xml +++ b/beamng_teleop_keyboard/package.xml @@ -1,12 +1,12 @@ beamng_teleop_keyboard - 1.2.1 + 0.6.0 Provides teleoperation using keyboard to move the vehicles in BemaNG simulation using geometry messages MIT - Abdul Rahman Saeed + BeamNG GmbH BeamNG GmbH catkin diff --git a/docs/source/conf.py b/docs/source/conf.py index 3a5c178..7f51bbb 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -17,9 +17,9 @@ # -- Project information ----------------------------------------------------- -project = 'BeamNG.tech ROS Integration' -copyright = '2021, BeamNG GmbH' -author = 'BeamNG GmbH' +project = "BeamNG-ROS-Integration" +copyright = "2024, BeamNG GmbH" +author = "BeamNG GmbH" # -- General configuration ---------------------------------------------------