def poseCB(self, p):
self.robot_pose = p.pose.pose
self.robot_pose.position.x, self.robot_pose.position.y = self.projection(
p.pose.pose.position.x, p.pose.pose.position.y)
self.robot_pose.orientation.x = -p.pose.pose.orientation.x
self.robot_pose.orientation.y = -p.pose.pose.orientation.y
self.robot_pose.orientation.z = -p.pose.pose.orientation.z
self.robot_pose.orientation = quat_rot(self.robot_pose.orientation, 0,
0, 90)
self.pose_get = True
self.orentation_get = True
if not self.goal_set:
return
self.distance = math.sqrt(
(self.goal.x - self.robot_pose.position.x)**2 +
(self.goal.y - self.robot_pose.position.y)**2)
self.z_dist = self.robot_pose.position.z - self.goal.z
robot_euler = tf.transformations.euler_from_quaternion(
(self.robot_pose.orientation.x, self.robot_pose.orientation.y,
self.robot_pose.orientation.z, self.robot_pose.orientation.w))
self.roll = 0
self.pitch = math.atan2(
self.goal.z - self.robot_pose.position.z,
math.sqrt((self.goal.x - self.robot_pose.position.x)**2 +
(self.goal.y -
self.robot_pose.position.y)**2)) - robot_euler[1]
self.yaw = math.atan2(
self.goal.y - self.robot_pose.position.y,
self.goal.x - self.robot_pose.position.x) - robot_euler[2]
self.roll = fit_in_rad(self.roll)
self.pitch = fit_in_rad(self.pitch)
self.yaw = fit_in_rad(self.yaw)
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 Start(self):
while not rospy.is_shutdown():
if self.odom_calibrating:
self.rate.sleep()
continue
try:
trans = self.tfBuffer.lookup_transform(self.gps_frame,
self.odom_frame,
rospy.Time())
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):
continue
self.rate.sleep()
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 Start(self):
while not rospy.is_shutdown():
if not self.vel_received:
self.rate.sleep()
continue
if not self.pose_received:
self.rate.sleep()
continue
pose = self.robot_pose
pose.position.x, pose.position.y = self.projection(
self.robot_pose.position.x, self.robot_pose.position.y)
pose.orientation.x = -self.robot_pose.orientation.x
pose.orientation.y = -self.robot_pose.orientation.y
pose.orientation.z = -self.robot_pose.orientation.z
pose.orientation = quat_rot(pose.orientation, 0, 0, 90)
v = self.vel.linear.x
w = self.vel.angular.z
alpha = w * self.dt / 2
euler = tf.transformations.euler_from_quaternion(
(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w))
theta = euler[2]
if w == 0:
d = v * self.dt
else:
r = v / w
d = 2 * r * math.sin(alpha)
pose.position.x += d * math.cos(alpha + theta)
pose.position.y += d * math.sin(alpha + theta)
pose.orientation = quat_rot(pose.orientation, 0, 0,
math.degrees(alpha))
pose.orientation.x = -pose.orientation.x
pose.orientation.y = -pose.orientation.y
pose.orientation.z = -pose.orientation.z
pose.orientation = quat_rot(pose.orientation, 0, 0, 90)
self.robot_prediction.pose.pose = pose
self.robot_prediction.pose.pose.position.x, self.robot_prediction.pose.pose.position.y = self.projection(
pose.position.x, pose.position.y, inverse=True)
self.pred_pub.publish(self.robot_prediction)
self.vel_received = False
self.rate.sleep()
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 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 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