def _wait_for_tf_init(self):
"""
Waits until all needed frames are present in tf.
:rtype: None
"""
self.tfBuffer.lookup_transform(
self.handeye_parameters.robot_base_frame,
self.handeye_parameters.robot_effector_frame, Time(0),
Duration(20))
self.tfBuffer.lookup_transform(
self.handeye_parameters.tracking_base_frame,
self.handeye_parameters.tracking_marker_frame, Time(0),
Duration(60))
def get_begin_end(time_rules, bag_file):
# Find the min/max relative time (0 at the beginning of the bag file)
t_min = None
t_max = None
for r in time_rules:
if r.is_time() and r.is_include():
if t_min is None or r.token_from < t_min:
t_min = r.token_from
if t_max is None or r.token_to > t_max:
t_max = r.token_to
t_start, t_end = bag_file.get_start_time(), bag_file.get_end_time()
t_min = t_start if t_min is None else t_min + t_start
t_max = t_end if t_max is None else t_max + t_start
return Time(t_min), Time(t_max)
def get_closest_qr(self, qrs):
print qrs
closet_qr = qrs[0]
min_distance = 1000
if len(qrs) == 1:
return closet_qr
# self.current_pose = self.explorer.pose_stamped.pose
for qr in qrs:
try:
(trans, rot) = self.transform_listener.lookupTransform(
qr.frame_id, '/base_footprint', Time(3))
except:
log.error(
"Transform listener failed to acquire transformation")
return closet_qr
current_pose = Pose()
current_pose.position.x = trans[0]
current_pose.position.y = trans[1]
current_pose.position.z = trans[2]
target_pose = qr.qrPose.pose
target_distance = distance_2d(target_pose, current_pose)
if target_distance < min_distance:
min_distance = target_distance
closet_qr = qr
return closet_qr
def make_marker_stub(self, stamped_pose, size=None, color=None):
if color is None:
color = (0.5, 0.5, 0.5, 1.0)
if size is None:
size = (1, 1, 1)
marker = Marker()
marker.header = stamped_pose.header
# marker.ns = "collision_scene"
marker.id = self.next_id
marker.lifetime = Time(0) # forever
marker.action = Marker.ADD
marker.pose = stamped_pose.pose
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
if len(color) == 4:
alpha = color[3]
else:
alpha = 1.0
marker.color.a = alpha
marker.scale.x = size[0]
marker.scale.y = size[1]
marker.scale.z = size[2]
self.next_id += 1
return marker
def choose_target(self, targets):
""" Choose the neareset possible target. """
# Should never be called with empty targets.
if not targets:
log.error('choose_target was called with no targets.')
return None
closest_target = targets[0]
min_distance = 1000
if len(targets) == 1:
return closest_target
# self.current_pose = self.explorer.pose_stamped.pose
for target in targets:
try:
(trans, rot) = self.transform_listener.lookupTransform(target.victimPose.header.frame_id,
'/base_footprint',
Time(0))
except:
log.error("Transform listener failed to acquire transformation")
return closest_target
self.current_pose = Pose()
self.current_pose.position.x = trans[0]
self.current_pose.position.y = trans[1]
self.current_pose.position.z = trans[2]
target_pose = target.victimPose.pose
target_distance = distance_2d(target_pose, self.current_pose)
if target_distance < min_distance:
min_distance = target_distance
closest_target = target
return closest_target
def dicttopath(dic):
path = Path()
path.header.frame_id = dic["frame_id"]
for point in dic["points"]:
path.poses.append(list_to_pose(point["pose"],
frame_id=dic["frame_id"]))
path.poses[-1].header.stamp = Time(point["time"])
return path
def get_relative_state(self):
# the source and target frames have been changed here b/c
# we dont want to find the position of nhbr wrt to 'this' (self's) frame
# Maybe implement twist transform ...
state = {}
for nhbr in self.neighbours:
source_frame = "{}_{}/base_link".format(self.model, self._id)
target_frame = "{}_{}/base_link".format(self.model, nhbr)
Ptransform = self.tf_listener.lookupTransform(
source_frame, target_frame, Time())
state[nhbr] = Ptransform
return state
def getImg(self, index=0):
i = index
st = Time(self._bag_timestamp[i].secs,
self._bag_timestamp[i].nsecs - 1e6)
if i > 0:
st = Time(self._bag_timestamp[i - 1].secs,
self._bag_timestamp[i - 1].nsecs + 1e6)
et = Time(self._bag_timestamp[i].secs, self._bag_timestamp[i].nsecs)
for topic, msg, t in self._bag.read_messages(topics=self.msg,
start_time=st,
end_time=et):
if topic == self.msg[0]:
self.img_0 = self.bridge.imgmsg_to_cv2(msg, self.img_type)
# self.img_prv[:, :self.img_w] = self.bridge.imgmsg_to_cv2(msg, self.img_type)
if topic == self.msg[1]:
self.img_1 = self.bridge.imgmsg_to_cv2(msg, self.img_type)
# self.img_prv[:, self.img_w:(self.img_w * 2)] = self.bridge.imgmsg_to_cv2(msg, self.img_type)
# if topic == '/odometry':
# odom = msg
return self.img_0, self.img_1
def join_tree_srv_function(self, command):
if self.debug:
print('\x1b[33;1m' + str('Joining the Two Separate Frame Tree!') +
'\x1b[0m')
# Set Flag to True
self.join_trees = command.status
# Check if we can calculate the TF Between Drones Bottom Camera and Ar Tag
while not self.tf_listen.can_transform(
self.drone_odom_frame, self.map_frame, Time(0), Duration(1.0)):
pass
# Return From Service Once Tree is Joint
return True
def calculate_error_from_tag(self):
# Calculate_TF and get Pose Error
if self.can_transform() and self.marker_detected:
# Get the most recent TF's
tf_ = self.tf.lookup_transform(self.ar_pose_frame,
self.drone_camera_frame, Time(0),
Duration(1.0))
h_tf = self.tf.lookup_transform(self.drone_base_footprint,
self.drone_base_link, Time(0),
Duration(1.0))
# The Height that Drone is currently fly
height = h_tf.transform.translation.z
# Calculate Yaw from TF
(_, _, yaw) = euler_from_quaternion([
tf_.transform.rotation.x, tf_.transform.rotation.y,
tf_.transform.rotation.z, tf_.transform.rotation.w
])
yaw += pi
# Manipulate Yaw to Rotate Drone in the Right Direction
if np.rad2deg(yaw) > 180:
yaw -= 2 * pi
# Get the Rotation Matrix
(r1, r2, r3, _) = quaternion_matrix([
tf_.transform.rotation.x, tf_.transform.rotation.y,
tf_.transform.rotation.z, tf_.transform.rotation.w
])
rotation_mat = np.asmatrix([r1[0:3], r2[0:3], r3[0:3]])
# Get Position in Matrix Form
pose_mat = np.asmatrix([[tf_.transform.translation.x],
[tf_.transform.translation.y],
[tf_.transform.translation.z]])
# Calculate the Cartesian Error
error = rotation_mat * pose_mat
return [error.item(0), error.item(1), error.item(2), yaw, height]
else:
return [-1, -1, -1, -1, -1]
def str_to_ros_time(string):
# time module supports up to microsecond resolution (ROS standard is nanosecond)
# parse date
no_nsec = string[:23] if len(string) > 23 else string
utc_time = time.strptime(no_nsec, "UTC_%Y/%m/%d_%H:%M:%S")
epoch_time = timegm(utc_time)
# compute nanoseconds
nsecs_str = string[24:] if len(string) > 24 else string
nsecs = int(nsecs_str) * 10**(9 - len(nsecs_str))
return Time(epoch_time, nsecs)
def pose_error(self, x, y, z):
target_to_map = PoseStamped()
target_to_map.pose.position.x = x
target_to_map.pose.position.y = y
target_to_map.pose.position.z = z
transform = self.tf.lookup_transform(self.drone_base_link,
self.map_frame, Time(0),
Duration(1.0))
error = do_transform_pose(target_to_map, transform)
return error.pose.position.x, error.pose.position.y, error.pose.position.z
def get_obs(self, apply_preprocessor=True):
t = time.time()
print(self, 'GET_OBS')
obs = super().get_obs(apply_preprocessor=False)
print('GOT OTHER OBS, WAITING FOR TF ...')
src = '/{}/base_link'.format(self.ns)
nbhrs = list(self.tf_obs.keys())
while nbhrs:
nbhr = nbhrs[-1]
try:
tgt = '/{}/base_link'.format(nbhr)
self.tf_obs[nbhr] = self.tfl.lookupTransform(src, tgt, Time())
self._is_obs_ready[nbhr] = True
nbhrs.pop()
except:
print('unable2tf b/w {} & {}'.format(nbhr, self.ns))
continue
obs.update(self.tf_obs)
print('{}: Time taken for obs: {}'.format(self.ns, time.time() - t))
if apply_preprocessor and self.preprocessor is not None:
return self.preprocessor(obs)
return obs
def can_transform(self):
if self.tf.can_transform(self.ar_pose_frame, self.drone_camera_frame,
Time(0), Duration(1.0)):
return True
else:
return False
def test_Time(self):
# This is a copy of test_roslib_rostime
from rospy import Time, Duration
# #1600 Duration > Time should fail
failed = False
try:
v = Duration.from_sec(0.1) > Time.from_sec(0.5)
failed = True
except:
pass
self.failIf(failed, "should have failed to compare")
try:
v = Time.from_sec(0.4) > Duration.from_sec(0.1)
failed = True
except:
pass
self.failIf(failed, "should have failed to compare")
try: # neg time fails
Time(-1)
failed = True
except:
pass
self.failIf(failed, "negative time not allowed")
try:
Time(1, -1000000001)
failed = True
except:
pass
self.failIf(failed, "negative time not allowed")
# test Time.now() is within 10 seconds of actual time (really generous)
import time
t = time.time()
v = Time.from_sec(t)
self.assertEquals(v.to_sec(), t)
# test from_sec()
self.assertEquals(Time.from_sec(0), Time())
self.assertEquals(Time.from_sec(1.), Time(1))
self.assertEquals(Time.from_sec(v.to_sec()), v)
self.assertEquals(v.from_sec(v.to_sec()), v)
# test to_time()
self.assertEquals(v.to_sec(), v.to_time())
# test addition
# - time + time fails
try:
v = Time(1, 0) + Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Time + Time must fail")
# - time + duration
v = Time(1, 0) + Duration(1, 0)
self.assertEquals(Time(2, 0), v)
v = Duration(1, 0) + Time(1, 0)
self.assertEquals(Time(2, 0), v)
v = Time(1, 1) + Duration(1, 1)
self.assertEquals(Time(2, 2), v)
v = Duration(1, 1) + Time(1, 1)
self.assertEquals(Time(2, 2), v)
v = Time(1) + Duration(0, 1000000000)
self.assertEquals(Time(2), v)
v = Duration(1) + Time(0, 1000000000)
self.assertEquals(Time(2), v)
v = Time(100, 100) + Duration(300)
self.assertEquals(Time(400, 100), v)
v = Duration(300) + Time(100, 100)
self.assertEquals(Time(400, 100), v)
v = Time(100, 100) + Duration(300, 300)
self.assertEquals(Time(400, 400), v)
v = Duration(300, 300) + Time(100, 100)
self.assertEquals(Time(400, 400), v)
v = Time(100, 100) + Duration(300, -101)
self.assertEquals(Time(399, 999999999), v)
v = Duration(300, -101) + Time(100, 100)
self.assertEquals(Time(399, 999999999), v)
# test subtraction
try:
v = Time(1, 0) - 1
failed = True
except:
pass
self.failIf(failed, "Time - non Duration must fail")
class Foob(object):
pass
try:
v = Time(1, 0) - Foob()
failed = True
except:
pass
self.failIf(failed, "Time - non TVal must fail")
# - Time - Duration
v = Time(1, 0) - Duration(1, 0)
self.assertEquals(Time(), v)
v = Time(1, 1) - Duration(-1, -1)
self.assertEquals(Time(2, 2), v)
v = Time(1) - Duration(0, 1000000000)
self.assertEquals(Time(), v)
v = Time(2) - Duration(0, 1000000000)
self.assertEquals(Time(1), v)
v = Time(400, 100) - Duration(300)
self.assertEquals(Time(100, 100), v)
v = Time(100, 100) - Duration(0, 101)
self.assertEquals(Time(99, 999999999), v)
# - Time - Time = Duration
v = Time(100, 100) - Time(100, 100)
self.assertEquals(Duration(), v)
v = Time(100, 100) - Time(100)
self.assertEquals(Duration(0, 100), v)
v = Time(100) - Time(200)
self.assertEquals(Duration(-100), v)
def test_Duration(self):
Duration = rospy.Duration
# test from_sec
v = Duration(1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
v = Duration(0, 1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
# test neg
v = -Duration(1, -1)
self.assertEquals(-1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, -1)
self.assertEquals(1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(999999999, v.nsecs)
# test addition
failed = False
try:
v = Duration(1, 0) + Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Duration + Time must fail")
try:
v = Duration(1, 0) + 1
failed = True
except:
pass
self.failIf(failed, "Duration + int must fail")
v = Duration(1, 0) + Duration(1, 0)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2, 0), v)
v = Duration(-1, -1) + Duration(1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(), v)
v = Duration(1) + Duration(0, 1000000000)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2), v)
v = Duration(100, 100) + Duration(300)
self.assertEquals(Duration(400, 100), v)
v = Duration(100, 100) + Duration(300, 300)
self.assertEquals(Duration(400, 400), v)
v = Duration(100, 100) + Duration(300, -101)
self.assertEquals(Duration(399, 999999999), v)
# test subtraction
try:
v = Duration(1, 0) - 1
failed = True
except:
pass
self.failIf(failed, "Duration - non duration must fail")
try:
v = Duration(1, 0) - Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Duration - Time must fail")
v = Duration(1, 0) - Duration(1, 0)
self.assertEquals(Duration(), v)
v = Duration(-1, -1) - Duration(1, 1)
self.assertEquals(Duration(-3, 999999998), v)
v = Duration(1) - Duration(0, 1000000000)
self.assertEquals(Duration(), v)
v = Duration(2) - Duration(0, 1000000000)
self.assertEquals(Duration(1), v)
v = Duration(100, 100) - Duration(300)
self.assertEquals(Duration(-200, 100), v)
v = Duration(100, 100) - Duration(300, 101)
self.assertEquals(Duration(-201, 999999999), v)
# test abs
self.assertEquals(abs(Duration()), Duration())
self.assertEquals(abs(Duration(1)), Duration(1))
self.assertEquals(abs(Duration(-1)), Duration(1))
self.assertEquals(abs(Duration(0, -1)), Duration(0, 1))
self.assertEquals(abs(Duration(-1, -1)), Duration(1, 1))
img_prv = np.zeros([img_h, img_w * 2, 3], dtype=np.uint8)
img = np.zeros([img_h, img_w, 3], dtype=np.uint8)
t_start = bag.get_start_time()
s_img0 = bag.get_message_count(topic_filters=[ros_msg[0]])
s_img1 = bag.get_message_count(topic_filters=[ros_msg[2]])
ros_t = []
ros_t.clear()
for topic, msg, t in bag.read_messages(topics=[ros_msg[0]]):
ros_t.append(t)
# pd.DataFrame(np.array(ros_t)).to_csv("viode_timestamp.csv")
for i in range(int(s_img0)):
st = Time(ros_t[i].secs, ros_t[i].nsecs - 1e6)
if i > 0: st = Time(ros_t[i - 1].secs, ros_t[i - 1].nsecs + 1e6)
et = Time(ros_t[i].secs, ros_t[i].nsecs)
# print(et.to_nsec() - st.to_nsec())
for topic, msg, t in bag.read_messages(topics=ros_msg,
start_time=st,
end_time=et):
if topic == ros_msg[0]:
img = bridge.imgmsg_to_cv2(msg, "bgr8")
# if topic == ros_msg[1]:
# img_prv[img_h:, :img_w, :] = bridge.imgmsg_to_cv2(msg, "bgr8")
...
# if topic == ros_msg[2]:
def loop(self):
"""Running loop to determine if the base link is moving."""
old_pose = PoseStamped()
old_pose.header.frame_id = self.params['base_frame']
old_pose.pose.orientation.w = 1.0
new_pose = PoseStamped()
new_pose.header.frame_id = self.params['base_frame']
new_pose.pose.orientation.w = 1.0
num_stationary = 0
total_time = self.params['loop_rate'] * self.params['num_stationary']
nav_goal = MoveBaseGoal()
Me.info_message("Starting loop")
while not is_shutdown():
try:
self.tfl.waitForTransform(self.params['world_frame'],
self.params['base_frame'], Time(0),
Duration(2))
new_pose = PoseStamped()
new_pose.header.frame_id = self.params['base_frame']
new_pose.pose.orientation.w = 1.0
new_pose = self.tfl.transformPose(self.params['world_frame'],
new_pose)
dist = \
sqrt((new_pose.pose.position.x -
old_pose.pose.position.x) ** 2.0 +
(new_pose.pose.position.y -
old_pose.pose.position.y) ** 2.0)
if self.params['debug']:
Me.info_message("New Pose: " + str(new_pose))
Me.info_message("Old Pose: " + str(old_pose))
Me.info_message("Dist: " + str(dist))
if dist <= self.params['tolerance']:
num_stationary += 1
elif dist > self.params['tolerance']:
# Movement occurred, zero number
num_stationary = 0
else:
Me.error_message("Error with distance/tolerance "
"comparision.")
raise KeyboardInterrupt
if num_stationary >= self.params['num_stationary']:
Me.info_message(
str(self.params['base_frame']) + " has "
"been stationary for required " + str(total_time) +
" seconds.")
if self.params['debug']:
Me.info_message("Polygons in map: " +
str(len(self.map.polygons)))
Me.info_message("Right_left: " +
str(self.params['right_left']))
# Call Adam's function (self.map, self.params['right_left'],
# new_pose)
# self.turtlebot_ac.send_goal(nav_goal)
nav_goal.target_pose = \
get_viewpoint(self.map, self.params['right_left'],
new_pose)
self.turtlebot_ac.send_goal(goal=nav_goal)
else:
current_time = self.params['loop_rate'] * num_stationary
Me.info_message(
str(self.params['base_frame']) + " has been"
" stationary for " + str(current_time) +
" seconds but " + str(total_time) +
" seconds are needed.")
# Store old pose
old_pose = deepcopy(new_pose)
# Wait before getting next pose
sleep(self.params['loop_rate'])
except TfExcept:
if self.params['debug']:
Me.info_message('Transform exception')
raise TfExcept
except KeyboardInterrupt:
break
return
def stampToTime(stamp):
return Time(secs=stamp.secs, nsecs=stamp.nsecs)
def update_info_text(self):
"""
update the displayed info text
"""
if not self._show_info:
return
try:
if ROS_VERSION == 1:
(position, rotation) = self.tf_listener.lookupTransform(
'/map', self.role_name, Time())
elif ROS_VERSION == 2:
transform = self.tf_buffer.lookup_transform(
target_frame='map', source_frame=self.role_name, time=Time())
position = [transform.transform.translation.x,
transform.transform.translation.y,
transform.transform.translation.z]
rotation = [transform.transform.rotation.w,
transform.transform.rotation.x,
transform.transform.rotation.y,
transform.transform.rotation.z]
_, _, yaw = quat2euler(rotation)
yaw = math.degrees(yaw)
x = position[0]
y = position[1]
z = position[2]
except (LookupException, ConnectivityException, ExtrapolationException):
x = 0
y = 0
z = 0
yaw = 0
heading = 'N' if abs(yaw) < 89.5 else ''
heading += 'S' if abs(yaw) > 90.5 else ''
heading += 'E' if 179.5 > yaw > 0.5 else ''
heading += 'W' if -0.5 > yaw > -179.5 else ''
fps = 0
time = str(datetime.timedelta(seconds=self.node.get_time()))[:10]
if self.carla_status.fixed_delta_seconds:
fps = 1 / self.carla_status.fixed_delta_seconds
self._info_text = [
'Frame: % 22s' % self.carla_status.frame,
'Simulation time: % 12s' % time,
'FPS: % 24.1f' % fps, '',
'Vehicle: % 20s' % ' '.join(self.vehicle_info.type.title().split('.')[1:]),
'Speed: % 15.0f km/h' % (3.6 * self.vehicle_status.velocity),
u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (yaw, heading),
'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (x, y)),
'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (self.latitude, self.longitude)),
'Height: % 18.0f m' % z, ''
]
self._info_text += [
('Throttle:', self.vehicle_status.control.throttle, 0.0, 1.0),
('Steer:', self.vehicle_status.control.steer, -1.0, 1.0),
('Brake:', self.vehicle_status.control.brake, 0.0, 1.0),
('Reverse:', self.vehicle_status.control.reverse),
('Hand brake:', self.vehicle_status.control.hand_brake),
('Manual:', self.vehicle_status.control.manual_gear_shift),
'Gear: %s' % {
-1: 'R',
0: 'N'
}.get(self.vehicle_status.control.gear, self.vehicle_status.control.gear), ''
]
self._info_text += [('Manual ctrl:', self.manual_control)]
if self.carla_status.synchronous_mode:
self._info_text += [('Sync mode running:', self.carla_status.synchronous_mode_running)]
self._info_text += ['', '', 'Press <H> for help']
