def main():
rospy.init_node('find_waypoints')
joy_subscriber = SubscriberValue('joy', Joy)
tf_listener = TransformListener()
names = [
'initial_pose', 'off_ramp_start', 'off_ramp_end', 'ar_search_1',
'ar_search_2', 'S1', 'S2', 'S3', 'S4', 'S5', 'S6', 'S7', 'S8',
'on_ramp'
]
coords = []
for name in names:
rospy.sleep(rospy.Duration(2))
print(name)
while not joy_subscriber.value.buttons[0]:
rospy.sleep(rospy.Duration(0, 1000))
pose = tf_listener.lookupTransform('/map', '/base_link', rospy.Time())
coords.append(pose)
print('Saved')
out_file = open(
path.join(path.dirname(__file__), '..', 'param',
'find_waypoints_generated.yaml'), 'w')
out_file.write('named_poses:\n')
for name, ((x, y, z), (rx, ry, rz, rw)) in zip(names, coords):
out_file.write(' {name}:\n'.format(name=name))
out_file.write(' position:\n')
out_file.write(' - {x}\n'.format(x=x))
out_file.write(' - {y}\n'.format(y=y))
out_file.write(' - {z}\n'.format(z=z))
out_file.write(' orientation:\n')
out_file.write(' - {x}\n'.format(x=rx))
out_file.write(' - {y}\n'.format(y=ry))
out_file.write(' - {z}\n'.format(z=rz))
out_file.write(' - {w}\n'.format(w=rw))
class ToolPose(object):
def __init__(self):
self.pose = PoseStamped()
self._tf_listener = TransformListener()
#self.pose_pub = rospy.Publisher('tool_pose', PoseStamped, queue_size=10)
#self.joint_sub = rospy.Subscriber('joint_states', JointState, self.tool_pose_callback)
self.joint_srv = rospy.Service('tool_pose', GetToolPose,
self.tool_pose_callback)
def tool_pose_callback(self, req):
''' get gripper link pose with respect to base_link'''
res = GetToolPoseResponse()
time = 0
trans = None
rot = None
self.pose.header.frame_id = 'base_link'
while not rospy.is_shutdown():
try:
time = self._tf_listener.getLatestCommonTime(
'base_link', 'wrist_roll_link')
trans, rot = self._tf_listener.lookupTransform(
'base_link', 'wrist_roll_link', time)
break
except (tf.Exception, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
continue
self.pose.pose.position.x = round(trans[0], 4)
self.pose.pose.position.y = round(trans[1], 4)
self.pose.pose.position.z = round(trans[2], 4)
self.pose.pose.orientation.x = round(rot[0], 4)
self.pose.pose.orientation.y = round(rot[1], 4)
self.pose.pose.orientation.z = round(rot[2], 4)
self.pose.pose.orientation.w = round(rot[3], 4)
#self.pose_pub.publish(self.pose)
res.tool_pose = self.pose
return res
class abbRobot:
errorDict = {
0: "Successful",
-1: "Invalid goal",
-2: "Invalid joints",
-3: "Old header timestamp",
-4: "Path tolerance violated",
-5: "Goal tolerance violated"
}
def __init__(self):
self.manip = mic.MoveGroupCommander("manipulator")
self.client = act.SimpleActionClient('joint_trajectory_action',
trajAction)
rp.loginfo("Waiting for server joint_trajectory_action.")
self.client.wait_for_server()
rp.loginfo("Successfuly connected to server.")
self.tfListener = TransformListener()
def getEEPoint(self, start_link='base_link', end_link="tool0"):
self.tfListener.waitForTransform(start_link, end_link, rp.Time(),
rp.Duration(0.5))
ptData = self.tfListener.lookupTransform(start_link, end_link,
rp.Time())
pts = [round(pt, 5) for pt in ptData[0]]
orient = [round(pt, 5) for pt in ptData[1]]
return pts, orient
def move2Point(self,
points,
eAngles=[[0, 0, 0]],
ax='sxyz',
end_effector='link_6'):
"""
Moves to a point using end effector point space.\n
Usage:\n
- points - list of lists that contain x,y,z coordinates\n
- eAngles - default [[0,0,0]]\n
- list of lists that contain a,b,g euler angles
- if multiple points are passed and only one list of euler angles then those angles are going to be used for all points
- ax - specify convention to use for euler angles
- default: 'sxyz'
- end_effector - default link_6
- link whose point you want to move
"""
print "Number of points recieved: ", len(points)
t1 = t.time()
if (len(points) == len(eAngles) or len(eAngles) == 1):
for i in xrange(len(points)):
if rp.is_shutdown():
print "ROS has been shutdown. Exiting..."
break
print i + 1, ". point:", [round(pt, 5) for pt in points[i]]
p = Point(points[i][0], points[i][1], points[i][2])
index = i
if len(eAngles) == 1:
index = 0
orient = Quaternion(*qEuler(eAngles[index][0], eAngles[index]
[1], eAngles[index][2], ax))
self.manip.set_pose_target(Pose(p, orient),
end_effector_link=end_effector)
self.manip.go(True)
rp.loginfo("Moving to multiple points finished.")
self.__displayDuration(t1, t.time())
else:
print "Number of points recieved does not match number of euler angles received\nneither number of euler angles is 1. Please check the input parameters."
def cartesian2Point(self,
points,
eAngles,
ax='sxyz',
resolution=0.01,
jumpStep=0,
end_effector='link_6'):
"""
Moves to a point in a straight line using end effector point space.\n
Usage:\n
- points - list of lists that contain x,y,z coordinates\n
- eAngles - list that contains a,b,g euler angles for constraint
- ax - specify convention to use for euler angles
- default: 'sxyz'
- resolution - maximum distance between 2 generated points
- default: 0.01
- jumpStep - maximum jump distance between 2 generated points
- default: 0
- end_effector - link whose point you want to move
- default link_6
"""
print "Number of points recieved: ", len(points)
wpose = Pose()
self.manip.set_end_effector_link(end_effector)
constraint = Constraints()
orientation_constraint = OrientationConstraint()
orientation_constraint.link_name = end_effector
orientation_constraint.orientation = Quaternion(
*qEuler(eAngles[0], eAngles[1], eAngles[2], ax))
constraint.orientation_constraints.append(orientation_constraint)
self.manip.set_path_constraints(constraint)
t1 = t.time()
for pt in points:
if not rp.is_shutdown():
print "Going to point: ", [round(p, 5) for p in pt]
wpose.position.x = pt[0]
wpose.position.y = pt[1]
wpose.position.z = pt[2]
(plan, factor) = self.manip.compute_cartesian_path([wpose],
resolution,
jumpStep)
trajLen = len(plan.joint_trajectory.points)
if trajLen <= 100 and factor == 1.0:
self.manip.execute(plan)
else:
rp.loginfo(
"Error while planning. No execution attempted.\nNo. points planned:{}\nPercentage of path found:{}%"
.format(trajLen, round(factor * 100, 2)))
rp.loginfo("Moving to multiple points finished.")
self.__displayDuration(t1, t.time())
self.manip.set_path_constraints(None)
def jointAction(self, jointAngles):
"""
Moves to a point using joint space\n
Usage:\n
- jointAngles - list of lists that contain 6 angles in order from joint_1 to joint_6
"""
print "Number of joint angles recieved: ", len(jointAngles)
t1 = t.time()
jointGoal = goal()
jointGoal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
jointGoal.trajectory.points.append(JointTrajectoryPoint())
for joint in jointAngles:
print "Going to joint values[degrees]: ", [
round(i * 180 / pi, 2) for i in joint
]
jointGoal.trajectory.points[0].positions = joint
# jointGoal.trajectory.points[0].velocities=[2.,2.,2.,6.,6.,7.]
# jointGoal.trajectory.points[0].accelerations=[1.,1.,1.,1.,1.,1.]
# jointGoal.trajectory.points[0].time_from_start=rp.Duration(2,0)
self.client.send_goal(jointGoal, feedback_cb=self.__feedback)
self.client.wait_for_result()
print "[Result:] ", self.errorDict[
self.client.get_result().error_code]
spent = self.__displayDuration(t1, t.time())
@staticmethod
def __feedback(msg):
print "[Feedback:] ", msg.error
@staticmethod
def __displayDuration(t1, t2):
spent = [(int(t2 - t1) / 60), 0, 0]
spent[1] = int((t2 - t1) - spent[0] * 60)
spent[2] = int(((t2 - t1) - spent[0] * 60 - spent[1]) * 1000)
rp.loginfo("Execution took [min:sec.ms]: %i:%02i.%03i ", spent[0],
spent[1], spent[2])
class DataCollectorAndLabeler:
def __init__(self, output_folder, server, menu_handler, marker_size, calibration_file):
if not os.path.exists(output_folder):
os.mkdir(output_folder) # Create the new folder
else:
while True:
msg = Fore.YELLOW + "To continue, the directory '{}' will be delete.\n"
msg = msg + "Do you wish to continue? [y/N] " + Style.RESET_ALL
answer = raw_input(msg.format(output_folder))
if len(answer) > 0 and answer[0].lower() in ('y', 'n'):
if answer[0].lower() == 'n':
sys.exit(1)
else:
break
else:
sys.exit(1) # defaults to N
shutil.rmtree(output_folder) # Delete old folder
os.mkdir(output_folder) # Recreate the folder
self.output_folder = output_folder
self.listener = TransformListener()
self.sensors = {}
self.sensor_labelers = {}
self.server = server
self.menu_handler = menu_handler
self.data_stamp = 0
self.collections = {}
self.bridge = CvBridge()
self.config = loadJSONConfig(calibration_file)
if self.config is None:
sys.exit(1) # loadJSON should tell you why.
self.world_link = self.config['world_link']
# Add sensors
print(Fore.BLUE + 'Sensors:' + Style.RESET_ALL)
print('Number of sensors: ' + str(len(self.config['sensors'])))
# Go through the sensors in the calib config.
for sensor_key, value in self.config['sensors'].items():
# Create a dictionary that describes this sensor
sensor_dict = {'_name': sensor_key, 'parent': value['link'],
'calibration_parent': value['parent_link'],
'calibration_child': value['child_link']}
# TODO replace by utils function
print("Waiting for message")
msg = rospy.wait_for_message(value['topic_name'], rospy.AnyMsg)
connection_header = msg._connection_header['type'].split('/')
ros_pkg = connection_header[0] + '.msg'
msg_type = connection_header[1]
print('Topic ' + value['topic_name'] + ' has type ' + msg_type)
sensor_dict['topic'] = value['topic_name']
sensor_dict['msg_type'] = msg_type
# If topic contains a message type then get a camera_info message to store along with the sensor data
if sensor_dict['msg_type'] == 'Image': # if it is an image must get camera_info
sensor_dict['camera_info_topic'] = os.path.dirname(sensor_dict['topic']) + '/camera_info'
from sensor_msgs.msg import CameraInfo
camera_info_msg = rospy.wait_for_message(sensor_dict['camera_info_topic'], CameraInfo)
from rospy_message_converter import message_converter
sensor_dict['camera_info'] = message_converter.convert_ros_message_to_dictionary(camera_info_msg)
# Get the kinematic chain form world_link to this sensor's parent link
now = rospy.Time()
self.listener.waitForTransform(value['link'], self.world_link, now, rospy.Duration(5))
chain = self.listener.chain(value['link'], now, self.world_link, now, self.world_link)
chain_list = []
for parent, child in zip(chain[0::], chain[1::]):
key = self.generateKey(parent, child)
chain_list.append({'key': key, 'parent': parent, 'child': child})
sensor_dict['chain'] = chain_list # Add to sensor dictionary
self.sensors[sensor_key] = sensor_dict
sensor_labeler = InteractiveDataLabeler(self.server, self.menu_handler, sensor_dict,
marker_size, self.config['calibration_pattern'])
self.sensor_labelers[sensor_key] = sensor_labeler
print('finished visiting sensor ' + sensor_key)
print(Fore.BLUE + sensor_key + Style.RESET_ALL + ':\n' + str(sensor_dict))
# print('sensor_labelers:')
# print(self.sensor_labelers)
self.abstract_transforms = self.getAllAbstractTransforms()
# print("abstract_transforms = " + str(self.abstract_transforms))
def getTransforms(self, abstract_transforms, time=None):
transforms_dict = {} # Initialize an empty dictionary that will store all the transforms for this data-stamp
if time is None:
time = rospy.Time.now()
for ab in abstract_transforms: # Update all transformations
self.listener.waitForTransform(ab['parent'], ab['child'], time, rospy.Duration(1.0))
(trans, quat) = self.listener.lookupTransform(ab['parent'], ab['child'], time)
key = self.generateKey(ab['parent'], ab['child'])
transforms_dict[key] = {'trans': trans, 'quat': quat, 'parent': ab['parent'], 'child': ab['child']}
return transforms_dict
def lockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.acquire()
print("Locked all labelers")
def unlockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.release()
print("Unlocked all labelers")
def getLabelersTimeStatistics(self):
stamps = [] # a list of the several time stamps of the stored messages
for sensor_name, sensor in self.sensors.iteritems():
stamps.append(copy.deepcopy(self.sensor_labelers[sensor_name].msg.header.stamp))
max_delta = getMaxTimeDelta(stamps)
average_time = getAverageTime(stamps) # For looking up transforms use average time of all sensor msgs
print('Times:')
for stamp in stamps:
printRosTime(stamp)
return stamps, average_time, max_delta
def saveCollection(self):
# --------------------------------------
# Collect sensor data and labels (images, laser scans, etc)
# --------------------------------------
# Lock the semaphore for all labelers
self.lockAllLabelers()
# Analyse message time stamps and decide if collection can be stored
stamps, average_time, max_delta = self.getLabelersTimeStatistics()
if max_delta.to_sec() > float(self.config['max_duration_between_msgs']): # times are close enough?
rospy.logwarn('Max duration between msgs in collection is ' + str(max_delta.to_sec()) +
'. Not saving collection.')
self.unlockAllLabelers()
return None
else: # test passed
rospy.loginfo('Max duration between msgs in collection is ' + str(max_delta.to_sec()))
# Collect all the transforms
transforms = self.getTransforms(self.abstract_transforms, average_time) # use average time of sensor msgs
printRosTime(average_time, "Collected transforms for time ")
all_sensor_data_dict = {}
all_sensor_labels_dict = {}
for sensor_name, sensor in self.sensors.iteritems():
print('collect sensor: ' + sensor_name)
msg = copy.deepcopy(self.sensor_labelers[sensor_name].msg)
labels = copy.deepcopy(self.sensor_labelers[sensor_name].labels)
# TODO add exception also for point cloud and depth image
# Update sensor data ---------------------------------------------
if sensor['msg_type'] == 'Image': # Special case of requires saving image data as png separate files
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") # Convert to opencv image and save image to disk
filename = self.output_folder + '/' + sensor['_name'] + '_' + str(self.data_stamp) + '.jpg'
filename_relative = sensor['_name'] + '_' + str(self.data_stamp) + '.jpg'
cv2.imwrite(filename, cv_image)
image_dict = message_converter.convert_ros_message_to_dictionary(msg) # Convert sensor data to dictionary
del image_dict['data'] # Remove data field (which contains the image), and replace by "data_file"
image_dict['data_file'] = filename_relative # Contains full path to where the image was saved
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = image_dict
else:
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = message_converter.convert_ros_message_to_dictionary(msg)
# Update sensor labels ---------------------------------------------
if sensor['msg_type'] in ['Image', 'LaserScan']:
all_sensor_labels_dict[sensor_name] = labels
else:
raise ValueError('Unknown message type.')
collection_dict = {'data': all_sensor_data_dict, 'labels': all_sensor_labels_dict, 'transforms': transforms}
self.collections[self.data_stamp] = collection_dict
self.data_stamp += 1
# Save to json file
D = {'sensors': self.sensors, 'collections': self.collections, 'calibration_config': self.config}
self.createJSONFile(self.output_folder + '/data_collected.json', D)
self.unlockAllLabelers()
def getAllAbstractTransforms(self):
rospy.sleep(0.5) # wait for transformations
# Get a list of all transforms to collect
transforms_list = []
now = rospy.Time.now()
all_frames = self.listener.getFrameStrings()
for frame in all_frames:
chain = self.listener.chain(frame, now, self.world_link, now, self.world_link)
for idx in range(0, len(chain) - 1):
parent = chain[idx]
child = chain[idx + 1]
transforms_list.append({'parent': parent, 'child': child, 'key': self.generateKey(parent, child)})
# https://stackoverflow.com/questions/31792680/how-to-make-values-in-list-of-dictionary-unique
uniq_l = list(map(dict, frozenset(frozenset(i.items()) for i in transforms_list)))
return uniq_l # get unique values
def createJSONFile(self, output_file, D):
print("Saving the json output file to " + str(output_file) + ", please wait, it could take a while ...")
f = open(output_file, 'w')
json.encoder.FLOAT_REPR = lambda f: ("%.6f" % f) # to get only four decimal places on the json file
print >> f, json.dumps(D, indent=2, sort_keys=True)
f.close()
print("Completed.")
@staticmethod
def generateKey(parent, child, suffix=''):
return parent + '-' + child + suffix
class Sensor:
def __init__(self, name, server, menu_handler, frame_world,
frame_opt_parent, frame_opt_child, frame_sensor,
marker_scale):
print('Creating a new sensor named ' + name)
self.name = name
self.server = server
self.menu_handler = menu_handler
self.listener = TransformListener()
self.br = tf.TransformBroadcaster()
self.marker_scale = marker_scale
# transforms
self.world_link = frame_world
self.opt_parent_link = frame_opt_parent
self.opt_child_link = frame_opt_child
self.sensor_link = frame_sensor
self.updateAll() # update all the transformations
# print('Collected pre, opt and pos transforms.')
#
# print('preT:\n' + str(self.preT))
# print('optT:\n' + str(self.optT))
# print('posT:\n' + str(self.posT))
self.optTInitial = copy.deepcopy(self.optT)
self.createInteractiveMarker() # create interactive marker
print('Created interactive marker.')
# Start publishing now
self.timer_callback = rospy.Timer(
rospy.Duration(.1),
self.publishTFCallback) # to periodically broadcast
def resetToInitalPose(self):
self.optT.matrix = self.optTInitial.matrix
trans = self.optT.getTranslation()
self.marker.pose.position.x = trans[0]
self.marker.pose.position.y = trans[1]
self.marker.pose.position.z = trans[2]
quat = self.optT.getQuaternion()
self.marker.pose.orientation.x = quat[0]
self.marker.pose.orientation.y = quat[1]
self.marker.pose.orientation.z = quat[2]
self.marker.pose.orientation.w = quat[3]
self.optTInitial = copy.deepcopy(self.optT)
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def publishTFCallback(self, _):
trans = self.optT.getTranslation()
quat = self.optT.getQuaternion()
self.br.sendTransform((trans[0], trans[1], trans[2]),
(quat[0], quat[1], quat[2], quat[3]),
rospy.Time.now(), self.opt_child_link,
self.opt_parent_link)
def markerFeedback(self, feedback):
# print(' sensor ' + self.name + ' received feedback')
self.optT.setTranslationFromPosePosition(feedback.pose.position)
self.optT.setQuaternionFromPoseQuaternion(feedback.pose.orientation)
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def updateAll(self):
self.updatePreT()
self.updateOptT()
self.updatePosT()
def updateOptT(self):
self.optT = self.updateT(self.opt_parent_link, self.opt_child_link,
rospy.Time.now())
def updatePreT(self):
self.preT = self.updateT(self.world_link, self.opt_parent_link,
rospy.Time.now())
def updatePosT(self):
self.posT = self.updateT(self.opt_child_link, self.sensor_link,
rospy.Time.now())
def updateT(self, parent_link, child_link, stamp):
# self.listener.waitForTransform(parent_link, child_link, stamp, rospy.Duration(3.0))
# (trans, quat) = self.listener.lookupTransform(parent_link, child_link, stamp)
self.listener.waitForTransform(parent_link, child_link, rospy.Time(),
rospy.Duration(1.0))
(trans, quat) = self.listener.lookupTransform(parent_link, child_link,
rospy.Time())
T = TransformationT(parent_link, child_link)
T.setTranslation(trans)
T.setQuaternion(quat)
return T
def createInteractiveMarker(self):
self.marker = InteractiveMarker()
self.marker.header.frame_id = self.opt_parent_link
trans = self.optT.getTranslation()
self.marker.pose.position.x = trans[0]
self.marker.pose.position.y = trans[1]
self.marker.pose.position.z = trans[2]
quat = self.optT.getQuaternion()
self.marker.pose.orientation.x = quat[0]
self.marker.pose.orientation.y = quat[1]
self.marker.pose.orientation.z = quat[2]
self.marker.pose.orientation.w = quat[3]
self.marker.scale = self.marker_scale
self.marker.name = self.name
self.marker.description = self.name + '_control'
# insert a box
control = InteractiveMarkerControl()
control.always_visible = True
marker_box = Marker()
marker_box.type = Marker.SPHERE
marker_box.scale.x = self.marker.scale * 0.3
marker_box.scale.y = self.marker.scale * 0.3
marker_box.scale.z = self.marker.scale * 0.3
marker_box.color.r = 0
marker_box.color.g = 1
marker_box.color.b = 0
marker_box.color.a = 0.2
control.markers.append(marker_box)
self.marker.controls.append(control)
self.marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
self.server.insert(self.marker, self.markerFeedback)
self.menu_handler.apply(self.server, self.marker.name)
class DataCollectorAndLabeler:
def __init__(self, args, server, menu_handler):
self.output_folder = utilities.resolvePath(args['output_folder'])
if os.path.exists(
self.output_folder
) and not args['overwrite']: # dataset path exists, abort
print(
'\n' + Fore.RED + 'Error: Dataset ' + self.output_folder +
' exists.\nIf you want to replace it add a "--overwrite" flag.'
+ Style.RESET_ALL + '\n')
rospy.signal_shutdown()
elif os.path.exists(
self.output_folder
) and args['overwrite']: # move existing path to a backup location
now = datetime.now()
dt_string = now.strftime("%Y-%m-%d-%H-%M-%S")
basename = os.path.basename(self.output_folder)
backup_folder = '/tmp/' + basename + '_' + dt_string
time.sleep(2)
print('\n\nWarning: Dataset ' + Fore.YELLOW + self.output_folder +
Style.RESET_ALL + ' exists.\nMoving it to a new folder: ' +
Fore.YELLOW + backup_folder +
'\nThis will be deleted after a system reboot!' +
Style.RESET_ALL + '\n\n')
time.sleep(2)
execute('mv ' + self.output_folder + ' ' + backup_folder,
verbose=True)
os.mkdir(self.output_folder) # Recreate the folder
self.listener = TransformListener()
self.sensors = {}
self.sensor_labelers = {}
self.server = server
self.menu_handler = menu_handler
self.data_stamp = 0
self.collections = {}
self.bridge = CvBridge()
self.config = loadConfig(args['calibration_file'])
if self.config is None:
sys.exit(1) # loadJSON should tell you why.
self.world_link = self.config['world_link']
# Add sensors
print(Fore.BLUE + 'Sensors:' + Style.RESET_ALL)
print('Number of sensors: ' + str(len(self.config['sensors'])))
# Go through the sensors in the calib config.
for sensor_key, value in self.config['sensors'].items():
# Create a dictionary that describes this sensor
sensor_dict = {
'_name': sensor_key,
'parent': value['link'],
'calibration_parent': value['parent_link'],
'calibration_child': value['child_link']
}
# TODO replace by utils function
print("Waiting for message " + value['topic_name'] + ' ...')
msg = rospy.wait_for_message(value['topic_name'], rospy.AnyMsg)
print('... received!')
connection_header = msg._connection_header['type'].split('/')
ros_pkg = connection_header[0] + '.msg'
msg_type = connection_header[1]
print('Topic ' + value['topic_name'] + ' has type ' + msg_type)
sensor_dict['topic'] = value['topic_name']
sensor_dict['msg_type'] = msg_type
# If topic contains a message type then get a camera_info message to store along with the sensor data
if sensor_dict[
'msg_type'] == 'Image': # if it is an image must get camera_info
sensor_dict['camera_info_topic'] = os.path.dirname(
sensor_dict['topic']) + '/camera_info'
from sensor_msgs.msg import CameraInfo
print('Waiting for camera_info message on topic ' +
sensor_dict['camera_info_topic'] + ' ...')
camera_info_msg = rospy.wait_for_message(
sensor_dict['camera_info_topic'], CameraInfo)
print('... received!')
from rospy_message_converter import message_converter
sensor_dict[
'camera_info'] = message_converter.convert_ros_message_to_dictionary(
camera_info_msg)
# Get the kinematic chain form world_link to this sensor's parent link
now = rospy.Time()
print('Waiting for transformation from ' + value['link'] + ' to ' +
self.world_link)
self.listener.waitForTransform(value['link'], self.world_link, now,
rospy.Duration(5))
print('... received!')
chain = self.listener.chain(value['link'], now, self.world_link,
now, self.world_link)
chain_list = []
for parent, child in zip(chain[0::], chain[1::]):
key = self.generateKey(parent, child)
chain_list.append({
'key': key,
'parent': parent,
'child': child
})
sensor_dict['chain'] = chain_list # Add to sensor dictionary
self.sensors[sensor_key] = sensor_dict
sensor_labeler = InteractiveDataLabeler(
self.server, self.menu_handler, sensor_dict,
args['marker_size'], self.config['calibration_pattern'])
self.sensor_labelers[sensor_key] = sensor_labeler
print('Setup for sensor ' + sensor_key + ' is complete.')
print(Fore.BLUE + sensor_key + Style.RESET_ALL + ':\n' +
str(sensor_dict))
# print('sensor_labelers:')
# print(self.sensor_labelers)
self.abstract_transforms = self.getAllAbstractTransforms()
# print("abstract_transforms = " + str(self.abstract_transforms))
def getTransforms(self, abstract_transforms, time=None):
transforms_dict = {
} # Initialize an empty dictionary that will store all the transforms for this data-stamp
if time is None:
time = rospy.Time.now()
for ab in abstract_transforms: # Update all transformations
self.listener.waitForTransform(ab['parent'], ab['child'], time,
rospy.Duration(1.0))
(trans,
quat) = self.listener.lookupTransform(ab['parent'], ab['child'],
time)
key = self.generateKey(ab['parent'], ab['child'])
transforms_dict[key] = {
'trans': trans,
'quat': quat,
'parent': ab['parent'],
'child': ab['child']
}
return transforms_dict
def lockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.acquire()
print("Locked all labelers")
def unlockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.release()
print("Unlocked all labelers")
def getLabelersTimeStatistics(self):
stamps = [] # a list of the several time stamps of the stored messages
for sensor_name, sensor in self.sensors.iteritems():
stamps.append(
copy.deepcopy(
self.sensor_labelers[sensor_name].msg.header.stamp))
max_delta = getMaxTimeDelta(stamps)
# TODO : this is because of Andre's bag file problem. We should go back to the getAverageTime
# average_time = getAverageTime(stamps) # For looking up transforms use average time of all sensor msgs
average_time = getMaxTime(
stamps
) # For looking up transforms use average time of all sensor msgs
print('Times:')
for stamp, sensor_name in zip(stamps, self.sensors):
printRosTime(stamp, prefix=sensor_name + ': ')
return stamps, average_time, max_delta
def saveCollection(self):
# --------------------------------------
# Collect sensor data and labels (images, laser scans, etc)
# --------------------------------------
# Lock the semaphore for all labelers
self.lockAllLabelers()
# Analyse message time stamps and decide if collection can be stored
stamps, average_time, max_delta = self.getLabelersTimeStatistics()
if max_delta is not None: # if max_delta is None (only one sensor), continue
if max_delta.to_sec() > float(
self.config['max_duration_between_msgs']
): # times are close enough?
rospy.logwarn('Max duration between msgs in collection is ' +
str(max_delta.to_sec()) +
'. Not saving collection.')
self.unlockAllLabelers()
return None
else: # test passed
rospy.loginfo('Max duration between msgs in collection is ' +
str(max_delta.to_sec()))
# Collect all the transforms
transforms = self.getTransforms(
self.abstract_transforms,
average_time) # use average time of sensor msgs
printRosTime(average_time, "Collected transforms for time ")
all_sensor_data_dict = {}
all_sensor_labels_dict = {}
for sensor_key, sensor in self.sensors.iteritems():
print('collect sensor: ' + sensor_key)
msg = copy.deepcopy(self.sensor_labelers[sensor_key].msg)
labels = copy.deepcopy(self.sensor_labelers[sensor_key].labels)
print('sensor' + sensor_key)
# TODO add exception also for point cloud and depth image
# Update sensor data ---------------------------------------------
if sensor[
'msg_type'] == 'Image': # Special case of requires saving image data as png separate files
cv_image = self.bridge.imgmsg_to_cv2(
msg,
"bgr8") # Convert to opencv image and save image to disk
filename = self.output_folder + '/' + sensor[
'_name'] + '_' + str(self.data_stamp) + '.jpg'
filename_relative = sensor['_name'] + '_' + str(
self.data_stamp) + '.jpg'
cv2.imwrite(filename, cv_image)
image_dict = message_converter.convert_ros_message_to_dictionary(
msg) # Convert sensor data to dictionary
del image_dict[
'data'] # Remove data field (which contains the image), and replace by "data_file"
image_dict[
'data_file'] = filename_relative # Contains full path to where the image was saved
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = image_dict
else:
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor[
'_name']] = message_converter.convert_ros_message_to_dictionary(
msg)
# Update sensor labels ---------------------------------------------
if sensor['msg_type'] in ['Image', 'LaserScan', 'PointCloud2']:
all_sensor_labels_dict[sensor_key] = labels
else:
raise ValueError('Unknown message type.')
collection_dict = {
'data': all_sensor_data_dict,
'labels': all_sensor_labels_dict,
'transforms': transforms
}
self.collections[self.data_stamp] = collection_dict
self.data_stamp += 1
# Save to json file
D = {
'sensors': self.sensors,
'collections': self.collections,
'calibration_config': self.config
}
print('Saving file ' + self.output_folder + '/data_collected.json')
self.createJSONFile(self.output_folder + '/data_collected.json', D)
self.unlockAllLabelers()
def getAllAbstractTransforms(self):
# Get a list of all transforms to collect
transforms_list = []
now = rospy.Time.now()
all_frames = self.listener.getFrameStrings()
for frame in all_frames:
# print('Waiting for transformation from ' + frame + ' to ' + self.world_link + '(max 3 secs)')
try:
self.listener.waitForTransform(frame, self.world_link, now,
rospy.Duration(3))
chain = self.listener.chain(frame, now, self.world_link, now,
self.world_link)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr('Could not get transform from ' + frame + ' to ' +
self.world_link + '(max 3 secs)')
continue
for idx in range(0, len(chain) - 1):
parent = chain[idx]
child = chain[idx + 1]
transforms_list.append({
'parent': parent,
'child': child,
'key': self.generateKey(parent, child)
})
# https://stackoverflow.com/questions/31792680/how-to-make-values-in-list-of-dictionary-unique
uniq_l = list(
map(dict,
frozenset(frozenset(i.items()) for i in transforms_list)))
return uniq_l # get unique values
def createJSONFile(self, output_file, D):
print("Saving the json output file to " + str(output_file) +
", please wait, it could take a while ...")
f = open(output_file, 'w')
json.encoder.FLOAT_REPR = lambda f: (
"%.6f" % f) # to get only four decimal places on the json file
print >> f, json.dumps(D, indent=2, sort_keys=True)
f.close()
print("Completed.")
@staticmethod
def generateKey(parent, child, suffix=''):
return parent + '-' + child + suffix
class ExpeSupervisor:
class State(Enum):
IDLE = 0
RUNNING = 1
ENDED = 2
def __init__(self):
self.marker_pub = rospy.Publisher(HUMAN_TRIGGER_MARKER_PUB,
Marker,
queue_size=10)
self.coord_signal = rospy.Publisher(COORD_SIGNAL_PUB,
CoordinationSignal,
queue_size=1)
self.tf_listener = TransformListener()
self.move_base = SimpleActionClient(MOVE_BASE_ACTION_SRV,
MoveBaseAction)
rospy.loginfo(
"{} waiting for move_base action server.".format(NODE_NAME))
self.move_base.wait_for_server()
rospy.loginfo("{} found move_base action server.".format(NODE_NAME))
self.navigating_fact_id = None
rospy.loginfo(NODE_NAME +
" waiting for underworlds start fact service server.")
self.start_fact = rospy.ServiceProxy(START_FACT_SRV_NAME, StartFact)
self.start_fact.wait_for_service()
rospy.loginfo(NODE_NAME +
" found underworlds start fact service server.")
rospy.loginfo(NODE_NAME +
" waiting for underworlds stop fact service server.")
self.end_fact = rospy.ServiceProxy(STOP_FACT_SRV_NAME, EndFact)
self.end_fact.wait_for_service()
rospy.loginfo(NODE_NAME +
" found underworlds stop fact service server.")
self.coordination_human_timer = None
self.trigger_zone_bb = BoundingBox(3.0, 13.0, 5.0, 16.0)
self.state = self.State.IDLE
rospy.Timer(rospy.Duration(1.0),
lambda _: self.draw_trigger_zone(False),
oneshot=True)
rospy.Timer(rospy.Duration(0.1), self.loop)
def draw_trigger_zone(self, activated):
marker = Marker()
marker.header.frame_id = MAP_FRAME
marker.header.stamp = rospy.Time(0)
marker.lifetime = rospy.Duration(0)
marker.ns = NS
marker.id = 0
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.pose.position.x = (self.trigger_zone_bb[0][0] +
self.trigger_zone_bb[1][0]) / 2
marker.pose.position.y = (self.trigger_zone_bb[0][1] +
self.trigger_zone_bb[1][1]) / 2
marker.pose.position.z = 0.25
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = abs(self.trigger_zone_bb[0][0] -
self.trigger_zone_bb[1][0]) / 2
marker.scale.y = abs(self.trigger_zone_bb[0][1] -
self.trigger_zone_bb[1][1]) / 2
marker.scale.z = 0.5
marker.color.a = 0.5
marker.color.r = 0.09 if activated else 0.18
marker.color.g = 0.15 if activated else 0.26
marker.color.b = 0.34 if activated else 0.46
self.marker_pub.publish(marker)
def send_look_at_human(self):
coord = CoordinationSignal()
t = TargetWithExpiration()
t.target.header.stamp = rospy.Time.now()
t.target.header.frame_id = HUMAN_HEAD_FRAME
t.duration = COORD_SIGNAL_DURATION
t.regex_end_condition = ""
t.target.point.x = 0.0
t.target.point.y = 0.0
t.target.point.z = 0.0
coord.header.frame_id = HUMAN_HEAD_FRAME
coord.header.stamp = rospy.Time.now()
coord.priority = 255
coord.expiration = rospy.Time.now() + rospy.Duration(3)
coord.predicate = ""
coord.regex_end_condition = ""
coord.targets = [t]
self.coord_signal.publish()
def start_navigation(self):
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = MAP_FRAME
goal.target_pose.pose.position.x = 8.0
goal.target_pose.pose.position.y = 15.5
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
self.start_fact()
resp = self.start_fact("base", "isNavigating(robot)",
rospy.Time.now().to_sec(), False)
if resp.success:
self.navigating_fact_id = resp.fact_id
self.coordination_human_timer = rospy.Timer(
rospy.Duration(DURATION_BETWEEN_COORD_SIGNAL +
COORD_SIGNAL_DURATION), self.send_look_at_human)
self.move_base.send_goal(goal)
def stop_navigation(self):
s = self.move_base.get_state()
if s != GoalStatus.SUCCEEDED or s != GoalStatus.ABORTED or s != GoalStatus.PREEMPTED:
self.move_base.cancel_all_goals()
if self.navigating_fact_id is not None:
resp = self.end_fact("base", self.move_to_fact_id)
if resp.success:
self.move_to_fact_id = None
if self.coordination_human_timer is not None:
self.coordination_human_timer.shutdown()
def loop(self, _):
t_human, r_human = self.tf_listener.lookupTransform(
HUMAN_FRAME, MAP_FRAME, rospy.Time(0))
if self.state == self.State.IDLE:
if self.trigger_zone_bb.is_in(t_human[0], t_human[1]):
self.state = self.State.RUNNING
self.start_navigation()
rospy.loginfo("{} idle -> running".format(NODE_NAME))
elif self.state == self.State.RUNNING:
s = self.move_base.get_state()
if s == GoalStatus.SUCCEEDED or s == GoalStatus.ABORTED or s == GoalStatus.PREEMPTED:
self.stop_navigation()
rospy.loginfo("{} running -> ended".format(NODE_NAME))
self.state = self.State.ENDED
def run(self):
rospy.loginfo("{} online".format(NODE_NAME))
rospy.spin()
class RoadmapServer:
def __init__(self, args):
rospy.init_node('roadmap_server')
self.optimization_distance = rospy.get_param('~optimization_distance', 10);
self.tf = TransformListener()
stereo_cam = camera.Camera((389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
self.skel = Skeleton(stereo_cam)
# self.skel.node_vdist = 1
if False:
self.skel.load(args[1])
self.skel.optimize()
self.startframe = 100000
else:
self.startframe = 0
self.frame_timestamps = {}
self.vo = None
self.pub = rospy.Publisher("roadmap", vslam.msg.Roadmap)
time.sleep(1)
#self.send_map(rospy.time(0))
self.wheel_odom_edges = set()
rospy.Subscriber('/wide_stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
def send_map(self, stamp):
if len(self.skel.nodes) < 4:
return
TG = nx.MultiDiGraph()
for i in self.skel.nodes:
TG.add_node(i)
for (a,b,p,c) in self.skel.every_edge + list(self.wheel_odom_edges):
TG.add_edge(a, b, (p, c))
here = max(self.skel.nodes)
# TG is the total graph, G is the local subgraph
uTG = TG.to_undirected()
print "uTG", uTG.nodes(), uTG.edges()
close = set([here])
for i in range(self.optimization_distance):
close |= set(nx.node_boundary(uTG, close))
print "close", close
G = TG.subgraph(close)
uG = uTG.subgraph(close)
pg = TreeOptimizer3()
def mk_covar(xyz, rp, yaw):
return (1.0 / math.sqrt(xyz),1.0 / math.sqrt(xyz), 1.0 / math.sqrt(xyz), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(yaw))
weak = mk_covar(9e10,3,3)
strong = mk_covar(0.0001, 0.000002, 0.00002)
pg.initializeOnlineOptimization()
def pgadd(p, n, data):
relpose, strength = data
if strength == 0.0:
cov = weak
else:
cov = strong
pg.addIncrementalEdge(p, n, relpose.xform(0,0,0), relpose.euler(), cov)
Gedges = G.edges(data = True)
revmap = {}
map = {}
for n in nx.dfs_preorder(uG, here):
revmap[len(map)] = n
map[n] = len(map)
priors = [p for p in uG.neighbors(n) if p in map]
if priors == []:
print "START NODE", n, "as", map[n]
else:
print "NEXT NODE", n
p = priors[0]
if not G.has_edge(p, n):
n,p = p,n
data = G.get_edge_data(p, n)[0]
Gedges.remove((p, n, data))
pgadd(map[p], map[n], data)
for (p,n,d) in Gedges:
pgadd(map[p], map[n], d)
pg.initializeOnlineIterations()
start_error = pg.error()
for i in range(10):
pg.iterate()
print "Error from", start_error, "to", pg.error()
pg.recomputeAllTransformations()
print self.tf.getFrameStrings()
target_frame = "base_link"
t = self.tf.getLatestCommonTime("wide_stereo_optical_frame", target_frame)
trans,rot = self.tf.lookupTransform(target_frame, "wide_stereo_optical_frame", t)
xp = Pose()
xp.fromlist(self.tf.fromTranslationRotation(trans,rot))
roadmap_nodes = dict([ (n, (30,0,0)) for n in TG.nodes() ])
nl = sorted(roadmap_nodes.keys())
print "nl", nl
for i in sorted(map.values()):
xyz,euler = pg.vertex(i)
p = from_xyz_euler(xyz, euler)
pose_in_target = xp * p
x,y,z = pose_in_target.xform(0,0,0)
euler = pose_in_target.euler()
print x,y,z, euler
roadmap_nodes[revmap[i]] = (x, y, euler[2])
roadmap_edges = []
for (p,n) in set(TG.edges()):
print p,n
best_length = max([ (confidence, pose.distance()) for (pose, confidence) in TG.get_edge_data(p, n).values()])[1]
roadmap_edges.append((p, n, best_length))
msg = vslam.msg.Roadmap()
msg.header.stamp = stamp
msg.header.frame_id = "base_link"
msg.nodes = [vslam.msg.Node(*roadmap_nodes[n]) for n in nl]
print "(p,n)", [ (p,n) for (p,n,l) in roadmap_edges ]
msg.edges = [vslam.msg.Edge(nl.index(p), nl.index(n), l) for (p, n, l) in roadmap_edges]
msg.localization = nl.index(here)
self.pub.publish(msg)
if 1:
import matplotlib.pyplot as plt
fig = plt.figure(figsize = (12, 6))
plt.subplot(121)
pos = nx.spring_layout(TG, iterations=1000)
nx.draw(TG, pos,node_color='#A0CBE2')
nx.draw_networkx_edges(G, pos, with_labels=False, edge_color='r', alpha=0.5, width=25.0, arrows = False)
plt.subplot(122)
nx.draw(G, pos = dict([(n, roadmap_nodes[n][:2]) for n in G.nodes()]))
#plt.show()
plt.savefig("/tmp/map_%d.png" % here)
def handle_raw_stereo(self, msg):
size = (msg.left_info.width, msg.left_info.height)
if self.vo == None:
cam = camera.StereoCamera(msg.right_info)
self.fd = FeatureDetectorFast(300)
self.ds = DescriptorSchemeCalonder()
self.vo = VisualOdometer(cam, scavenge = False,
inlier_error_threshold = 3.0, sba = None,
inlier_thresh = 100,
position_keypoint_thresh = 0.2, angle_keypoint_thresh = 0.15)
self.vo.num_frames = self.startframe
pair = [Image.fromstring("L", size, i.uint8_data.data) for i in [ msg.left_image, msg.right_image ]]
af = SparseStereoFrame(pair[0], pair[1], feature_detector = self.fd, descriptor_scheme = self.ds)
self.vo.handle_frame(af)
self.frame_timestamps[af.id] = msg.header.stamp
if self.skel.add(self.vo.keyframe):
print "====>", self.vo.keyframe.id, self.frame_timestamps[self.vo.keyframe.id]
#print self.tf.getFrameStrings()
#assert self.tf.frameExists("wide_stereo_optical_frame")
#assert self.tf.frameExists("odom_combined")
N = sorted(self.skel.nodes)
for a,b in zip(N, N[1:]):
if (a,b) in self.skel.edges:
t0 = self.frame_timestamps[a]
t1 = self.frame_timestamps[b]
if self.tf.canTransformFull("wide_stereo_optical_frame", t0, "wide_stereo_optical_frame", t1, "odom_combined"):
t,r = self.tf.lookupTransformFull("wide_stereo_optical_frame", t0, "wide_stereo_optical_frame", t1, "odom_combined")
relpose = Pose()
relpose.fromlist(self.tf.fromTranslationRotation(t,r))
self.wheel_odom_edges.add((a, b, relpose, 1.0))
self.send_map(msg.header.stamp)
# 创建一个坐标关系的收听者
listener = TransformListener()
# 测试观测距离变化的
distance_pub = rospy.Publisher('/test/distance', Float32, queue_size=10)
rate = rospy.Rate(10) # 一秒钟要10次
while not rospy.is_shutdown():
try:
# 函数参数:
# target_frame: 目标坐标系, 父坐标系
# source_frame: 源坐标系, 子坐标系
# time: 时间, 最近的时间
# 函数返回值: 位置[x, y, z], 姿态四元素[x, y, z, w]
transform = listener.lookupTransform('frame_b', 'frame_a',
rospy.Time())
x, y, z = transform[0]
# 线性距离
distance = sqrt(x**2 + y**2)
angular = atan2(y, x)
# msg = Float32()
# msg.data = distance
# distance_pub.publish(msg)
distance_pub.publish(Float32(data=distance))
# vel = 距离 / 时间
twist = Twist()
twist.linear.x = 0.80 * distance
twist.angular.z = 2.0 * angular
class SEMAPEnvironmentServices():
server = None
objects = {}
tf_broadcaster = None
tf_listener = None
marker_pub = None
range_query2d_marker = None
range_query3d_marker = None
range_query_marker = None
area_query_marker = None
volume_query_marker = None
def __init__(self):
self.setup_node()
self.server = InteractiveMarkerServer("semap_environment")
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
def setup_node(self):
rospy.init_node('semap_environment_services')
rospy.loginfo( "SEMAP Environment Services are initializing...\n" )
rospy.Timer(rospy.Duration(0.02), self.publishTF)
#rospy.Timer(rospy.Duration(1.0), self.activate_robot_surroundings)
rospy.Subscriber("create_object", PoseStamped, self.createObjectCb)
#rospy.Subscriber("polygon", PolygonStamped, self.findObjectWithinPolygonCb)
#rospy.Subscriber("polygon", PolygonStamped, self.copyObjectWithinPolygonCb)
rospy.Subscriber("polygon", PolygonStamped, self.findObjectWithinVolumeCb)
rospy.Subscriber("range2d", PointStamped, self.findObjectWithinRange2DCb)
rospy.Subscriber("range_query", PointStamped, self.findObjectWithinRangeCb)
self.range_query_marker = rospy.Publisher("distance_query", MarkerArray, queue_size=10)
self.area_query_marker = rospy.Publisher("area_query", PolygonStamped, queue_size=10)
self.volume_query_marker = rospy.Publisher("volume_query", MarkerArray, queue_size=10)
self.range_query2d_marker = rospy.Publisher("range_query2d_marker", Marker, queue_size=10)
self.marker_pub = rospy.Publisher("collection_marker", MarkerArray, queue_size=10)
user = rospy.get_param('~user')
password = rospy.get_param('~password')
host = rospy.get_param('~host')
database = rospy.get_param('~database')
initializeConnection(user, password, host, database, False)
## Active Objects
srv_refresh_objects = rospy.Service('refresh_objects', ActivateObjects, self.refresh_objects)
srv_refresh_all_objects = rospy.Service('refresh_all_objects', ActivateAllObjects, self.refresh_all_objects)
srv_activate_objects = rospy.Service('activate_objects', ActivateObjects, self.activate_objects)
srv_activate_all_objects = rospy.Service('activate_all_objects', ActivateAllObjects, self.activate_all_objects)
srv_deactivate_objects = rospy.Service('deactivate_objects', DeactivateObjects, self.deactivate_objects)
srv_deactivate_all_object = rospy.Service('deactivate_all_objects', DeactivateAllObjects, self.deactivate_all_objects)
srv_show_objects = rospy.Service('show_objects', ActivateObjects, self.show_objects)
srv_show_all_objects = rospy.Service('show_all_objects', ActivateAllObjects, self.show_all_objects)
#srv_unshow_objects = rospy.Service('unshow_objects', ActivateObjects, self.activate_all_objects)
#srv_unshow_all_objects = rospy.Service('unshow_all_objects', ActivateAllObjects, self.activate_all_objects)
srv_show_distance_between_objects = rospy.Service('show_distance_between_objects', GetDistanceBetweenObjects, self.showDistanceBetweenObjects)
srv_show_objects_within_range = rospy.Service('show_objects_within_range', GetObjectsWithinRange, self.showObjectWithinRange)
srv_show_objects_within_area = rospy.Service('show_objects_within_area', GetObjectsWithinArea, self.showObjectWithinArea)
srv_show_objects_within_volume = rospy.Service('show_objects_within_volume', GetObjectsWithinVolume, self.showObjectWithinVolume)
srv_activate_objects_in_objects = rospy.Service('activate_objects_in_objects', GetObjectsInObjects, self.activateObjectsInObjects)
srv_show_objects_in_objects = rospy.Service('show_objects_in_objects', GetObjectsInObjects, self.showObjectsInObjects)
srv_activate_objects_on_objects = rospy.Service('activate_objects_on_objects', GetObjectsOnObjects, self.activateObjectsOnObjects)
srv_show_objects_on_objects = rospy.Service('show_objects_on_objects', GetObjectsOnObjects, self.showObjectsOnObjects)
rospy.loginfo( "SEMAP DB Services are running...\n" )
def spin(self):
rospy.spin()
### Services
def activate_objects(self, req):
then = rospy.Time.now()
if len( req.ids) > 0:
rospy.loginfo("SpatialEnv SRVs: activate_objects")
#self.deactivate_objects(req)
get_res = call_get_object_instances(req.ids)
rospy.loginfo("Call Get Object Instances took %f seconds" % (rospy.Time.now() - then).to_sec())
for obj in get_res.objects:
#rospy.loginfo("Activate object: %s" % obj.name)
# get inst visu from db
# if exits: convertNonDBtype
# else create one, store and pass on
if obj.name in self.objects.keys():
#if self.objects[obj.name].status == "active":
#print 'object', obj.name, 'is already active and gets reactivated'
if self.objects[obj.name].status == "inactive":
del self.objects[obj.name]
#print 'object', obj.name, 'is inactive and gets removed from inactive pool'
#else:
# print 'object was unknown so far, now its active'
then2 = rospy.Time.now()
active_object = EnvironmentObject(obj.id, obj.name, obj, InteractiveObjectMarker(obj, self.server, None ), status = "active")
#rospy.loginfo("Marker took %f seconds" % (rospy.Time.now() - then2).to_sec())
self.objects[obj.name] = active_object
#rospy.loginfo("Took %f seconds" % (rospy.Time.now() - now).to_sec())
self.publishTF(None)
rospy.loginfo("Took %f seconds" % (rospy.Time.now() - then).to_sec())
rospy.loginfo("SpatialEnv SRVs: activate_objects - done")
return ActivateObjectsResponse()
def show_objects(self, req):
if len( req.ids) > 0:
rospy.loginfo("SpatialEnv SRVs: show_objects")
get_res = call_get_object_instances(req.ids)
for obj in get_res.objects:
if obj.name in self.objects.keys():
if self.objects[obj.name].status == "active":
print 'object', obj.name, 'is already active'
break
elif self.objects[obj.name].status == "inactive":
print 'object', obj.name, 'is inactive'
ghost = EnvironmentObject(obj.id, obj.name, obj, GhostObjectMarker(obj, self.server, None ), status = "inactive")
self.objects[obj.name] = ghost
else:
print 'object was unknown so far, now its inactive'
ghost = EnvironmentObject(obj.id, obj.name, obj, GhostObjectMarker(obj, self.server, None ), status = "inactive")
self.objects[obj.name] = ghost
rospy.loginfo("SpatialEnv SRVs: show_objects - done")
return ActivateObjectsResponse()
def refresh_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: reactivate_objects")
print req.ids
active_req = ActivateObjectsRequest()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].id in req.ids:
if self.objects[key].status == "active":
active_req.ids.append( self.objects[key].id )
elif self.objects[key].status == "inactive":
inactive_req.ids.append( self.objects[key].id )
self.activate_objects(active_req)
self.show_objects(inactive_req)
res = ActivateObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: reactivate_objects - done")
return res
def refresh_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: reactivate_all_objects")
active_req = ActivateObjectsRequest()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].status == "active":
active_req.ids.append( self.objects[key].id )
elif self.objects[key].status == "inactive":
inactive_req.ids.append( self.objects[key].id )
self.activate_objects(active_req)
self.show_objects(inactive_req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: reactivate_all_objects - done")
return res
def deactivate_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: deactivate_objects")
res = DeactivateObjectsResponse()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].id in req.ids:
del self.objects[key]
self.show_objects(req)
self.publishTF(None)
return res
def deactivate_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: deactivate_all_objects")
self.objects = {}
self.publishTF(None)
self.show_all_objects(req)
return DeactivateAllObjectsResponse()
def activate_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: activate_all_objects")
ids = db().query(ObjectInstance.id).all()
req = ActivateObjectsRequest()
req.ids = [id for id, in ids]
self.activate_objects(req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: activate_all_objects - done")
return res
def show_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: show_all_objects")
ids = db().query(ObjectInstance.id).all()
req = ActivateObjectsRequest()
req.ids = [id for id, in ids]
self.show_objects(req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: show_all_objects - done")
return res
def publishTF(self, msg):
if len(self.objects) > 0:
for key in self.objects.keys():
if self.objects[key].status == "active":
object = self.objects[key].object
translation = object.pose.pose.position
rotation = object.pose.pose.orientation
time = rospy.Time.now()
#print 'publish tf'
self.tf_broadcaster.sendTransform( (translation.x, translation.y, translation.z), \
(rotation.x, rotation.y, rotation.z, rotation.w), \
time, object.name, object.pose.header.frame_id)
def activate_robot_surroundings(self, msg):
#if activate_robot_surroundings:
try:
(trans,rot) = self.tf_listener.lookupTransform('/world', '/base_link', rospy.Time(0))
print 'current robot position', trans
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print 'could not determine current robot position'
#else:
# robot inactive
def createObjectCb(self, pose):
print 'Create Object from RViz pose'
app = QApplication(sys.argv)
widget = ChooseObjectDescriptionWidget(0)
widget.show()
app.exec_()
desc_name, desc_id = widget.getChoice()
del app, widget
if(desc_id == -1):
new_desc = ROSObjectDescription()
new_desc.type = desc_name
res = call_add_object_descriptions( [ new_desc ] )
print res
desc_id = res.ids[0]
obj = ROSObjectInstance()
obj.description.id = desc_id
obj.pose = pose
res = call_add_object_instances( [obj] )
call_activate_objects( res.ids )
def activateObjectsInObjects(self, req):
res = call_get_objects_in_objects(req.reference, req.target, req.fully_within)
ids = []
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
return res
def showObjectsInObjects(self, req):
res = call_get_objects_in_objects(req.reference, req.target, req.fully_within)
ids = []
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_show_objects( ids )
return res
def activateObjectsOnObjects(self, req):
print 'GOT SO FAR'
res = call_get_objects_on_objects(req.reference_object_types, req.target_object_types, req.threshold)
print 'NOW WHAT'
ids = []
for pair in res.pairs:
#if pair.reference_id not in ids:
# ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
return res
def showObjectsOnObjects(self, req):
print 'GOT SO FAR'
res = call_get_objects_on_objects(req.reference_object_types, req.target_object_types, req.threshold)
print 'NOW WHAT'
ids = []
for pair in res.pairs:
#if pair.reference_id not in ids:
# ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_show_objects( ids )
return res
def findObjectWithinRange2DCb(self, point_stamped):
distance = 2.0
print 'find Objects Within Range2D'
#res = call_get_objects_within_range2d([], "Position2D", point_stamped.point, distance, fully_within = False)
#res = call_get_objects_within_range2d([], "FootprintBox", point_stamped.point, distance, fully_within = False)
#res = call_get_objects_within_range2d(["ConferenceTable", "ConferenceChair"], "Position2D", point_stamped.point, 3.0)
#call_activate_objects( res.ids )
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "2d_range_query_marker"
marker.id = 0
marker.type = 3 #cylinder
marker.scale.x = distance*2
marker.scale.y = distance*2
marker.scale.z = 0.01
marker.pose.position = point_stamped.point
marker.pose.position.z = 0.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.5
self.range_query2d_marker.publish(marker)
def findObjectWithinRangeCb(self, point_stamped):
distance = 1.5
res = call_get_objects_within_range(point_stamped.point, [], "FootprintHull", distance, fully_within = False)
ids = []
array = MarkerArray()
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "range"
marker.id = 0
marker.type = 3 #sphere
marker.scale.x = distance*2
marker.scale.y = distance*2
marker.scale.z = 0.005
marker.pose.position = point_stamped.point
marker.pose.position.z = 0.0
marker.color.r = 0.25
marker.color.g = 0.25
marker.color.b = 0.75
marker.color.a = 0.5
array.markers.append(marker)
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
def showObjectWithinRange(self, req):
res = call_get_objects_within_range(req.reference_point, req.target_object_types, req.target_object_geometry_type,
req.distance, req.fully_within)
ids = []
array = MarkerArray()
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "range"
marker.id = 0
marker.type = 3 #sphere
marker.scale.x = req.distance*2
marker.scale.y = req.distance*2
marker.scale.z = 0.005
marker.pose.position = req.reference_point
marker.pose.position.z = 0.0
marker.color.r = 0.25
marker.color.g = 0.25
marker.color.b = 0.75
marker.color.a = 0.5
array.markers.append(marker)
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
return res
def findObjectWithinVolumeCb(self, polygon_stamped):
extrude = call_extrude_polygon(polygon_stamped.polygon, 0.0, 0.0, 0.9)
mesh = PolygonMeshStamped()
mesh.header.frame_id = "world"
mesh.mesh = extrude.mesh
print extrude.mesh
res = call_get_objects_within_volume(extrude.mesh, ['Mug','Teapot','Monitor','Keyboard'], 'BoundingBox', True)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
array = MarkerArray()
pose = ROSPoseStamped()
pose.header.frame_id = 'world'
for polygon in extrude.mesh.polygons:
poly = Polygon()
for point in polygon.vertex_indices:
poly.points.append(extrude.mesh.vertices[point])
geo_marker = create_polygon_marker("VolumeQuery", pose, [0,1.0,0,1.0], [0.01, 0.01, 0.01], poly)
array.markers.append(geo_marker)
id = 0
for m in array.markers:
m.header.frame_id = 'world'
m.id = id
id += 1
self.volume_query_marker.publish(array)
def showObjectWithinVolume(self, req):
res = call_get_objects_within_volume(req.reference_mesh, req.target_object_types, req.target_object_geometry_type, req.fully_within)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
array = MarkerArray()
pose = ROSPoseStamped()
pose.header.frame_id = 'world'
for polygon in req.reference_mesh.polygons:
poly = Polygon()
for point in polygon.vertex_indices:
poly.points.append(req.reference_mesh.vertices[point])
geo_marker = create_polygon_marker("VolumeQuery", pose, [0,1.0,0,1.0], [0.01, 0.01, 0.01], poly)
array.markers.append(geo_marker)
id = 0
for m in array.markers:
m.header.frame_id = 'world'
m.id = id
id += 1
self.volume_query_marker.publish(array)
return res
def findObjectWithinPolygonCb(self, polygon_stamped):
res = call_get_objects_within_area(polygon_stamped.polygon, ["Chair"], "Position2D", fully_within = False)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
self.area_query_marker.publish(polygon_stamped)
return res
def copyObjectWithinPolygonCb(self, polygon_stamped):
res = call_get_objects_within_area(polygon_stamped.polygon, [], "Position2D", fully_within = False)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
print 'COPY'
copy_res = call_copy_object_instances(ids)
if len( copy_res.ids ) > 1:
print 'copy_res', copy_res
obj_res = call_get_object_instances( [copy_res.ids[0]] )
print 'bind to ', obj_res.objects[0].name
frame = obj_res.objects[0].name
for id in copy_res.ids:
if id != obj_res.objects[0].id:
print 'bind', id, 'to', obj_res.objects[0].id
call_change_frame(id, frame, False)
call_activate_objects( copy_res.ids )
call_refresh_objects( copy_res.ids )
return res
def showObjectWithinArea(self, req):
res = call_get_objects_within_area(req.reference_polygon, req.target_object_types, req.target_object_geometry_type, req.fully_within)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
polygon_stamped = PolygonStamped()
polygon_stamped.header.frame_id = "world"
polygon_stamped.polygon = req.reference_polygon
self.area_query_marker.publish(polygon_stamped)
return res
def showDistanceBetweenObjects(self, req):
res = call_get_distance_between_objects(req.reference_object_types, req.reference_object_geometry_type,
req.target_object_types, req.target_object_geometry_type,
req.min_range, req.max_range,
req.sort_descending, req.max_distance, return_points = True)
array = MarkerArray()
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
ids =[]
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
return res
def list_active_objects(self, msg):
for key in self.objects.keys():
active = active_objects[key]
print 'ID :', active.id
print 'TYPE :', active.object.description.type
print 'NAME :', active.object.name
print 'ALIAS :', active.object.alias
print 'POSE :', active.object.pose
# 创建一个坐标关系的收听者
listener = TransformListener()
# 测试让小乌龟Ｂ运动 geometry_msgs/Twist
publisher = rospy.Publisher('/{}/cmd_vel'.format(follow), Twist, queue_size=10)
distance_pub = rospy.Publisher('/{}/distance'.format(follow), Float32, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
# 函数参数:
# target_frame: 目标坐标系, 父坐标系
# source_frame: 源坐标系, 子坐标系
# time: 时间, 最近的时间
# 函数返回值: 位置[x, y, z], 姿态四元素[x, y, z, w]
transform = listener.lookupTransform(follow, parent, rospy.Time())
x, y, z = transform[0]
# 线性距离
distance = sqrt(x ** 2 + y ** 2)
angular = atan2(y, x)
# vel = 距离 / 时间
twist = Twist()
twist.linear.x = 2.0 * distance
twist.angular.z = 4.0 * angular
publisher.publish(twist)
distance_pub.publish(Float32(data=distance))
except Exception as e:
print e
object_odom_position_solved = False
twist_1 = Twist()
twist_1.angular.z = 0.3
Cmd_vel.publish(twist_1)
while not object_odom_position_solved:
if tag not in tags:
print("Find tag:", tag)
tags.append(tag)
twist_2 = Twist()
Cmd_vel.publish(twist_2)
rospy.sleep(1)
SMaRt_messages.update({int(tag): SMaRt_element})
try:
# obtatin object_odom_position
#(trans, rot) = listener.lookupTransform('/odom', '/camera_optical_link', rospy.Time(0))
(trans, rot) = listener.lookupTransform(
'/odom', '/qr_code', rospy.Time(0))
# (r_cam, p_cam, y_cam) = tf.transformations.euler_from_quaternion([rot[0], rot[1], rot[2], rot[3]])
# trans_rotation_cam_odom = np.insert(eulerAnglesToRotationMatrix(r_cam, p_cam, y_cam), 3, [0, 0, 0], 0)
# trans_translation_cam_odom = [trans[0], trans[1], trans[2], 1]
# trans_cam_odom = np.insert(trans_rotation_cam_odom, 3, trans_translation_cam_odom, 1)
# object_odom_position = np.dot(trans_cam_odom, object_camera_position)
# objects_odom_position.append(object_odom_position)
objects_odom_position.append(
[trans[0], trans[1], 0, 1])
objects_secret_position.append(object_secret_position)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
object_odom_position_solved = True
# print("object_odom_position: ", object_odom_position)
