def update_geo_path(self): if (self.waypoints and self.zone and self.band): waypoints_geo = [] for point in self.waypoints: x, y = point geo = UTMPoint(x, y, zone=self.zone, band=self.band).toMsg() wp = WayPoint() wp.id = unique_id.toMsg(unique_id.fromRandom()) wp.position = geo waypoints_geo.append(wp) segments_geo = [] last_wp = waypoints_geo[0].id for wp in waypoints_geo: seg = RouteSegment() seg.id = unique_id.toMsg(unique_id.fromRandom()) seg.start = last_wp seg.end = wp.id last_wp = wp.id segments_geo.append(seg) self.route_network.points = waypoints_geo self.route_network.segments = segments_geo else: rospy.logwarn('[update_geo_path] waypoints, zone, and/or band\n' + '\thas not been set/updated.\n\tIgnoring...')
def read(file): yaml_data = None with open(file) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = t['name'] ann.type = 'yocs_msgs/Table' for i in range(0, random.randint(0, 11)): ann.keywords.append('kw' + str(random.randint(1, 11))) if 'prev_id' in vars(): ann.relationships.append(prev_id) prev_id = ann.id ann.shape = 3 #CYLINDER ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.8 ann.color.a = 0.5 ann.size.x = float(t['radius']) * 2 ann.size.y = float(t['radius']) * 2 ann.size.z = float(t['height']) ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', t['pose']) # tables are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z / 2.0 anns_list.append(ann) object = Table() object.name = t['name'] object.radius = float(t['radius']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', t['pose']) data = AnnotationData() data.id = ann.data_id data.type = ann.type data.data = serialize_msg(object) data_list.append(data) return anns_list, data_list
def read(filename): yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = t['name'] ann.type = 'yocs_msgs/Wall' for i in range(0, random.randint(0,11)): ann.keywords.append('kw'+str(random.randint(1,11))) if 'prev_id' in vars(): ann.relationships.append(prev_id) prev_id = ann.id ann.shape = 1 # CUBE ann.color.r = 0.8 ann.color.g = 0.2 ann.color.b = 0.2 ann.color.a = 0.4 ann.size.x = float(t['width']) ann.size.y = float(t['length']) ann.size.z = float(t['height']) ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) # walls are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z/2.0 anns_list.append(ann) object = Wall() object.name = t['name'] object.length = float(t['length']) object.width = float(t['width']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) data = AnnotationData() data.id = ann.data_id data.data = pickle.dumps(object) data_list.append(data) print ann, object, data return anns_list, data_list
def read(file): yaml_data = None with open(file) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = t['name'] ann.type = 'yocs_msgs/Table' for i in range(0, random.randint(0,11)): ann.keywords.append('kw'+str(random.randint(1,11))) if 'prev_id' in vars(): ann.relationships.append(prev_id) prev_id = ann.id ann.shape = 3 #CYLINDER ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.8 ann.color.a = 0.5 ann.size.x = float(t['radius'])*2 ann.size.y = float(t['radius'])*2 ann.size.z = float(t['height']) ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) # tables are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z/2.0 anns_list.append(ann) object = Table() object.name = t['name'] object.radius = float(t['radius']) object.height = float(t['height']) object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) data = AnnotationData() data.id = ann.data_id data.type = ann.type data.data = serialize_msg(object) data_list.append(data) return anns_list, data_list
def read(file): yaml_data = None with open(file) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = t['name'] ann.type = 'ar_track_alvar_msgs/AlvarMarker' for i in range(0, random.randint(0, 11)): ann.keywords.append('kw' + str(random.randint(1, 11))) # if 'prev_id' in vars(): # ann.relationships.append(prev_id) # prev_id = ann.id ann.shape = 1 # CUBE ann.color.r = 1.0 ann.color.g = 1.0 ann.color.b = 1.0 ann.color.a = 1.0 ann.size.x = 0.18 ann.size.y = 0.18 ann.size.z = 0.01 ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', t['pose']) anns_list.append(ann) object = AlvarMarker() object.id = t['id'] object.confidence = t['confidence'] object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', t['pose']) data = AnnotationData() data.id = ann.data_id data.type = ann.type data.data = serialize_msg(object) data_list.append(data) return anns_list, data_list
def read(filename): yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) anns_list = [] data_list = [] for t in yaml_data: ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = t['name'] ann.type = 'ar_track_alvar/AlvarMarker' for i in range(0, random.randint(0,11)): ann.keywords.append('kw'+str(random.randint(1,11))) # if 'prev_id' in vars(): # ann.relationships.append(prev_id) # prev_id = ann.id ann.shape = 1 # CUBE ann.color.r = 1.0 ann.color.g = 1.0 ann.color.b = 1.0 ann.color.a = 1.0 ann.size.x = 0.18 ann.size.y = 0.18 ann.size.z = 0.01 ann.pose.header.frame_id = t['frame_id'] ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) anns_list.append(ann) object = AlvarMarker() object.id = t['id'] object.confidence = t['confidence'] object.pose.header.frame_id = t['frame_id'] object.pose.header.stamp = rospy.Time.now() object.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']) data = AnnotationData() data.id = ann.data_id data.data = pickle.dumps(object) data_list.append(data) print ann, object, data return anns_list, data_list
def create_alvar_marker_from_info(annotation_info, world, frame_id): ann = Annotation() ann.timestamp = rospy.Time.now() ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = "marker " + str(annotation_info['name']) ann.type = 'ar_track_alvar_msgs/AlvarMarker' ann.keywords.append(str(world)) ann.shape = 0 # Cylinder ann.color.r = 1.0 ann.color.g = 1.0 ann.color.b = 1.0 ann.color.a = 1.0 ann.size.x = 0.18 ann.size.y = 0.18 ann.size.z = 0.01 ann.pose.header.frame_id = frame_id ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose.position.x = annotation_info['x'] ann.pose.pose.pose.position.y = annotation_info['y'] ann.pose.pose.pose.position.z = annotation_info['height'] (ann.pose.pose.pose.orientation.x, ann.pose.pose.pose.orientation.y, ann.pose.pose.pose.orientation.z, ann.pose.pose.pose.orientation.w ) = tf.transformations.quaternion_from_euler( radians(annotation_info['roll']), radians(annotation_info['pitch']), radians(annotation_info['yaw'])) obj = ar_msgs.AlvarMarker() obj.id = int(annotation_info['name']) obj.confidence = 80 obj.pose.header = ann.pose.header obj.pose.pose = ann.pose.pose.pose return ann, obj
def __init__(self, feedback, uuid=None, priority=0, topic=common.SCHEDULER_TOPIC, frequency=common.HEARTBEAT_HZ): """ Constructor. """ if uuid is None: uuid = unique_id.fromRandom() self.requester_id = uuid """ :class:`uuid.UUID` of this requester. """ self.rset = RequestSet([], self.requester_id) """ :class:`.RequestSet` containing the current status of every :class:`.ResourceRequest` made by this requester. All requester operations are done using this object and its contents. """ self.priority = priority """ Default for new requests' priorities if none specified. """ self.feedback = feedback # requester feedback self.pub_topic = topic self.sub_topic = common.feedback_topic(uuid, topic) rospy.loginfo('ROCON requester feedback topic: ' + self.sub_topic) self.sub = rospy.Subscriber(self.sub_topic, SchedulerRequests, self._feedback) self.pub = rospy.Publisher(self.pub_topic, SchedulerRequests, latch=True) self.time_delay = rospy.Duration(1.0 / frequency) self._set_timer()
def __call__(self, msg, timeout=None, callback=None, error_callback=None): ''' Initiates and executes the client request to the server. The type of arguments supplied determines whether to apply blocking or non-blocking behaviour. If no callback is supplied, the mode is blocking, otherwise non-blocking. If no timeout is specified, then a return of None indicates that the operation timed out. :param msg: the request message :type msg: <name>Request :param rospy.Duration timeout: time to wait for data :param callback: user callback invoked for responses of non-blocking calls :type callback: method with arguments (uuid_msgs.UniqueID, <name>Response) :returns: msg/id for blocking calls it is the response message, for non-blocking it is the unique id :rtype: <name>Response/uuid_msgs.UniqueID or None (if timed out) ''' pair_request_msg = self.ServicePairRequest() pair_request_msg.id = unique_id.toMsg(unique_id.fromRandom()) pair_request_msg.request = msg key = unique_id.toHexString(pair_request_msg.id) if callback == None and error_callback == None: self._request_handlers[key] = BlockingRequestHandler(key) return self._make_blocking_call(self._request_handlers[key], pair_request_msg, timeout) else: request_handler = NonBlockingRequestHandler(key, callback, error_callback) self._request_handlers[key] = request_handler.copy() self._make_non_blocking_call(request_handler, pair_request_msg, timeout) return pair_request_msg.id
def new_request(self, resources, priority=None, uuid=None): """ Add a new scheduler request. Call this method for each desired new request, then invoke :py:meth:`.send_requests` to notify the scheduler. :param resources: ROCON resources requested :type resources: list of scheduler_msgs/Resource :param priority: Scheduling priority of this request. If ``None`` provided, use this requester's priority. :type priority: int :param uuid: UUID_ of this request. If ``None`` provided, a random UUID will be assigned. :type uuid: :class:`uuid.UUID` or ``None`` :returns: UUID (:class:`uuid.UUID`) assigned. :raises: :exc:`.WrongRequestError` if request already exists. """ if priority is None: priority = self.priority if uuid is None: uuid = unique_id.fromRandom() if uuid in self.rset: raise WrongRequestError('UUID already in use.') msg = Request(id=unique_id.toMsg(uuid), priority=priority, resources=resources, status=Request.NEW) self.rset[uuid] = ResourceRequest(msg) return uuid
def __call__(self, msg, timeout=None, callback=None, error_callback=None): ''' Initiates and executes the client request to the server. The type of arguments supplied determines whether to apply blocking or non-blocking behaviour. @param msg : the request message @type <name>Request @param timeout : time to wait for data @type rospy.Duration @param callback : user callback invoked for responses of non-blocking calls @type method with arguments (uuid_msgs.UniqueID, <name>Response) @return msg/id : for blocking calls it is the response message, for non-blocking it is the unique id @rtype <name>Response/uuid_msgs.UniqueID ''' pair_request_msg = self.ServicePairRequest() pair_request_msg.id = unique_id.toMsg(unique_id.fromRandom()) pair_request_msg.request = msg key = unique_id.toHexString(pair_request_msg.id) if callback == None and error_callback == None: self._request_handlers[key] = BlockingRequestHandler(key) return self._make_blocking_call(self._request_handlers[key], pair_request_msg, timeout) else: request_handler = NonBlockingRequestHandler( key, callback, error_callback) self._request_handlers[key] = request_handler.copy() self._make_non_blocking_call(request_handler, pair_request_msg, timeout) return pair_request_msg.id
def __init__(self, feedback, uuid=None, priority=0, topic=None, frequency=common.HEARTBEAT_HZ, lock=None): """ Constructor. """ if lock is None: lock = threading.RLock() self.lock = lock """ .. _Big_Requester_Lock: Big Requester Lock. This recursive Python threading lock serializes access to requester data. The requester *feedback* function is always invoked holding it. All :class:`.Requester` methods acquire it automatically, whenever needed. In any other thread, acquire it when updating shared request set objects. Never hold it when sleeping or waiting for I/O. """ if uuid is None: uuid = unique_id.fromRandom() self.requester_id = uuid """ :class:`uuid.UUID` of this requester. """ self.rset = RequestSet([], self.requester_id) """ :class:`.RequestSet` containing the current status of every :class:`.ResourceRequest` made by this requester. All requester operations are done using this object and its contents. """ self.priority = priority """ Default for new requests' priorities if none specified. """ self.feedback = feedback # requester feedback if topic is None: topic_param = rospy.search_param('topic_name') if topic_param is None: topic = common.SCHEDULER_TOPIC else: topic = rospy.get_param(topic_param) self.pub_topic = topic self.sub_topic = common.feedback_topic(uuid, topic) rospy.loginfo('ROCON requester feedback topic: ' + self.sub_topic) self.sub = rospy.Subscriber(self.sub_topic, SchedulerRequests, self._feedback, queue_size=1, tcp_nodelay=True) self.pub = rospy.Publisher(self.pub_topic, SchedulerRequests, latch=True, queue_size=1) self.time_delay = rospy.Duration(1.0 / frequency) self._set_timer()
def _ros_service_enable_concert_service(self, req): name = req.name success = False message = "unknown error" if req.enable: self.loginfo("serving request to enable '%s'" % name) else: self.loginfo("serving request to disable '%s'" % name) self.lock.acquire() # could be an expensive lock? # DJS : reload the service pool try: if req.enable: self._service_pool.reload_services() # Check if the service name is in the currently loaded service profiles if name not in self._enabled_services.keys(): try: service_instance = ServiceInstance( self._parameters['concert_name'], self._parameters['disable_cache'], self._service_pool.find(name).msg) except NoServiceExistsException: # do some updating of the service pool here raise NoServiceExistsException( "service not found on the package path [%s]" % name) unique_identifier = unique_id.fromRandom() self._setup_service_parameters( service_instance.msg.name, service_instance.msg.description, service_instance.msg.priority, unique_identifier) success, message = service_instance.enable( unique_identifier, self._interactions_loader) if not success: self._cleanup_service_parameters( service_instance.msg.name) else: self._enabled_services[ service_instance.name] = service_instance else: success = True message = "already enabled" else: if not name in self._enabled_services: raise NoServiceExistsException( "no enabled service with that name [%s]" % name) self._cleanup_service_parameters( self._enabled_services[name].msg.name) success, message = self._enabled_services[name].disable( self._interactions_loader) del self._enabled_services[name] except NoServiceExistsException as e: rospy.logwarn("Service Manager : %s" % str(e)) success = False message = str(e) self.publish_update() self.lock.release() return concert_srvs.EnableServiceResponse(success, message)
def add(self, annotation, msg=None, gen_uuid=True): ''' Add a new annotation with a new associated data or for an existing data. :param annotation: The new annotation. :param msg: Its associated data. If None, we assume that we are adding an annotation to existing data. :param gen_uuid: Generate an unique id for the new annotation or use the received one. :raises WCFError: If something went wrong. ''' if gen_uuid: annotation.id = unique_id.toMsg(unique_id.fromRandom()) else: for a in self._annotations: if a.id == annotation.id: message = "Duplicated annotation with uuid '%s'" % unique_id.toHexString( annotation.id) rospy.logerr(message) raise WCFError(message) if msg is None: # Msg not provided, so we assume that we are adding an annotation to existing data; find it by its id msg_found = False for d in self._annots_data: if d.id == annotation.data_id: rospy.logdebug("Annotation data with uuid '%s' found", unique_id.toHexString(annotation.data_id)) msg_found = True break if not msg_found: message = "Annotation data with uuid '%s' not found" % unique_id.toHexString( annotation.data_id) rospy.logerr(message) raise WCFError(message) else: # Annotation comes with its data; create a common unique id to link both annotation.data_id = unique_id.toMsg(unique_id.fromRandom()) annot_data = world_canvas_msgs.msg.AnnotationData() annot_data.id = annotation.data_id annot_data.type = annotation.type annot_data.data = serialize_msg(msg) self._annots_data.append(annot_data) self._annotations.append(annotation) self._saved = False
def _create_resource(self, uri): # Create a resource to request resource = scheduler_msgs.Resource() resource.id = unique_id.toMsg(unique_id.fromRandom()) resource.rapp = self.resource_type resource.uri = uri compressed_image_topic_remapped = self._get_remapped_topic(rocon_uri.parse(resource.uri).name.string) resource.remappings = [rocon_std_msgs.Remapping(self._default_compressed_image_topic, compressed_image_topic_remapped)] return resource
def pub_resource_alloc(self, uri, options): self.resource_alloc_flag = False data = adapter_msgs.Adapter() data.resource.id = unique_id.toMsg(unique_id.fromRandom()) data.resource.uri = uri data.resource.rapp = self.rapp_parse(uri) data.command = options time.sleep(1) self.resource_alloc_pub.publish(data)
def add(self, annotation, msg=None, gen_uuid=True): ''' Add a new annotation with a new associated data or for an existing data. @param annotation: The new annotation. @param msg: Its associated data. If None, we assume that we are adding an annotation to existing data. @param gen_uuid: Generate an unique id for the new annotation or use the received one. @raise WCFError: If something went wrong. ''' if gen_uuid: annotation.id = unique_id.toMsg(unique_id.fromRandom()) else: for a in self._annotations: if a.id == annotation.id: message = "Duplicated annotation with uuid '%s'" % unique_id.toHexString(annotation.id) rospy.logerr(message) raise WCFError(message) if msg is None: # Msg not provided, so we assume that we are adding an annotation to existing data; find it by its id msg_found = False for d in self._annots_data: if d.id == annotation.data_id: rospy.logdebug("Annotation data with uuid '%s' found", unique_id.toHexString(annotation.data_id)) msg_found = True break if not msg_found: message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(annotation.data_id) rospy.logerr(message) raise WCFError(message) else: # Annotation comes with its data; create a common unique id to link both annotation.data_id = unique_id.toMsg(unique_id.fromRandom()) annot_data = world_canvas_msgs.msg.AnnotationData() annot_data.id = annotation.data_id annot_data.type = annotation.type annot_data.data = serialize_msg(msg) self._annots_data.append(annot_data) self._annotations.append(annotation) self._saved = False
def pub_command(self, duration, options): self.command_flag = False data = adapter_msgs.Adapter() data.resource.id = unique_id.toMsg(unique_id.fromRandom()) data.command_duration = int(duration) data.command = options print data time.sleep(1) self.command_pub.publish(data)
def insert_marker(self, position, name=None, uuid=None, frame_id=None): if frame_id is None: frame_id = self.marker_frame if uuid is None: uuid = unique_id.fromRandom() if name is None: name = "wp{0}".format(self.next_waypoint_id) self.next_waypoint_id += 1 self.uuid_name_map[str(uuid)] = name # insert waypoint int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id int_marker.pose.position = position int_marker.pose.orientation.w = 1 int_marker.scale = 0.5 int_marker.name = str(uuid) int_marker.description = name # make markers moveable in the plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE # interactive menu for each marker menu_handler = MenuHandler() menu_handler.insert("Connect/Disconnect", callback=self.process_feedback) menu_handler.insert("Clear", callback=self.process_feedback) menu_handler.insert("Remove", callback=self.process_feedback) # make a box which also moves in the plane control.markers.append(self._make_marker(int_marker)) control.always_visible = True int_marker.controls.append(control) # we want to use our special callback function self.server.insert(int_marker, self.process_feedback) menu_handler.apply(self.server, int_marker.name) # set different callback for POSE_UPDATE feedback pose_update = InteractiveMarkerFeedback.POSE_UPDATE self.server.setCallback(int_marker.name, self.move_feedback, pose_update) self.server.applyChanges() # insert into graph self.waypoint_graph.add_node(str(uuid))
def __init__(self, service_namespace="~"): """ @param str service_namespace: """ self._service_namespace = rospy.resolve_name( service_namespace ) # converts relateive, '~' -> global appropriately self._unload_service_name = rosgraph.names.ns_join( self._service_namespace, 'unload') self._unload_configuration = rospy.ServiceProxy( self._unload_service_name, feed_the_troll_srvs.Unload) self._unique_identifier = unique_id.toMsg(unique_id.fromRandom())
def __init__(self, parent): self.parent = parent self.map = None # Generate a unique uuid for the whole session # No name field, so by default we save maps anonymously # Use the current ROS time in seconds as the session id for saved maps self.metadata = {'uuid': str(unique_id.fromRandom()), 'session_id': str(rospy.get_time()) } self.auto_save = rospy.get_param('~auto_save_map', False) self.map_saved = False
def read(file): ''' Parse a yaml file containing a single map message @param file Target file path ''' yaml_data = None with open(file) as f: yaml_data = yaml.load(f) ann = Annotation() ann.timestamp = rospy.Time.now() ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = map_name ann.type = 'nav_msgs/OccupancyGrid' ann.keywords.append(map_name) ann.shape = 1 # CUBE ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.2 ann.color.a = 0.01 ann.size.x = yaml_data['info']['width'] * yaml_data['info']['resolution'] ann.size.y = yaml_data['info']['height'] * yaml_data['info']['resolution'] ann.size.z = 0.000001 ann.pose.header.frame_id = '/map' ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', yaml_data['info']['origin']) object = OccupancyGrid() genpy.message.fill_message_args(object, yaml_data) map = AnnotationData() map.id = ann.data_id map.type = ann.type map.data = serialize_msg(object) return [ann], [ map ] # return as lists, as is what expects save_annotations_data service
def request_turtle(self, x_vel=0.1, z_vel=0.1, scale=1.0): ''' Request a turtle. ''' resource = scheduler_msgs.Resource() resource.id = unique_id.toMsg(unique_id.fromRandom()) resource.rapp = 'turtle_concert/turtle_stroll' resource.uri = 'rocon:/' resource_request_id = self.requester.new_request([resource], priority=self.service_priority) resource.parameters = [rocon_std_msgs.KeyValue('turtle_x_vel', str(x_vel)), rocon_std_msgs.KeyValue('turtle_z_vel', str(z_vel)), rocon_std_msgs.KeyValue('square_scale', str(scale))] self.pending_requests.append(resource_request_id) self.requester.send_requests()
def __init__(self, parent): self.parent = parent self.map = None # Generate a unique uuid for the whole session # No name field, so by default we save maps anonymously # Use the current ROS time in seconds as the session id for saved maps self.metadata = { 'uuid': str(unique_id.fromRandom()), 'session_id': str(rospy.get_time()) } self.auto_save = rospy.get_param('~auto_save_map', False) self.map_saved = False
def request_turtle(self, x_vel, z_vel, scale): ''' Request a turtle. ''' resource = scheduler_msgs.Resource() resource.id = unique_id.toMsg(unique_id.fromRandom()) resource.rapp = 'turtle_concert/turtle_stroll' resource.uri = 'rocon:/' resource_request_id = self.requester.new_request([resource], priority=self.service_priority) resource.parameters = [rocon_std_msgs.KeyValue('turtle_x_vel', str(x_vel)), rocon_std_msgs.KeyValue('turtle_z_vel', str(z_vel)), rocon_std_msgs.KeyValue('square_scale', str(scale))] self.pending_requests.append(resource_request_id) self.requester.send_requests()
def read(filename): ''' Parse a yaml file containing a single map message @param filename Target file path ''' yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) ann = Annotation() ann.timestamp = rospy.Time.now() ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id)) ann.data_id = unique_id.toMsg(unique_id.fromRandom()) ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.name = map_name ann.type = 'nav_msgs/OccupancyGrid' ann.keywords.append(map_name) ann.shape = 1 # CUBE ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.2 ann.color.a = 1.0 ann.size.x = yaml_data['info']['width'] * yaml_data['info']['resolution'] ann.size.y = yaml_data['info']['height'] * yaml_data['info']['resolution'] ann.size.z = 0.000001 ann.pose.header.frame_id = '/map' ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose', yaml_data['info']['origin']) object = OccupancyGrid() genpy.message.fill_message_args(object, yaml_data) map = AnnotationData() map.id = ann.data_id map.data = pickle.dumps(object) print ann return [ann], [map] # return as lists, as is what expects save_annotations_data service
def new_request(self, resources, priority=None, uuid=None, reservation=rospy.Time(), hold_time=rospy.Duration()): """ Add a new scheduler request. Call this method for each desired new request, then invoke :py:meth:`.send_requests` to notify the scheduler. :param resources: ROCON resources requested :type resources: list of scheduler_msgs/Resource :param priority: Scheduling priority of this request. If ``None`` provided, use this requester's default priority. :type priority: int :param uuid: UUID_ of this request. If ``None`` provided, a random UUID will be assigned. :type uuid: :class:`uuid.UUID` or ``None`` :param reservation: time when request desired, default: immediately. :type reservation: rospy.Time :param hold_time: estimated duration the resource will be held, default: unknown. :type hold_time: rospy.Duration :returns: UUID (:class:`uuid.UUID`) assigned. :raises: :exc:`.WrongRequestError` if request already exists. """ if priority is None: priority = self.priority status = Request.NEW if reservation != rospy.Time(): status = Request.RESERVED if uuid is None: uuid = unique_id.fromRandom() if uuid in self.rset: raise WrongRequestError('UUID already in use.') msg = Request(id=unique_id.toMsg(uuid), priority=priority, resources=resources, status=status, availability=reservation, hold_time=hold_time) with self.lock: self.rset[uuid] = msg return uuid
def __init__(self, name="", *args, **kwargs): """ :param str name: the behaviour name (doesn't have to be unique and can include capitals and spaces) """ assert isinstance(name, basestring), "a behaviour name should be a string, but you passed in %s" % type(name) self.id = unique_id.fromRandom() # used to uniquely identify this node (helps with removing children from a tree) self.name = name self.status = Status.INVALID self.iterator = self.tick() self.parent = None # will get set if a behaviour is added to a composite self.children = [] # only set by composite behaviours self.logger = logging.get_logger("Behaviour") self.feedback_message = "" # useful for debugging, or human readable updates, but not necessary to implement self.blackbox_level = common.BlackBoxLevel.NOT_A_BLACKBOX
def new_request(self, resources, priority=None, uuid=None, reservation=rospy.Time(), hold_time=rospy.Duration()): """ Add a new scheduler request. Call this method for each desired new request, then invoke :py:meth:`.send_requests` to notify the scheduler. :param resources: ROCON resources requested :type resources: list of scheduler_msgs/Resource :param priority: Scheduling priority of this request. If ``None`` provided, use this requester's default priority. :type priority: int :param uuid: UUID_ of this request. If ``None`` provided, a random UUID will be assigned. :type uuid: :class:`uuid.UUID` or ``None`` :param reservation: time when request desired, default: immediately. :type reservation: rospy.Time :param hold_time: estimated duration the resource will be held, default: unknown. :type hold_time: rospy.Duration :returns: UUID (:class:`uuid.UUID`) assigned. :raises: :exc:`.WrongRequestError` if request already exists. """ if priority is None: priority = self.priority status = Request.NEW if reservation != rospy.Time(): status = Request.RESERVED if uuid is None: uuid = unique_id.fromRandom() if uuid in self.rset: raise WrongRequestError("UUID already in use.") msg = Request( id=unique_id.toMsg(uuid), priority=priority, resources=resources, status=status, availability=reservation, hold_time=hold_time, ) with self.lock: self.rset[uuid] = msg return uuid
def __init__(self, minimum, resources): ''' Constructor fully initialises this request. @param minimum @type int @param resources : dict of resources eligible for use in constructing scheduler requests @type { uuid hexstring : scheduler_msgs.Resource } ''' self._resources = {} for resource in resources: resource.id = unique_id.toMsg(unique_id.fromRandom()) key = unique_id.toHexString(resource.id) self._resources[key] = ResourceTracker(resource) self._min = minimum self._max = len(resources) self._validate() # can raise an exception
def create_waypoint_from_info(annotation_info, world, frame_id): ann = Annotation() ann.timestamp = rospy.Time.now() ann.id = unique_id.toMsg(unique_id.fromRandom()) ann.world = world ann.name = annotation_info['name'] ann.type = 'yocs_msgs/Waypoint' ann.keywords.append(str(world)) ann.shape = 3 # Arrow ann.color.r = 0.2 ann.color.g = 0.2 ann.color.b = 0.8 ann.color.a = 0.5 ann.size.x = float(annotation_info['radius']) * 2 ann.size.y = float(annotation_info['radius']) * 2 ann.size.z = float(annotation_info['height']) ann.pose.header.frame_id = frame_id ann.pose.header.stamp = rospy.Time.now() ann.pose.pose.pose.position.x = annotation_info['x'] ann.pose.pose.pose.position.y = annotation_info['y'] ann.pose.pose.pose.position.z = 0.0 #annotation_info['height'] (ann.pose.pose.pose.orientation.x, ann.pose.pose.pose.orientation.y, ann.pose.pose.pose.orientation.z, ann.pose.pose.pose.orientation.w ) = tf.transformations.quaternion_from_euler( radians(annotation_info['roll']), radians(annotation_info['pitch']), radians(annotation_info['yaw'])) obj = yocs_msgs.Waypoint() obj.name = annotation_info['name'] obj.header.frame_id = frame_id obj.header.stamp = rospy.Time.now() obj.pose.position.x = annotation_info['x'] obj.pose.position.y = annotation_info['y'] obj.pose.position.z = 0.0 (obj.pose.orientation.x, obj.pose.orientation.y, obj.pose.orientation.z, obj.pose.orientation.w) = tf.transformations.quaternion_from_euler( radians(annotation_info['roll']), radians(annotation_info['pitch']), radians(annotation_info['yaw'])) # waypoints are assumed to lay on the floor, so z coordinate is zero; # but WCF assumes that the annotation pose is the center of the object ann.pose.pose.pose.position.z += ann.size.z / 2.0 return ann, obj
def resources_alloc_request_callback(self, msg): ''' For allocating a resource, this method is invoked by receiving a topic from bpel service @param msg: incoming message @type msg: adapter_msgs.Adapter ''' result = False # send a request rospy.loginfo("AdapterSvc : send request by topic") data = adapter_msgs.Adapter() data.resource.id = unique_id.toMsg(unique_id.fromRandom()) data.resource.uri = msg.resource.uri data.resource.rapp = msg.resource.rapp self.command = msg.command self.command_duration = msg.command_duration data.command = self.command data.command_duration = self.command_duration resource_request_id = self.requester.new_request([data.resource]) self.pending_requests.append(resource_request_id) self.requester.send_requests() timeout_time = time.time() + self.allocation_timeout while not rospy.is_shutdown() and time.time() < timeout_time: if resource_request_id in self.pending_requests: self.allocated_requests[msg.resource.uri] = resource_request_id result = True break rospy.rostime.wallsleep(0.1) if result == False: rospy.logwarn( "AdapterSvc : couldn't capture required resource [timed out][%s]" % msg.resource.uri) self.requester.rset[resource_request_id].cancel() else: rospy.loginfo( "AdapterSvc : captured resouce [%s][%s]" % (msg.resource.uri, self.allocated_requests[msg.resource.uri]))
def _service_profile_to_msg(self, loaded_profile): """ Change service proflies data to ros message :returns: generated service profile message :rtype: [concert_msgs.ServiceProfile] """ msg = concert_msgs.ServiceProfile() msg.uuid = unique_id.toMsg(unique_id.fromRandom()) # todo change more nice method if 'resource_name' in loaded_profile: msg.resource_name = loaded_profile['resource_name'] if 'name' in loaded_profile: msg.name = loaded_profile['name'] if 'description' in loaded_profile: msg.description = loaded_profile['description'] if 'author' in loaded_profile: msg.author = loaded_profile['author'] if 'priority' in loaded_profile: msg.priority = loaded_profile['priority'] if 'launcher_type' in loaded_profile: msg.launcher_type = loaded_profile['launcher_type'] if 'icon' in loaded_profile: msg.icon = rocon_python_utils.ros.icon_resource_to_msg(loaded_profile['icon']) if 'launcher' in loaded_profile: msg.launcher = loaded_profile['launcher'] if 'interactions' in loaded_profile: msg.interactions = loaded_profile['interactions'] if 'parameters' in loaded_profile: msg.parameters = loaded_profile['parameters'] if 'parameters_detail' in loaded_profile: for param_key in loaded_profile['parameters_detail'].keys(): msg.parameters_detail.append(rocon_std_msgs.KeyValue(param_key, str(loaded_profile['parameters_detail'][param_key]))) # t = type(loaded_profile['parameters_detail'][param_key]) # if t is list or t is dict: # msg.parameters_detail.append(rocon_std_msgs.KeyValue(param_key, eval(loaded_profile['parameters_detail'][param_key]))) # else: # msg.parameters_detail.append(rocon_std_msgs.KeyValue(param_key, str(loaded_profile['parameters_detail'][param_key]))) return msg
def resources_alloc_request_callback(self, msg): ''' For allocating a resource, this method is invoked by receiving a topic from bpel service @param msg: incoming message @type msg: adapter_msgs.Adapter ''' result = False # send a request rospy.loginfo("AdapterSvc : send request by topic") data = adapter_msgs.Adapter() data.resource.id = unique_id.toMsg(unique_id.fromRandom()) data.resource.uri = msg.resource.uri data.resource.rapp = msg.resource.rapp self.command = msg.command self.command_duration = msg.command_duration data.command = self.command data.command_duration = self.command_duration resource_request_id = self.requester.new_request([data.resource]) self.pending_requests.append(resource_request_id) self.requester.send_requests() timeout_time = time.time() + self.allocation_timeout while not rospy.is_shutdown() and time.time() < timeout_time: if resource_request_id in self.pending_requests: self.allocated_requests[msg.resource.uri] = resource_request_id result = True break rospy.rostime.wallsleep(0.1) if result == False: rospy.logwarn("AdapterSvc : couldn't capture required resource [timed out][%s]" % msg.resource.uri) self.requester.rset[resource_request_id].cancel() else: rospy.loginfo("AdapterSvc : captured resouce [%s][%s]" % (msg.resource.uri, self.allocated_requests[msg.resource.uri]))
def _load_profile(self): """ :raises: :exc:`rospkg.ResourceNotFound` if the service profile yaml is not found :raises: :exc:`concert_service_manager.InvalidServiceProfileException` if the service profile yaml is invalid """ msg = concert_msgs.ServiceProfile() try: filename = self._filename() # this can raise ResourceNotFound, IOError with open(filename) as f: loaded_profile = yaml.load(f) except rospkg.ResourceNotFound as e: raise e loaded_profile['resource_name'] = self.resource_name # override parameters for key in loaded_profile: if key in self._overrides and self._overrides[key] is not None: loaded_profile[key] = self._overrides[key] if 'launcher_type' not in loaded_profile.keys(): # not set loaded_profile['launcher_type'] = concert_msgs.ServiceProfile.TYPE_SHADOW # We need to make sure the service description name is a valid rosgraph namespace name # @todo Actually call the rosgraph function to validate this (do they have converters?) loaded_profile['name'] = loaded_profile['name'].lower().replace(" ", "_") # load icon if necessary replace = {} if 'icon' in loaded_profile: replace[loaded_profile['icon']] = rocon_python_utils.ros.icon_resource_to_msg(loaded_profile['icon']) # generate message genpy.message.fill_message_args(msg, loaded_profile, replace) # fill in unique identifier used by services and their requesters msg.uuid = unique_id.toMsg(unique_id.fromRandom()) return msg
def insert_marker(self, position, waypoint_type=WAYPOINT_REGULAR, name=None, uuid=None, frame_id=None, floor_level=0): if frame_id is None: frame_id = self.marker_frame if uuid is None: uuid = unique_id.fromRandom() if name is None: name = "wp{:03}".format(self.next_waypoint_id) self.next_waypoint_id += 1 self.uuid_name_map[str(uuid)] = name self.waypoint_type = waypoint_type # insert waypoint int_marker = InteractiveMarker() int_marker.header.frame_id = frame_id if waypoint_type == WAYPOINT_REGULAR: int_marker.pose.position = position int_marker.pose.orientation.w = 1 elif waypoint_type == WAYPOINT_TERMINATING: int_marker.pose.position = position.position int_marker.pose.orientation = position.orientation int_marker.scale = 0.5 int_marker.name = str(uuid) int_marker.description = name # make markers moveable in the plane control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 if waypoint_type == WAYPOINT_REGULAR: control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE elif waypoint_type == WAYPOINT_TERMINATING: control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE # interactive menu for each marker menu_handler = MenuHandler() menu_handler.insert("Connect normal edge...", callback=self.process_feedback) menu_handler.insert("Connect door edge...", callback=self.process_feedback) menu_handler.insert("Connect elevator edge...", callback=self.process_feedback) menu_handler.insert("Disconnect edge...", callback=self.process_feedback) menu_handler.insert("Clear connected edges", callback=self.process_feedback) menu_handler.insert("Rename marker...", callback=self.process_feedback) menu_handler.insert("Remove marker", callback=self.process_feedback) # make a box which also moves in the plane control.markers.append(self._make_marker(int_marker)) control.always_visible = True int_marker.controls.append(control) # we want to use our special callback function self.server.insert(int_marker, self.process_feedback) menu_handler.apply(self.server, int_marker.name) # set different callback for POSE_UPDATE feedback pose_update = InteractiveMarkerFeedback.POSE_UPDATE self.server.setCallback(int_marker.name, self.move_feedback, pose_update) self.server.applyChanges() # insert into graph self.waypoint_graph.add_node(str(uuid), waypoint_type=waypoint_type, floor_level=floor_level, position=position)
def ros_capture_teleop_callback(self, request_id, msg): ''' Processes the service pair server 'capture_teleop'. This will run in a thread of its own for each request. It has a significantly long lock though - this needs to get fixed. ''' response = rocon_service_msgs.CaptureTeleopResponse() response.result = False # Todo : request the scheduler for this resource, # use self.allocation_timeout to fail gracefully self.lock.acquire() if not msg.release: # i.e. we're capturing: if msg.rocon_uri not in [r.uri for r in self.teleopable_robots]: rospy.logwarn( "TeleopPimp : couldn't capture teleopable robot [not available][%s]" % msg.rocon_uri) response.result = False self.allocate_teleop_service_pair_server.reply( request_id, response) else: # send a request resource = scheduler_msgs.Resource() resource.id = unique_id.toMsg(unique_id.fromRandom()) resource.rapp = 'turtle_concert/teleop' resource.uri = msg.rocon_uri resource_request_id = self.requester.new_request([resource]) self.pending_requests.append(resource_request_id) self.requester.send_requests() timeout_time = time.time() + 5.0 while not rospy.is_shutdown() and time.time() < timeout_time: if resource_request_id not in self.pending_requests: self.allocated_requests[ msg.rocon_uri] = resource_request_id response.result = True break rospy.rostime.wallsleep(0.1) if response.result == False: rospy.logwarn( "TeleopPimp : couldn't capture teleopable robot [timed out][%s]" % msg.rocon_uri) self.requester.rset[resource_request_id].cancel() else: rospy.loginfo( "TeleopPimp : captured teleopable robot [%s][%s]" % (msg.rocon_uri, self.allocated_requests[msg.rocon_uri])) self.allocate_teleop_service_pair_server.reply( request_id, response) else: # we're releasing if msg.rocon_uri in self.allocated_requests.keys(): rospy.loginfo( "TeleopPimp : released teleopable robot [%s][%s]" % (msg.rocon_uri, self.allocated_requests[msg.rocon_uri].hex)) self.requester.rset[self.allocated_requests[ msg.rocon_uri]].cancel() self.requester.send_requests() response.result = True self.allocate_teleop_service_pair_server.reply( request_id, response) self.lock.release()
def __init__(self): """ Constructor for AgrosPathGenerator """ rospy.loginfo('Initializing AgrosPathGenerator...\n\n' '\t(Note: this class polls rosparams on\n' + '\tconstruction. If you are setting any parameters\n' + '\tmanually, please review the API and call the\n' + '\tright functions (e.x. call update_ab_line()\n' + '\tafter setting a and b with geographic points).\n') self.path_pub = rospy.Publisher('~path', Path, queue_size=1) self.route_pub = rospy.Publisher('~route_network', RouteNetwork, queue_size=1) self.path = Path() self.route_network = RouteNetwork() self.route_network.id = unique_id.toMsg(unique_id.fromRandom()) self.frame_id = rospy.get_param('~frame_id', 'odom') # Check UTM params ----------------------------------------------------- self.zone = rospy.get_param('~utm_zone', None) if not self.zone: rospy.logwarn('The ROS param ~utm_zone is required by this node') self.band = rospy.get_param('~utm_band', None) if not self.zone: rospy.logwarn('The ROS param ~utm_band is required by this node') # Configure the AB line, if given -------------------------------------- self.ab = None self.azimuth = None temp_a = rospy.get_param('~a', {}) # temp dict if ('latitude' in temp_a and 'longitude' in temp_a and 'altitude' in temp_a): lat = temp_a['latitude'] lon = temp_a['longitude'] alt = temp_a['altitude'] self.a_geo = fromLatLong(lat, lon, alt) temp_b = rospy.get_param('~b', {}) # temp dict if ('latitude' in temp_b and 'longitude' in temp_b and 'altitude' in temp_b): lat = temp_b['latitude'] lon = temp_b['longitude'] alt = temp_b['altitude'] self.b_geo = fromLatLong(lat, lon, alt) # Update the AB line itself as a shapely LineString self.update_ab_line() # ab and ab_angle should be set after calling # Configure the boundaries, if given ----------------------------------- self.perimeter = None temp_boundary = rospy.get_param('~boundary', None) # temp dict if 'points' in temp_boundary: self.boundary_geo = [] for point in temp_boundary['points']: if ('latitude' in point and 'longitude' in point and 'altitude' in point): lat = point['latitude'] lon = point['longitude'] alt = point['altitude'] wp = fromLatLong(lat, lon, alt) self.boundary_geo.append(wp) # Update the perimeter itself as a shapely LinearRing self.update_perimeter() # perimeter should be set after calling # Configure headlands, if given ---------------------------------------- self.headlands = None self.headland_width = rospy.get_param('~headland_width', None) # Update the headlands as a shapely LinearRing self.update_headlands() # headlands should be set after calling # Attempt to generate boustrophedon path ------------------------------- self.waypoints = None self.segments = None self.tool_width = rospy.get_param('~tool_width', None) temp_entry = rospy.get_param('~entry_position', None) if ('latitude' in temp_entry and 'longitude' in temp_entry and 'altitude' in temp_entry): lat = temp_entry['latitude'] lon = temp_entry['longitude'] alt = temp_entry['altitude'] self.entry_geo = fromLatLong(lat, lon, alt) # Generate a Boustrophedon patten if all other properties are updated self.generate_boustrophedon() # Convert the Euclidean path back to geographic coordinates ------------ self.update_path() self.update_geo_path() # Perform Visualization using matplotlib ------------------------------- self.visualize = rospy.get_param('~visualize', False) if self.visualize: self.plot()