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 __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 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 __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 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 __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 __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 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()
