Skip to content

Commit b1b4fbc

Browse files
author
Henry Dalrymple
committed
Added odometry to imu, gps, and servo
1 parent b137c11 commit b1b4fbc

File tree

10 files changed

+770
-316
lines changed

10 files changed

+770
-316
lines changed

‎software/.startCan.sh.swp‎

Whitespace-only changes.

‎software/ros_packages/rover2_control/rover2_control/tower_pan_tilt_control.py‎

Lines changed: 48 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import minimalmodbus
1212

1313
from std_msgs.msg import UInt8, UInt16
14+
from sensor_msgs.msg import NavSatFix, NavSatStatus
1415

1516
# Custom Imports
1617
from rover2_control_interface.msg import TowerPanTiltControlMessage, LightControlMessage, GPSStatusMessage
@@ -26,6 +27,7 @@
2627
DEFAULT_TOWER_LIGHT_CONTROL_TOPIC = "tower/light/control"
2728
DEFAULT_TOWER_GPS_STATUS_TOPIC = "tower/status/gps"
2829
DEFAULT_PAN_TILT_CONTROL_TOPIC = "tower/pan_tilt/control"
30+
DEFAULT_GPS_NAVSATFIX_TOPIC = "gps/fix"
2931

3032
TOWER_NODE_ID = 1
3133
PAN_TILT_NODE_ID = 2
@@ -116,6 +118,9 @@ def __init__(self):
116118

117119
self.tower_gps_publisher_topic = self.declare_parameter("~tower_gps_status_topic",
118120
DEFAULT_TOWER_GPS_STATUS_TOPIC).value
121+
122+
self.gps_navsatfix_topic = self.declare_parameter("~gps_navsatfix_topic",
123+
DEFAULT_GPS_NAVSATFIX_TOPIC).value
119124

120125
self.wait_time = 1.0 / self.declare_parameter("~hertz", DEFAULT_HERTZ).value
121126

@@ -132,6 +137,9 @@ def __init__(self):
132137
self.tower_light_control_callback, 1)
133138

134139
self.tower_gps_publisher = self.create_publisher(GPSStatusMessage, self.tower_gps_publisher_topic, 1)
140+
141+
# Add NavSatFix publisher for robot_localization
142+
self.gps_navsatfix_publisher = self.create_publisher(NavSatFix, self.gps_navsatfix_topic, 1)
135143

136144
self.pan_tilt_control_message = None
137145
self.tower_light_control_message = None
@@ -222,8 +230,46 @@ def broadcast_gps_reading_message(self):
222230
gps_status.rover_longitude = gps_coords[GPS_COORDINATES["rover_longitude"]]
223231
gps_status.astronaut_latitude = gps_coords[GPS_COORDINATES["astronaut_latitude"]]
224232
gps_status.astronaut_longitude = gps_coords[GPS_COORDINATES["astronaut_longitude"]]
225-
#print(gps_status)
233+
234+
# Publish custom GPS status message
226235
self.tower_gps_publisher.publish(gps_status)
236+
237+
# Publish NavSatFix message for robot_localization
238+
self.publish_navsatfix(gps_status.rover_latitude, gps_status.rover_longitude)
239+
240+
def publish_navsatfix(self, latitude, longitude):
241+
"""
242+
Publish GPS data as NavSatFix message for robot_localization
243+
"""
244+
nav_msg = NavSatFix()
245+
246+
# Header
247+
nav_msg.header.stamp = self.get_clock().now().to_msg()
248+
nav_msg.header.frame_id = "gps"
249+
250+
# GPS Status
251+
nav_msg.status.status = NavSatStatus.STATUS_FIX # Assume we have a fix
252+
nav_msg.status.service = NavSatStatus.SERVICE_GPS
253+
254+
# Position
255+
nav_msg.latitude = latitude
256+
nav_msg.longitude = longitude
257+
nav_msg.altitude = 0.0 # No altitude data available
258+
259+
# Covariance (diagonal matrix)
260+
# Set reasonable GPS accuracy - adjust based on your GPS unit specs
261+
# Typical consumer GPS: ~3-5 meters accuracy
262+
gps_accuracy = 5.0 # meters
263+
nav_msg.position_covariance[0] = gps_accuracy ** 2 # East variance
264+
nav_msg.position_covariance[4] = gps_accuracy ** 2 # North variance
265+
nav_msg.position_covariance[8] = gps_accuracy ** 2 # Up variance
266+
nav_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
267+
268+
# Check for valid GPS data (not zero)
269+
if latitude == 0.0 and longitude == 0.0:
270+
nav_msg.status.status = NavSatStatus.STATUS_NO_FIX
271+
272+
self.gps_navsatfix_publisher.publish(nav_msg)
227273

228274
def send_tower_control_message(self):
229275
if self.new_tower_light_control_message:
@@ -246,4 +292,4 @@ def main(args=None):
246292
rclpy.shutdown()
247293

248294
if __name__ == "__main__":
249-
TowerPanTiltControl()
295+
main()

‎software/ros_packages/rover2_odometry/ekf_configs/ef-map-ekf.yaml‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
### ekf config file ###
2+
ekf_node:
3+
ros__parameters:
4+
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
5+
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
6+
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
7+
frequency: 30.0
8+
9+
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
10+
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
11+
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
12+
# by, for example, an IMU. Defaults to false if unspecified.
13+
two_d_mode: false
14+
15+
# Whether to publish the acceleration state. Defaults to false if unspecified.
16+
publish_acceleration: true
17+
18+
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
19+
publish_tf: true
20+
21+
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
22+
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
23+
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
24+
# to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
25+
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
26+
# observations) then:
27+
# 3a. Set your "world_frame" to your map_frame value
28+
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
29+
# from robot_localization! However, that instance should *not* fuse the global data.
30+
map_frame: map # Defaults to "map" if unspecified
31+
odom_frame: odom # Defaults to "odom" if unspecified
32+
base_link_frame: base_link # Defaults to "base_link" if unspecified
33+
world_frame: odom # Defaults to the value of odom_frame if unspecified
34+
35+
odom0: demo/odom
36+
odom0_config: [false, false, false,
37+
false, false, false,
38+
true, true, false,
39+
false, false, true,
40+
false, false, false]
41+
42+
imu0: demo/imu
43+
imu0_config: [false, false, false,
44+
false, false, false,
45+
false, false, false,
46+
false, false, true,
47+
false, false, false]

‎software/ros_packages/rover2_odometry/launch/rover2_odometry_launch.py‎

Lines changed: 120 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -26,11 +26,124 @@ def generate_launch_description():
2626
executable='scimech_sensors',
2727
name='scimech_sensors',
2828
**config
29-
)
30-
# Node(
31-
# package='rover2_odometry',
32-
# executable='odometry',
33-
# name='gps',
34-
# **config
35-
# )
29+
),
30+
Node(
31+
package='rover2_odometry',
32+
executable='odometry',
33+
name='odometry',
34+
**config
35+
),
36+
Node(
37+
package='rover2_odometry',
38+
executable='odrive_can_info',
39+
name='odrive_can_info',
40+
**config
41+
),
42+
# Global EKF (fuses local EKF and GPS - also publishes odom->base_link)
43+
Node(
44+
package='robot_localization',
45+
executable='ekf_node',
46+
name='ekf_global',
47+
output='screen',
48+
parameters=[{
49+
'frequency': 50.0,
50+
'sensor_timeout': 0.1,
51+
'two_d_mode': True,
52+
'transform_time_offset': 0.0,
53+
'transform_timeout': 0.0,
54+
'print_diagnostics': True,
55+
'debug': False,
56+
'publish_tf': True,
57+
58+
'map_frame': 'map',
59+
'odom_frame': 'odom',
60+
'base_link_frame': 'base_link',
61+
'world_frame': 'odom',
62+
63+
# Local odometry
64+
'odom0': '/odom',
65+
'odom0_config': [False, False, False,
66+
False, False, False,
67+
True, True, False,
68+
False, False, True,
69+
False, False, False],
70+
'odom0_queue_size': 10,
71+
'odom0_differential': False,
72+
'odom0_relative': False,
73+
74+
# GPS odometry (from navsat_transform)
75+
'odom1': '/odometry/gps',
76+
'odom1_config': [True, True, False, # Use GPS x, y position
77+
False, False, False,
78+
False, False, False,
79+
False, False, False,
80+
False, False, False],
81+
'odom1_queue_size': 10,
82+
'odom1_differential': False,
83+
'odom1_relative': False,
84+
85+
# IMU (same as local)
86+
'imu0': '/imu/data',
87+
'imu0_config': [False, False, False,
88+
False, False, True,
89+
False, False, False,
90+
False, False, True,
91+
True, True, False],
92+
'imu0_queue_size': 10,
93+
'imu0_differential': False,
94+
'imu0_relative': False,
95+
'imu0_remove_gravitational_acceleration': True,
96+
}],
97+
remappings=[
98+
('/odometry/filtered', '/odometry/global'),
99+
]
100+
),
101+
102+
# 5. NavSat Transform - converts GPS to map frame
103+
Node(
104+
package='robot_localization',
105+
executable='navsat_transform_node',
106+
name='navsat_transform',
107+
output='screen',
108+
parameters=[{
109+
# Frequency and timing
110+
'frequency': 30.0,
111+
'delay': 3.0,
112+
113+
# Magnetic declination at your location (radians)
114+
# Find yours: https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml
115+
'magnetic_declination_radians': 0.0,
116+
'yaw_offset': 0.0,
117+
118+
# 2D navigation
119+
'zero_altitude': True,
120+
121+
# Publishing options
122+
'publish_filtered_gps': True,
123+
'broadcast_utm_transform': False,
124+
'broadcast_utm_transform_as_parent_frame': False,
125+
126+
# Use odometry heading instead of IMU
127+
'use_odometry_yaw': True,
128+
129+
# Let first GPS message set origin
130+
'wait_for_datum': False,
131+
132+
# Manual datum (only used if wait_for_datum is true)
133+
'use_manual_datum': False,
134+
'datum': [40.0, -105.0, 0.0], # [lat, lon, alt] - adjust if needed
135+
136+
# Frame IDs
137+
'map_frame_id': 'map',
138+
'odom_frame_id': 'odom',
139+
'base_link_frame_id': 'base_link',
140+
'world_frame_id': 'odom', # Match the EKFs
141+
}],
142+
remappings=[
143+
('/gps/fix', '/gps/fix'), # YOUR GPS INPUT TOPIC
144+
('/imu/data', '/imu/data'), # Your IMU topic
145+
('/odometry/filtered', '/odometry/global'), # Which EKF to use for heading
146+
('/odometry/gps', '/odometry/gps'), # GPS output topic
147+
]
148+
),
36149
])

0 commit comments

Comments
 (0)