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() # 测试让小乌龟B运动 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)