1- from dataclasses import replace
2- import math
3- from typing import List
4- from ...utils import settings
5- from ...mathutils import transforms
6- from ...state .vehicle import VehicleState ,VehicleGearEnum
7- from ...state .physical_object import ObjectFrameEnum ,ObjectPose ,convert_xyhead
8- from ...knowledge .vehicle .geometry import front2steer ,steer2front
9- from ...mathutils .signal import OnlineLowPassFilter
10- from ..interface .gem import GEMInterface
11- from ..component import Component
12- from ..interface .gem import GNSSReading
13-
14- class GNSSStateEstimator (Component ):
15- """Just looks at the GNSS reading to estimate the vehicle state"""
16- def __init__ (self , vehicle_interface : GEMInterface ):
17- self .vehicle_interface = vehicle_interface
18- if 'gnss' not in vehicle_interface .sensors ():
19- raise RuntimeError ("GNSS sensor not available" )
20- vehicle_interface .subscribe_sensor ('gnss' ,self .gnss_callback ,GNSSReading )
21- self .gnss_pose = None
22- self .location = settings .get ('vehicle.calibration.gnss_location' )[:2 ]
23- self .yaw_offset = settings .get ('vehicle.calibration.gnss_yaw' )
24- self .speed_filter = OnlineLowPassFilter (1.2 , 30 , 4 )
25- self .status = None
26-
27- # Get GNSS information
28- def gnss_callback (self , reading : GNSSReading ):
29- self .gnss_pose = reading .pose
30- self .gnss_speed = reading .speed
31- self .status = reading .status
32-
33- def rate (self ):
34- return 10.0
35-
36- def state_outputs (self ) -> List [str ]:
37- return ['vehicle' ]
38-
39- def healthy (self ):
40- return self .gnss_pose is not None
41-
42- def update (self ) -> VehicleState :
43- if self .gnss_pose is None :
44- return
45- #TODO: figure out what this status means
46- #print("INS status",self.status)
47-
48- # vehicle gnss heading (yaw) in radians
49- # vehicle x, y position in fixed local frame, in meters
50- # reference point is located at the center of GNSS antennas
51- localxy = transforms .rotate2d (self .location ,- self .yaw_offset )
52- gnss_xyhead_inv = (- localxy [0 ],- localxy [1 ],- self .yaw_offset )
53- center_xyhead = self .gnss_pose .apply_xyhead (gnss_xyhead_inv )
54- vehicle_pose_global = replace (self .gnss_pose ,
55- t = self .vehicle_interface .time (),
56- x = center_xyhead [0 ],
57- y = center_xyhead [1 ],
58- yaw = center_xyhead [2 ])
59-
60- readings = self .vehicle_interface .get_reading ()
61- raw = readings .to_state (vehicle_pose_global )
62-
63- #filtering speed
64- raw .v = self .gnss_speed
65- #filt_vel = self.speed_filter(raw.v)
66- #raw.v = filt_vel
67- return raw
68-
69-
70-
71- class OmniscientStateEstimator (Component ):
72- def __init__ (self , vehicle_interface : GEMInterface ):
73- self .vehicle_interface = vehicle_interface
74- if 'gnss' not in vehicle_interface .sensors ():
75- raise RuntimeError ("GNSS sensor not available" )
76- vehicle_interface .subscribe_sensor ('gnss' ,self .fake_gnss_callback )
77- self .vehicle_state = None
78-
79- # Get GNSS information
80- def fake_gnss_callback (self , vehicle_state ):
81- self .vehicle_state = vehicle_state
82-
83- def rate (self ):
84- return 50.0
85-
86- def state_outputs (self ) -> List [str ]:
87- return ['vehicle' ]
88-
89- def healthy (self ):
90- return self .vehicle_state is not None
91-
92- def update (self ) -> VehicleState :
1+ from dataclasses import replace
2+ import math
3+ from typing import List
4+ from ...utils import settings
5+ from ...mathutils import transforms
6+ from ...state .vehicle import VehicleState ,VehicleGearEnum
7+ from ...state .physical_object import ObjectFrameEnum ,ObjectPose ,convert_xyhead
8+ from ...knowledge .vehicle .geometry import front2steer ,steer2front
9+ from ...mathutils .signal import OnlineLowPassFilter
10+ from ..interface .gem import GEMInterface
11+ from ..component import Component
12+ from ..interface .gem import GNSSReading
13+
14+ class GNSSStateEstimator (Component ):
15+ """Just looks at the GNSS reading to estimate the vehicle state"""
16+ def __init__ (self , vehicle_interface : GEMInterface ):
17+ self .vehicle_interface = vehicle_interface
18+ if 'gnss' not in vehicle_interface .sensors ():
19+ raise RuntimeError ("GNSS sensor not available" )
20+ vehicle_interface .subscribe_sensor ('gnss' ,self .gnss_callback ,GNSSReading )
21+ self .gnss_pose = None
22+ self .location = settings .get ('vehicle.calibration.gnss_location' )[:2 ]
23+ self .yaw_offset = settings .get ('vehicle.calibration.gnss_yaw' )
24+ self .speed_filter = OnlineLowPassFilter (1.2 , 30 , 4 )
25+ self .status = None
26+
27+ # Get GNSS information
28+ def gnss_callback (self , reading : GNSSReading ):
29+ self .gnss_pose = reading .pose
30+ self .gnss_speed = reading .speed
31+ self .status = reading .status
32+
33+ def rate (self ):
34+ return 10.0
35+
36+ def state_outputs (self ) -> List [str ]:
37+ return ['vehicle' ]
38+
39+ def healthy (self ):
40+ return self .gnss_pose is not None
41+
42+ def update (self ) -> VehicleState :
43+ if self .gnss_pose is None :
44+ return
45+ #TODO: figure out what this status means
46+ #print("INS status",self.status)
47+
48+ # vehicle gnss heading (yaw) in radians
49+ # vehicle x, y position in fixed local frame, in meters
50+ # reference point is located at the center of GNSS antennas
51+ localxy = transforms .rotate2d (self .location ,- self .yaw_offset )
52+ gnss_xyhead_inv = (- localxy [0 ],- localxy [1 ],- self .yaw_offset )
53+ center_xyhead = self .gnss_pose .apply_xyhead (gnss_xyhead_inv )
54+ vehicle_pose_global = replace (self .gnss_pose ,
55+ t = self .vehicle_interface .time (),
56+ x = center_xyhead [0 ],
57+ y = center_xyhead [1 ],
58+ yaw = center_xyhead [2 ])
59+
60+ readings = self .vehicle_interface .get_reading ()
61+ raw = readings .to_state (vehicle_pose_global )
62+
63+ #filtering speed
64+ raw .v = self .gnss_speed
65+
66+ # Assume no backward slide, use gear to decide velocity sign
67+ if raw .gear == - 1 :
68+ raw .v *= - 1
69+
70+ #filt_vel = self.speed_filter(raw.v)
71+ #raw.v = filt_vel
72+ return raw
73+
74+
75+
76+ class OmniscientStateEstimator (Component ):
77+ def __init__ (self , vehicle_interface : GEMInterface ):
78+ self .vehicle_interface = vehicle_interface
79+ if 'gnss' not in vehicle_interface .sensors ():
80+ raise RuntimeError ("GNSS sensor not available" )
81+ vehicle_interface .subscribe_sensor ('gnss' ,self .fake_gnss_callback )
82+ self .vehicle_state = None
83+
84+ # Get GNSS information
85+ def fake_gnss_callback (self , vehicle_state ):
86+ self .vehicle_state = vehicle_state
87+
88+ def rate (self ):
89+ return 50.0
90+
91+ def state_outputs (self ) -> List [str ]:
92+ return ['vehicle' ]
93+
94+ def healthy (self ):
95+ return self .vehicle_state is not None
96+
97+ def update (self ) -> VehicleState :
9398 return self .vehicle_state
0 commit comments