Dear community,
I want to use the zed sdk (pyzed.sl) to fused the gnss data since it have not been supported for Zed_wrapper yet. So far, I want to subscribe to topic /ublox/fix and fuse with my gnss data and publish to /ublox/fix_fused. I attached my code below since I cannot attached the file (new user policy). The problem is after the robot moving, the status turns out to be “NOT_CALIBRATED”. Is that my parameters are wrong or any other problems. How can I fix it? Thank youu.
GPS reader from ros
import threading
import time
import rospy
from sensor_msgs.msg import NavSatFix
import pyzed.sl as sl
class ROSGPSReader:
def __init__(self):
self.continue_to_grab = True
self.new_data = False
self.current_gnss_data = None
self.subscriber = None
self.gnss_data_lock = threading.Lock()
def initialize(self):
# rospy.init_node('gnss_reader', anonymous=True)
self.subscriber = rospy.Subscriber("/ublox/fix", NavSatFix, self.gnss_callback)
print("Waiting for GNSS fix...")
def gnss_callback(self, data):
with self.gnss_data_lock:
self.current_gnss_data = sl.GNSSData()
self.current_gnss_data.set_coordinates(data.latitude, data.longitude, data.altitude, False)
self.current_gnss_data.longitude_std = data.position_covariance[0]
self.current_gnss_data.latitude_std = data.position_covariance[4]
self.current_gnss_data.altitude_std = data.position_covariance[8]
position_covariance = data.position_covariance
ts = sl.Timestamp()
ts.set_microseconds(rospy.Time.now().to_nsec() // 1000)
self.current_gnss_data.ts = ts
self.new_data = True
def grab(self):
with self.gnss_data_lock:
if self.new_data:
self.new_data = False
return sl.ERROR_CODE.SUCCESS, self.current_gnss_data
return sl.ERROR_CODE.FAILURE, None
def stop(self):
self.continue_to_grab = False
if __name__ == "__main__":
reader = ROSGPSReader()
reader.initialize()
try:
while not rospy.is_shutdown():
time.sleep(1) # Main thread continues to run, allowing Ctrl+C to stop ROS
finally:
reader.stop()
----------------------------------------------------------------------
FUSE with zed VIO
-----------------------------------------------------------------------
import pyzed.sl as sl
import rospy
from sensor_msgs.msg import Image, CompressedImage, NavSatFix
from cv_bridge import CvBridge
import cv2
import numpy as np
from gnss_reader.gpsd_reader_ros import ROSGPSReader
def publish_gnss_data(gnss_data, publisher):
"""
Publish GNSS data to ROS topic
"""
fix = NavSatFix()
fix.header.stamp = rospy.Time.now()
fix.latitude = gnss_data["latitude"]
fix.longitude = gnss_data["longitude"]
fix.altitude = gnss_data["altitude"]
publisher.publish(fix)
def main():
# Initialize ROS node
rospy.init_node('zed_gps_camera_publisher', anonymous=True)
pub_raw = rospy.Publisher('/zed2i/zed_node/left/image_rect_color', Image, queue_size=10)
pub_compressed = rospy.Publisher('/zed2i/zed_node/rgb/image_rect_color/compressed', CompressedImage, queue_size=10)
pub = rospy.Publisher('/ublox/fix_fused', NavSatFix, queue_size=10)
# Open the camera
zed = sl.Camera()
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.HD720 # Choose camera resolution
init_params.camera_fps = 30 #Choose camera FPS
init_params.sdk_verbose = 1
status = zed.open(init_params)
if status != sl.ERROR_CODE.SUCCESS:
print("[ZED][ERROR] Camera Open : "+repr(status)+". Exit program.")
exit()
# Create a CV bridge
bridge = CvBridge()
# Enable positional tracking:
positional_init = zed.enable_positional_tracking()
if positional_init != sl.ERROR_CODE.SUCCESS:
print("[ZED][ERROR] Can't start tracking of camera : "+repr(status)+". Exit program.")
exit()
# Create Fusion object:
fusion = sl.Fusion()
# fusion.enable_positionnal_tracking({"gnss_calibration_parameters" : gnss_calibration_parameters , "enable_GNSS_fusion" : True})
init_fusion_param = sl.InitFusionParameters()
init_fusion_param.coordinate_units = sl.UNIT.METER
fusion_init_code = fusion.init(init_fusion_param)
if fusion_init_code != sl.FUSION_ERROR_CODE.SUCCESS:
print("[ZED][ERROR] Failed to initialize fusion :"+repr(fusion_init_code)+". Exit program")
exit()
# Enable odometry publishing:
configuration = sl.CommunicationParameters()
zed.start_publishing(configuration)
# Enable GNSS data producing:
gnss_reader = ROSGPSReader() # Modified line to use ROS GNSS reader
gnss_reader.initialize() # Initialize ROS GNSS reader
# Subscribe to Odometry
uuid = sl.CameraIdentifier(zed.get_camera_information().serial_number)
fusion.subscribe(uuid,configuration,sl.Transform(0,0,0))
# Enable positional tracking for Fusion object
positional_tracking_fusion_parameters = sl.PositionalTrackingFusionParameters()
positional_tracking_fusion_parameters.enable_GNSS_fusion = True
positional_tracking_fusion_parameters.gnss_calibration_parameters.target_yaw_uncertainty = 0.1
positional_tracking_fusion_parameters.gnss_calibration_parameters.enable_translation_uncertainty_target = False
positional_tracking_fusion_parameters.gnss_calibration_parameters.target_translation_uncertainty = 0.1
positional_tracking_fusion_parameters.gnss_calibration_parameters.enable_reinitialization = True
positional_tracking_fusion_parameters.gnss_calibration_parameters.gnss_vio_reinit_threshold = 10
positional_tracking_fusion_parameters.gnss_calibration_parameters.enable_rolling_calibration = True
# {
# "target_yaw_uncertainty" : 0.1,
# "enable_translation_uncertainty_target" : False,
# "target_translation_uncertainty" : 10e-1,
# "enable_reinitialization" : True,
# "gnss_vio_reinit_threshold" : 5,
# "enable_rolling_calibration" : True
# }
# fusion.enable_positionnal_tracking({"gnss_calibration_parameters" : gnss_calibration_parameters , "enable_GNSS_fusion" : True})
fusion.enable_positionnal_tracking(positional_tracking_fusion_parameters)
py_translation = sl.Translation()
# Setup viewer:
# viewer = GenericDisplay()
# viewer.init(zed.get_camera_information().camera_model)
# Capture images
runtime_parameters = sl.RuntimeParameters()
print("Start grabbing data ...")
# while viewer.isAvailable():
while not rospy.is_shutdown():
try:
# Grab camera:
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
zed_pose = sl.Pose()
# You can still use the classical getPosition for your application, just not that the position returned by this method
# is the position without any GNSS/cameras fusion
zed.get_position(zed_pose, sl.REFERENCE_FRAME.CAMERA)
# Retrieve left image
left_image = sl.Mat()
zed.retrieve_image(left_image, sl.VIEW.LEFT)
# Convert image to numpy array
left_image_array = left_image.get_data()
# Convert RGBA to BGR
left_image_array_bgr = cv2.cvtColor(left_image_array, cv2.COLOR_RGBA2BGR)
# Convert the image to ROS message
ros_image = bridge.cv2_to_imgmsg(left_image_array_bgr, encoding="rgb8")
# Publish raw image
pub_raw.publish(ros_image)
# Convert image to compressed ROS message
ros_compressed_image = bridge.cv2_to_compressed_imgmsg(left_image_array_bgr)
# Publish compressed image
pub_compressed.publish(ros_compressed_image)
# Release the image
left_image.free()
# Get GNSS data:
status, input_gnss = gnss_reader.grab()
if status == sl.ERROR_CODE.SUCCESS:
# Publish GNSS data to Fusion
ingest_error = fusion.ingest_gnss_data(input_gnss)
if ingest_error != sl.FUSION_ERROR_CODE.SUCCESS:
print("Ingest error occurred when ingesting GNSSData: ", ingest_error)
# Process data and compute positions:
if fusion.process() == sl.FUSION_ERROR_CODE.SUCCESS:
fused_position = sl.Pose()
# Get position into the ZED CAMERA coordinate system:
current_state = fusion.get_position(fused_position)
# Display it on OpenGL:
rotation = fused_position.get_rotation_vector()
translation = fused_position.get_translation(py_translation)
text_rotation = str((round(rotation[0], 2), round(rotation[1], 2), round(rotation[2], 2)))
text_translation = str((round(translation.get()[0], 2), round(translation.get()[1], 2), round(translation.get()[2], 2)))
# viewer.updatePoseData(fused_position.pose_data(), text_translation, text_rotation, current_state)
# Get position into the GNSS coordinate system - this needs a initialization between CAMERA
# and GNSS. When the initialization is finish the getGeoPose will return sl.POSITIONAL_TRACKING_STATE.OK
current_geopose = sl.GeoPose()
current_geopose_satus = fusion.get_geo_pose(current_geopose)
print(current_geopose_satus)
print(rotation, translation.get())
print(text_rotation, text_translation)
if current_geopose_satus == sl.GNSS_CALIBRATION_STATE.CALIBRATED:
gnss_input= {
"latitude": current_geopose.latlng_coordinates.get_latitude(False),
"longitude": current_geopose.latlng_coordinates.get_longitude(False),
"altitude": current_geopose.latlng_coordinates.get_altitude()}
publish_gnss_data(gnss_input, pub)
# viewer.updateGeoPoseData(current_geopose, zed.get_timestamp(sl.TIME_REFERENCE.CURRENT).data_ns/1000)
"""
else:
GNSS coordinate system to ZED coordinate system is not initialize yet
The initialisation between the coordinates system is basicaly an optimization problem that
Try to fit the ZED computed path with the GNSS computed path. In order to do it just move
your system by the distance you specified in positional_tracking_fusion_parameters.gnss_initialisation_distance
"""
except rospy.ROSException as e:
print("ROS Exception:", e)
fusion.close()
zed.close()
if __name__ == '__main__' :
main()
