Updating automation sensors and tf frame ground work (#30)

* updated the lidar arg names and added bng to get lidar
pepped it

* updated the lidar publisher to correctly poll the sensor and pack the pointCloud

* added get sensor publisher to bridge so we can grab the automation sensors
change sesnor into two types, classical and automation since they need to be handled differently and this seemed to be the least effort way to address this
updated the sensors key in the west_coast_with_lidar.json example only - need to do the rest

* created dyn_spec

* fixed bug preventing lidar being published

* aded tf frame to vehicle but somethings not right

* updated the tf frame

* added intensity channel

* added publisher threading to get better ros hz

* removed threading for now, for some reason I cant get all the topics to publsh when threading

* moved the transformstamped for the vehicle state to the constructor

* added a static transform for the lidar sensor. lidar points are in world coordinates though

* added a static transform for the lidar sensor. lidar points are in world coordinates though

* fed through the same time to all nodes for publishing. this means the transform lookups will work when driving around

* addeda rviz window for the example.launch

* added threads to the vehicle publisher for a modest 1 fps gain

* updated the sesnsor field in all the scenarios to reflect the new sensors_classical and sensors_automation fields

* added a tf listeer to the lidar poinntcloud so we can convert to ego centric lidar data

* removed unused imports
updated frame names in exception message

* updated camera fields for the constructor

* comminted the updates to sensor.json for the camera to work
set is_visualised to off for in game lidar

* updated sensor json

* addd rviz

* image sensors now have their correct data pulled - but instance dont work still

* camera publisher now ony polls one per step this makes it faster
if instance is required we must use full poll which isnt fast

* change default cam position for all configs with cam

* reupdated pos of cam - had my coordinate system wrong

* updated the bbox publisher for when booxes work in future

* added bboxes

* Added environment variable for Simulator IP

* added static tf frame broadcaster that is populated by the scenario config

* update the marker timestams so the road pdates when visualizing the ego frame

* update the marker timestams so the road pdates when visualizing the ego frame

* added camera vector rotations for pitch and yaw (cant seem to get the rol to work setting up seems to break things)

* set up to be (0,0,1)

* added the rotation to the lidar
change the neame from rotate camera to rotate direction vector

* updated all the automation sensors in the configs

* fixed a bug

* yaw works

* pitch works
updated the rotation for the lidar and imu
removed unneeded function

* removed lidar rtation update since there is no update dir func

* added a rotation to align the yawing of the vehicle with the road network. something is not quite correct tho becase the lidar is upsidown and proceesds in reverse?

* updated rviz

---------

Co-authored-by: Stefan Podgorski <[email protected]>
Co-authored-by: James <[email protected]>

beamng_control/config/rviz/example.rviz

@@ -0,0 +1,279 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+        - /TF1/Frames1
+        - /TF1/Tree1
+        - /MarkerArray1
+      Splitter Ratio: 0.5
+    Tree Height: 304
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        ego_vehicle:
+          Value: true
+        ego_vehicle_front_cam_left:
+          Value: true
+        ego_vehicle_front_cam_right:
+          Value: true
+        ego_vehicle_lidar0:
+          Value: true
+        map:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          ego_vehicle:
+            ego_vehicle_front_cam_left:
+              {}
+            ego_vehicle_front_cam_right:
+              {}
+            ego_vehicle_lidar0:
+              {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: false
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: true
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.05000000074505806
+      Style: Flat Squares
+      Topic: /beamng_control/ego_vehicle/lidar0
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /beamng_control/road_network
+      Name: MarkerArray
+      Namespaces:
+        beamng_control: true
+      Queue Size: 1
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /beamng_control/ego_vehicle/front_cam_left/colour
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: colour_left
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /beamng_control/ego_vehicle/front_cam_right/colour
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: colour_right
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /beamng_control/ego_vehicle/front_cam_left/depth
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: depth_left
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /beamng_control/ego_vehicle/front_cam_left/annotation
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: annotation_left
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /beamng_control/ego_vehicle/front_cam_left/instance
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: instance_left
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /beamng_control/ego_vehicle/front_cam_left/bounding_box
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: bbox_left
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: ego_vehicle
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 69.4802017211914
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: true
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6547950506210327
+      Target Frame: <Fixed Frame>
+      Yaw: 4.299472332000732
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27
+  annotation_left:
+    collapsed: false
+  bbox_left:
+    collapsed: false
+  colour_left:
+    collapsed: false
+  colour_right:
+    collapsed: false
+  depth_left:
+    collapsed: false
+  instance_left:
+    collapsed: false

beamng_control/config/scenarios/.json

@@ -10,7 +10,7 @@
       "model": "etk800",
       "position": [568.908, 13.422,  148.565],
       "rotation": [0, 0, 0, 1],
-      "sensors": [
+      "sensors_classical": [
         {
           "name":"damage0",
           "type":"Damage"
@@ -22,28 +22,11 @@
         {
           "name":"time0",
           "type":"Timer"
-        }, 
-        {
-          "name": "front_cam",
-          "type": "Camera.default",
-          "position": [-0.3, 1, 1],
-          "rotation": [0, 1, 0]
-        },
-        {
-          "name":"parking_sensor",
-          "type":"Ultrasonic.smallrange",
-          "position":[0, 1, 2],
-          "rotation":[0, 1, 0]
         },
         {
           "name": "electrics0",
           "type": "Electrics"
         },
-                {
-          "name": "lidar0",
-          "type": "Lidar.default",
-          "position": [0, 0, 1.7]
-        },   
         {
           "name": "node_imu",
           "type": "IMU",
@@ -54,6 +37,26 @@
           "type": "IMU",
           "position": [0.73, 0.51, 0.8]
         }
+      ],
+      "sensors_automation":  [
+        {
+          "name":"parking_sensor",
+          "type":"Ultrasonic.smallrange",
+          "position":[0, 1, 2],
+          "rotation":[0, 0]
+        },
+        {
+          "name": "front_cam",
+          "type": "Camera.default",
+          "position": [-0.3, 1, 1],
+          "rotation": [0, 0]
+        },
+        {
+          "name": "lidar0",
+          "type": "Lidar.default",
+          "position": [0, 0, 1.7],
+          "rotation": [0, 0]
+        }
       ]
     }
   ]

beamng_control/config/scenarios/GridMap.json

@@ -10,12 +10,12 @@
       "model": "etk800",
       "position": [568.908, 13.422,  148.565],
       "rotation": [0, 0, 0, 1],
-      "sensors": [
+      "sensors_automation": [
         {
           "name": "front_cam",
           "type": "Camera.default",
           "position": [-0.3, 1, 1],
-          "rotation": [0, 1, 0]
+          "rotation": [0, 0]
         }
       ]
     }