def odomCB(self, odo):
self.robot_odom = odo
if not self.list_cleaning:
self.odom_list[0].append(odo.header.stamp)
self.odom_list[1].append(odo.pose.pose)
if self.odom_calibrating:
self.rate.sleep()
return
try:
trans = self.tfBuffer.lookup_transform(self.gps_frame, self.odom_frame, rospy.Time())
if self.verify_stamp > trans.header.stamp:
debug_info(self.debug_output, "Looking up transformations")
return
robot_odom_pose = PoseStamped()
robot_odom_pose.pose = self.robot_odom.pose.pose
pose_transformed = tf2_geometry_msgs.do_transform_pose(robot_odom_pose, trans)
robot_gps_pose = Odometry()
robot_gps_pose.header.frame_id = self.gps_frame
robot_gps_pose.child_frame_id = self.robot_frame
robot_gps_pose.pose.pose = pose_transformed.pose
robot_gps_pose.pose.pose.position.x,robot_gps_pose.pose.pose.position.y = self.projection(pose_transformed.pose.position.x,pose_transformed.pose.position.y,inverse=True)
robot_gps_pose.pose.pose.orientation.x = -pose_transformed.pose.orientation.x
robot_gps_pose.pose.pose.orientation.y = -pose_transformed.pose.orientation.y
robot_gps_pose.pose.pose.orientation.z = -pose_transformed.pose.orientation.z
robot_gps_pose.pose.pose.orientation = quat_rot(robot_gps_pose.pose.pose.orientation,0,0,90)
self.robot_gps_pub.publish(robot_gps_pose)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
return
def poseCB(self, p):
self.odom_calibrating = True
robot_pose = Pose()
robot_pose = p.pose.pose
robot_pose.position.x, robot_pose.position.y = self.projection(
p.pose.pose.position.x, p.pose.pose.position.y)
robot_pose.orientation.x = -p.pose.pose.orientation.x
robot_pose.orientation.y = -p.pose.pose.orientation.y
robot_pose.orientation.z = -p.pose.pose.orientation.z
robot_pose.orientation = quat_rot(robot_pose.orientation, 0, 0, 90)
debug_info(self.debug_output, "Odom calib tf received")
# odom to reference
while 1:
try:
odo_ref_trans = self.tfBuffer.lookup_transform(
self.robot_frame, self.odom_frame, rospy.Time())
tf_odo_ref = TransformStamped()
tf_odo_ref.header.frame_id = "odom_utm_calib"
tf_odo_ref.child_frame_id = self.odom_frame
tf_odo_ref.header.stamp = rospy.Time.now()
tf_odo_ref.transform = odo_ref_trans.transform
tfmsg_odo_ref = tf2_msgs.msg.TFMessage([tf_odo_ref])
self.tf_pub.publish(tfmsg_odo_ref)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
# reference to utm
tf_ref_utm = TransformStamped()
tf_ref_utm.header.frame_id = self.gps_frame
tf_ref_utm.child_frame_id = "odom_utm_calib"
tf_ref_utm.header.stamp = rospy.Time.now()
tf_ref_utm.transform.translation.x = robot_pose.position.x
tf_ref_utm.transform.translation.y = robot_pose.position.y
tf_ref_utm.transform.translation.z = robot_pose.position.z
tf_ref_utm.transform.rotation = robot_pose.orientation
tfmsg_ref_utm = tf2_msgs.msg.TFMessage([tf_ref_utm])
self.tf2_pub.publish(tfmsg_ref_utm)
self.rate.sleep()
# Check the tf exists correctly
try:
trans = self.tfBuffer.lookup_transform(self.gps_frame,
"odom_utm_calib",
rospy.Time())
trans2 = self.tfBuffer.lookup_transform(
"odom_utm_calib", self.odom_frame, rospy.Time())
if math.fabs(trans.transform.translation.x -
robot_pose.position.x) > 0.1 or math.fabs(
trans.transform.translation.y -
robot_pose.position.y) > 0.1:
continue
self.odom_calibrating = False
debug_info(self.debug_output, "Odometry calibrated")
break
except:
continue
def goalCB(self, g):
self.goal_set = False
x, y = self.projection(g.longitude, g.latitude)
z = g.altitude
if not self.pose_get:
self.robot_pose.position.x = x
self.robot_pose.position.y = y
self.robot_pose.position.z = z
self.pose_get = True
return
self.goal.x = x
self.goal.y = y
self.goal.z = z
self.waypoint = g
debug_info(self.debug_output, "Waypoint received")
if self.orentation_get:
return
dx = x - self.robot_pose.position.x
dy = y - self.robot_pose.position.y
dz = z - self.robot_pose.position.z
robot_roll = 0
robot_pitch = math.atan2(dz, math.sqrt(dx**2 + dy**2))
robot_yaw = math.atan2(dy, dx)
robot_quat = tf.transformations.quaternion_from_euler(
robot_roll, robot_pitch, robot_yaw)
self.robot_pose.orientation.x = robot_quat[0]
self.robot_pose.orientation.y = robot_quat[1]
self.robot_pose.orientation.z = robot_quat[2]
self.robot_pose.orientation.w = robot_quat[3]
self.orentation_get = True
# Publish robot initial position to calibrate the Odometry
robot_gps_pose = Odometry()
robot_gps_pose.pose.pose = self.robot_pose
robot_gps_pose.pose.pose.position.x, robot_gps_pose.pose.pose.position.y = self.projection(
self.robot_pose.position.x,
self.robot_pose.position.y,
inverse=True)
robot_gps_pose.pose.pose.orientation.x = -self.robot_pose.orientation.x
robot_gps_pose.pose.pose.orientation.y = -self.robot_pose.orientation.y
robot_gps_pose.pose.pose.orientation.z = -self.robot_pose.orientation.z
robot_gps_pose.pose.pose.orientation = quat_rot(
robot_gps_pose.pose.pose.orientation, 0, 0, 90)
self.robot_gps_pub.publish(robot_gps_pose)
def accCB(self, a):
if len(a.data) <= 0:
debug_info(self.debug_output, "waypoint_control acceleration invalid")
return
parts = a.data.split(',')
if len(parts) != 2:
debug_info(self.debug_output, "waypoint_control error: 2 accelerations needed, only " + str(len(parts)) + " sent")
return
try:
self.ACC = float(parts[0])
self.ACC_R = float(parts[1])
debug_info(self.debug_output, "Acceleration updated: " + str(self.ACC) +", " + str(self.ACC_R))
except:
return
def paraCB(self, p):
if len(p.data) <= 0:
debug_info(self.debug_output, "odometry_control parameter invalid")
return
parts = p.data.split(',')
if len(parts) != 4:
debug_info(self.debug_output, "odometry_control error: 4 parameters needed, only " + str(len(parts)) + " sent")
return
try:
self.K_RHO = float(parts[0])
self.K_ROLL = float(parts[1])
self.K_PITCH = float(parts[2])
self.K_YAW = float(parts[3])
debug_info(self.debug_output, "Parameter updated: " + str(self.K_RHO) +", " + str(self.K_ROLL) +", " + str(self.K_PITCH) +", " + str(self.K_YAW))
except:
return
def __init__(self):
# Variables
self.projection = Proj(proj="utm", zone="34", ellps='WGS84')
self.tfBuffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tfBuffer)
self.reference_id = 0
self.fiducial_gps_map = FiducialMapEntryArray()
self.state = "STOP"
self.prestate = "STOP"
self.previous_fiducial = 0
self.robot_stopped = True
self.waiting_time = 0
self.waiting = False
self.displacement = 0
self.pre_odom = Odometry()
self.pre_odom_get = False
# Init ROS node
rospy.init_node('fiducial_waypoint_localization')
# rosparams
self.robot_frame = rospy.get_param("~waypoint_control/base_frame",
"base_footprint")
self.gps_frame = rospy.get_param("~waypoint_control/gps_frame", "utm")
self.camera_frame = rospy.get_param("~waypoint_control/camera_frame",
"raspicam")
self.trackWidth = float(
rospy.get_param("~driveGeometry/trackWidth", "0.403"))
self.fiducial_map_file = rospy.get_param("~waypoint_control/map_file",
"Fiducials.json")
# Publishers
self.robot_gps_pub = rospy.Publisher('robot_gps_pose',
Odometry,
queue_size=10,
latch=True)
self.tf_pub = rospy.Publisher("tf",
tf2_msgs.msg.TFMessage,
queue_size=30,
latch=True)
self.state_pub = rospy.Publisher('waypoint/state',
String,
queue_size=10,
latch=True)
self.debug_output = rospy.Publisher('debug_output',
String,
queue_size=10)
# Subscribers
rospy.Subscriber('fiducial_map_gps', FiducialMapEntryArray,
self.mapGPSCB)
rospy.Subscriber('fiducial_transforms', FiducialTransformArray,
self.transCB)
rospy.Subscriber('waypoint/state', String, self.stateCB)
rospy.Subscriber('odom', Odometry, self.odomCB)
# 1 Hz
self.rate = rospy.Rate(1)
# Init fiducials map in GPS from json, publish all fiducial to utm trans to tf
try:
json_data = json.load(open(self.fiducial_map_file))
# Save the map in FiducialMapEntryArray()
json_map = FiducialMapEntryArray()
for fid in json_data["FiducialCollections"][0]["SavedFiducials"]:
fid_gps = FiducialMapEntry()
fid_gps.fiducial_id = fid["Id"]
fid_gps.x = fid["Position"]["longitude"]
fid_gps.y = fid["Position"]["latitude"]
fid_gps.z = fid["Position"]["altitude"]
fid_gps.rx = fid["Rotation"]["east"]
fid_gps.ry = fid["Rotation"]["north"]
fid_gps.rz = fid["Rotation"]["heading"]
json_map.fiducials.append(fid_gps)
self.fiducial_gps_map = json_map
debug_info(self.debug_output, "Fiducial map initialised")
except:
debug_info(self.debug_output,
"Initialise map from json file failed")
def transCB(self, t):
prev_in_view = False
reference_in_view = False
for fid_trans in t.transforms:
# Check whether the detected fiducial is still in the view
if fid_trans.fiducial_id == self.reference_id:
reference_in_view = True
# Check whether the previous fiducial is out of the view
if fid_trans.fiducial_id == self.previous_fiducial:
prev_in_view = True
if self.displacement < 3:
return
# If the detected is out of view when the robot stops, continue
if self.waiting and not reference_in_view:
# Cotinue move to the waypoint
self.waiting = False
state_msg = String()
state_msg.data = self.prestate
self.state_pub.publish(state_msg)
debug_info(self.debug_output,
"Fiducial detected is out of the view")
return
for fid_trans in t.transforms:
# Check the image error
if fid_trans.image_error > 0.3:
return
# Check whether detected fiducial is in the map
for fid_gps in self.fiducial_gps_map.fiducials:
# Only process the detected fiducial in the map
if fid_trans.fiducial_id != fid_gps.fiducial_id:
continue
self.reference_id = fid_trans.fiducial_id
fid_name = str(self.reference_id)
# Pause the navigation and stop the robot
if not self.waiting:
self.waiting = True
self.prestate = self.state
debug_info(self.debug_output, "Reading fiducial ...")
state_msg = String()
state_msg.data = "STOP"
self.state_pub.publish(state_msg)
# wait until robot stops
if not self.robot_stopped:
# Initialise the waiting time
self.waiting_time = 0
self.rate.sleep()
debug_info(self.debug_output, "Wait until robot stop ...")
return
# Wait another 5 seconds for updating the image
if self.waiting_time < 5:
self.waiting_time += 1
self.rate.sleep()
debug_info(self.debug_output, "Wait another 5 seconds ...")
return
# Publish tf from fid to camera
tf_fid_cam = TransformStamped()
tf_fid_cam.header.frame_id = self.camera_frame
tf_fid_cam.child_frame_id = fid_name
tf_fid_cam.header.stamp = rospy.Time.now()
tf_fid_cam.transform = fid_trans.transform
tfmsg_fid_cam = tf2_msgs.msg.TFMessage([tf_fid_cam])
self.tf_pub.publish(tfmsg_fid_cam)
if self.robot_stopped:
self.rate.sleep()
# Publish tf from fid to utm
tf_fid_utm = TransformStamped()
tf_fid_utm.header.frame_id = self.gps_frame
tf_fid_utm.child_frame_id = "fiducial" + fid_name
tf_fid_utm.header.stamp = rospy.Time.now()
tf_fid_utm.transform.translation.x, tf_fid_utm.transform.translation.y = self.projection(
fid_gps.x, fid_gps.y)
tf_fid_utm.transform.translation.z = fid_gps.z
quat = tf.transformations.quaternion_from_euler(
-math.radians(fid_gps.rx), -math.radians(fid_gps.ry),
-math.radians(fid_gps.rz))
tf_fid_utm.transform.rotation.x = quat[0]
tf_fid_utm.transform.rotation.y = quat[1]
tf_fid_utm.transform.rotation.z = quat[2]
tf_fid_utm.transform.rotation.w = quat[3]
tfmsg_fid_utm = tf2_msgs.msg.TFMessage([tf_fid_utm])
self.tf_pub.publish(tfmsg_fid_utm)
if self.robot_stopped:
self.rate.sleep()
# Publish tf from robot to fid
try:
robot_fid_trans = self.tfBuffer.lookup_transform(
fid_name, self.robot_frame, rospy.Time())
tf_robot_fid = TransformStamped()
tf_robot_fid.header.frame_id = "fiducial" + fid_name
tf_robot_fid.child_frame_id = "robot_fid"
tf_robot_fid.header.stamp = rospy.Time.now()
tf_robot_fid.transform = robot_fid_trans.transform
tfmsg_robot_fid = tf2_msgs.msg.TFMessage([tf_robot_fid])
self.tf_pub.publish(tfmsg_robot_fid)
if self.robot_stopped:
self.rate.sleep()
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
debug_info(self.debug_output,
"Creating tf from robot to fid")
return
# Publish tf from robot to utm
try:
verify_trans = self.tfBuffer.lookup_transform(
self.gps_frame, "fiducial" + fid_name, rospy.Time())
verify_trans2 = self.tfBuffer.lookup_transform(
"fiducial" + fid_name, "robot_fid", rospy.Time())
robot_utm_trans = self.tfBuffer.lookup_transform(
self.gps_frame, "robot_fid", rospy.Time())
robot_gps_pose = Odometry()
robot_gps_pose.pose.pose.position.z = robot_utm_trans.transform.translation.z
robot_gps_pose.pose.pose.position.x, robot_gps_pose.pose.pose.position.y = self.projection(
robot_utm_trans.transform.translation.x,
robot_utm_trans.transform.translation.y,
inverse=True)
robot_gps_pose.pose.pose.orientation.x = -robot_utm_trans.transform.rotation.x
robot_gps_pose.pose.pose.orientation.y = -robot_utm_trans.transform.rotation.y
robot_gps_pose.pose.pose.orientation.z = -robot_utm_trans.transform.rotation.z
robot_gps_pose.pose.pose.orientation.w = robot_utm_trans.transform.rotation.w
robot_gps_pose.pose.pose.orientation = quat_rot(
robot_gps_pose.pose.pose.orientation, 0, 0, 90)
self.robot_gps_pub.publish(robot_gps_pose)
# Resume the navigation when the update is done
state_msg = String()
state_msg.data = self.prestate
self.state_pub.publish(state_msg)
self.waiting = False
self.previous_fiducial = self.reference_id
self.displacement = 0
debug_info(self.debug_output, "Fiducial position updated")
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
debug_info(self.debug_output, "Updating the position")
return
def flyThresCB(self, thres):
self.FLYING_THRES = thres.data
debug_info(self.debug_output,
"Flying threshold is set to: " + str(self.FLYING_THRES))
def trunThresCB(self, thres):
self.TURNING_THRES = thres.data
debug_info(self.debug_output,
"Turning threshold is set to: " + str(self.TURNING_THRES))
def fwdThresCB(self, thres):
self.FORWARDING_THRES = thres.data
debug_info(
self.debug_output,
"Forwarding threshold is set to: " + str(self.FORWARDING_THRES))
def angCB(self, a):
self.VEL_MAX_ANG = a.data
debug_info(self.debug_output,
"Max angular speed is set to: " + str(self.VEL_MAX_ANG))
def linCB(self, l):
self.VEL_MAX_LIN = l.data
debug_info(self.debug_output,
"Max linear speed is set to: " + str(self.VEL_MAX_LIN))
def poseCB(self, p):
self.odom_calibrating = True
robot_pose = Pose()
robot_pose = p.pose.pose
robot_pose.position.x, robot_pose.position.y = self.projection(p.pose.pose.position.x, p.pose.pose.position.y)
robot_pose.orientation.x = -p.pose.pose.orientation.x
robot_pose.orientation.y = -p.pose.pose.orientation.y
robot_pose.orientation.z = -p.pose.pose.orientation.z
robot_pose.orientation = quat_rot(robot_pose.orientation,0,0,90)
debug_info(self.debug_output, "Odom calib tf received")
# Align odometry with odom_calib and calculate offset
current_stamp = p.header.stamp
if len(self.odom_list[1]) <= 0:
return
i = bisect_left(self.odom_list[0], current_stamp)
if i >= len(self.odom_list[1]):
i = len(self.odom_list[1]) - 1
current_euler = tf.transformations.euler_from_quaternion((self.robot_odom.pose.pose.orientation.x, self.robot_odom.pose.pose.orientation.y, self.robot_odom.pose.pose.orientation.z, self.robot_odom.pose.pose.orientation.w))
bench_euler = tf.transformations.euler_from_quaternion((self.odom_list[1][i].orientation.x, self.odom_list[1][i].orientation.y, self.odom_list[1][i].orientation.z, self.odom_list[1][i].orientation.w))
offset_odom_x = self.robot_odom.pose.pose.position.x - self.odom_list[1][i].position.x
offset_odom_y = self.robot_odom.pose.pose.position.y - self.odom_list[1][i].position.y
offset_odom_z = self.robot_odom.pose.pose.position.z - self.odom_list[1][i].position.z
offset_odom_rz = current_euler[2] - bench_euler[2]
# Remove out dated elements after calculating offset
self.list_cleaning = True
self.odom_list[0] = self.odom_list[0][i+1:]
self.odom_list[1] = self.odom_list[1][i+1:]
self.list_cleaning = False
try:
odo_utm_trans = self.tfBuffer.lookup_transform(self.gps_frame, self.odom_frame, rospy.Time())
odo_utm_euler = tf.transformations.euler_from_quaternion((odo_utm_trans.transform.rotation.x, odo_utm_trans.transform.rotation.y, odo_utm_trans.transform.rotation.z, odo_utm_trans.transform.rotation.w))
theta = odo_utm_euler[2]
robot_pose.position.x += offset_odom_x * math.cos(theta) - offset_odom_y * math.sin(theta)
robot_pose.position.y += offset_odom_x * math.sin(theta) + offset_odom_y * math.cos(theta)
robot_pose.position.z += offset_odom_z
robot_pose.orientation = quat_rot(robot_pose.orientation,0,0,math.degrees(offset_odom_rz))
debug_info(self.debug_output, "Odometry offset calculated")
except:
debug_info(self.debug_output, "Initialise odometry frame in utm")
# odom to reference
while True:
try:
odo_ref_trans = self.tfBuffer.lookup_transform(self.robot_frame, self.odom_frame, rospy.Time())
tf_odo_ref = TransformStamped()
tf_odo_ref.header.frame_id = "odom_utm_calib"
tf_odo_ref.child_frame_id = self.odom_frame
tf_odo_ref.header.stamp = rospy.Time.now()
tf_odo_ref.transform = odo_ref_trans.transform
tfmsg_odo_ref = tf2_msgs.msg.TFMessage([tf_odo_ref])
self.tf_pub.publish(tfmsg_odo_ref)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
debug_info(self.debug_output, "Publishing odometry calibration")
continue
# reference to utm
tf_ref_utm = TransformStamped()
tf_ref_utm.header.frame_id = self.gps_frame
tf_ref_utm.child_frame_id = "odom_utm_calib"
tf_ref_utm.header.stamp = rospy.Time.now()
tf_ref_utm.transform.translation.x = robot_pose.position.x
tf_ref_utm.transform.translation.y = robot_pose.position.y
tf_ref_utm.transform.translation.z = robot_pose.position.z
tf_ref_utm.transform.rotation = robot_pose.orientation
tfmsg_ref_utm = tf2_msgs.msg.TFMessage([tf_ref_utm])
self.tf2_pub.publish(tfmsg_ref_utm)
self.rate.sleep()
# Check the tf exists correctly
try:
trans = self.tfBuffer.lookup_transform(self.gps_frame, "odom_utm_calib", rospy.Time())
trans2 = self.tfBuffer.lookup_transform("odom_utm_calib", self.odom_frame, rospy.Time())
if math.fabs(trans.transform.translation.x - robot_pose.position.x) > 0.1 or math.fabs(trans.transform.translation.y - robot_pose.position.y) > 0.1:
continue
self.odom_calibrating = False
debug_info(self.debug_output, "Odometry calibrated")
break
except:
continue
self.verify_stamp = tf_odo_ref.header.stamp
def transCB(self, t):
for fid_trans in t.transforms:
# Check the image error
if fid_trans.image_error > 0.3:
return
# Check whether detected fiducial is in the map
for fid_gps in self.fiducial_gps_map.fiducials:
# Only process the detected fiducial in the map
if fid_trans.fiducial_id != fid_gps.fiducial_id:
continue
self.reference_id = fid_trans.fiducial_id
fid_name = str(self.reference_id)
# Keep publishing tf until it is created
while True:
# Publish tf from fid to camera
tf_fid_cam = TransformStamped()
tf_fid_cam.header.frame_id = self.camera_frame
tf_fid_cam.child_frame_id = fid_name
tf_fid_cam.header.stamp = rospy.Time.now()
tf_fid_cam.transform = fid_trans.transform
tfmsg_fid_cam = tf2_msgs.msg.TFMessage([tf_fid_cam])
self.tf_pub.publish(tfmsg_fid_cam)
# Publish tf from fid to utm
tf_fid_utm = TransformStamped()
tf_fid_utm.header.frame_id = self.gps_frame
tf_fid_utm.child_frame_id = "fiducial" + fid_name
tf_fid_utm.header.stamp = rospy.Time.now()
tf_fid_utm.transform.translation.x, tf_fid_utm.transform.translation.y = self.projection(
fid_gps.x, fid_gps.y)
tf_fid_utm.transform.translation.z = fid_gps.z
quat = tf.transformations.quaternion_from_euler(
-math.radians(fid_gps.rx), -math.radians(fid_gps.ry),
-math.radians(fid_gps.rz))
tf_fid_utm.transform.rotation.x = quat[0]
tf_fid_utm.transform.rotation.y = quat[1]
tf_fid_utm.transform.rotation.z = quat[2]
tf_fid_utm.transform.rotation.w = quat[3]
tfmsg_fid_utm = tf2_msgs.msg.TFMessage([tf_fid_utm])
self.tf_pub.publish(tfmsg_fid_utm)
# Publish tf from robot to fid
try:
robot_fid_trans = self.tfBuffer.lookup_transform(
fid_name, self.robot_frame, rospy.Time())
tf_robot_fid = TransformStamped()
tf_robot_fid.header.frame_id = "fiducial" + fid_name
tf_robot_fid.child_frame_id = "robot_fid"
tf_robot_fid.header.stamp = rospy.Time.now()
tf_robot_fid.transform = robot_fid_trans.transform
tfmsg_robot_fid = tf2_msgs.msg.TFMessage(
[tf_robot_fid])
self.tf_pub.publish(tfmsg_robot_fid)
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
debug_info(self.debug_output,
"Creating tf from robot to fid")
continue
# Publish tf from robot to utm
try:
verify_stamp = tf_fid_utm.header.stamp
robot_utm_trans = self.tfBuffer.lookup_transform(
self.gps_frame, "robot_fid", rospy.Time())
if verify_stamp > robot_utm_trans.header.stamp:
debug_info(self.debug_output,
"Looking up transformations")
continue
robot_gps_pose = Odometry()
robot_gps_pose.header.stamp = t.header.stamp # Important to apply offset
robot_gps_pose.pose.pose.position.z = robot_utm_trans.transform.translation.z
robot_gps_pose.pose.pose.position.x, robot_gps_pose.pose.pose.position.y = self.projection(
robot_utm_trans.transform.translation.x,
robot_utm_trans.transform.translation.y,
inverse=True)
robot_gps_pose.pose.pose.orientation.x = -robot_utm_trans.transform.rotation.x
robot_gps_pose.pose.pose.orientation.y = -robot_utm_trans.transform.rotation.y
robot_gps_pose.pose.pose.orientation.z = -robot_utm_trans.transform.rotation.z
robot_gps_pose.pose.pose.orientation.w = robot_utm_trans.transform.rotation.w
robot_gps_pose.pose.pose.orientation = quat_rot(
robot_gps_pose.pose.pose.orientation, 0, 0, 90)
self.robot_gps_pub.publish(robot_gps_pose)
debug_info(self.debug_output,
"Fiducial position updated")
break
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
debug_info(self.debug_output, "Updating the position")
continue
