def main():
rospy.init_node('warehouse')
if not rospy.has_param('~size'):
raise rospy.ROSInitException('Parameter "size" must be set [length, height]')
if not rospy.has_param('~offset'):
raise rospy.ROSInitException('Parameter "offset" must be set [length, height]')
if not rospy.has_param('~warehouse'):
raise rospy.ROSInitException('Parameter "warehouse" muse be set "raws" or "goods"')
if not rospy.has_param('~content'):
raise rospy.ROSInitException('Items type must be specified in parameter "content"')
warehouse_size = ast.literal_eval(rospy.get_param('~size'))
global warehouse_offset
warehouse_offset = ast.literal_eval(rospy.get_param('~offset'))
global wh
wh = Warehouse(warehouse_size)
rospy.Service('~place', Place, handle_place) # put item in cell
rospy.Service('~order', Order, handle_order) # get item from cell
rospy.Service('~fill_all', FillAll, handle_fill_all)
rospy.Service('~empty_all', EmptyAll, handle_empty_all)
rospy.Service('~quantity_available', QuantityAvailable, handle_quantity_available)
rospy.Service('~content', Content, handle_content)
rospy.spin()
def initializeRobots():
"""!
This function determines the names of each robot that should be controlled. It then creates
a @ref Robot object for each robot. Those objects perform the correct initialization of needed
parameters.
@return A list of all created Robot objects
@throw rospy.ROSInitException Thrown if no robot_list parameter is found or if no robots are
specified.
"""
# Determine the list of robots and error if not found.
robot_names = ParameterLookup.lookup('/robot_list')
# Throw an error if there are no robots specified.
if len(robot_names) < 1:
error_string = 'Must specify at least one robot in /robot_list'
rospy.logfatal(error_string)
raise rospy.ROSInitException(error_string)
# There is a chance the user specifies only a single robot without brackets, which would make
# this parameter as a string. Check for that and convert it to a list for use later.
if type(robot_names) is not list:
single_robot_name = robot_names
robot_names = [single_robot_name]
# Create each robot object and add it to a list.
robot_list = []
for name in robot_names:
robot_list.append(Robot(name))
return robot_list
def start(self):
""" Starts the Profiler
:raises: ROSInitException when /enable_statistics has not been set to True
"""
# Make sure that /enable_statistics is set
if not rospy.get_param('/enable_statistics', False):
raise rospy.ROSInitException(
"Rosparam '/enable_statistics' has not been set to true. Aborting"
)
if self._monitor_timer is not None:
raise Exception("Monitor Timer already started!")
if self._graphupdate_timer is not None:
raise Exception("Graph Update Timer already started!")
if self._publisher_timer is not None:
raise Exception("Publisher Timer already started!")
# Initialize process list
self._update_node_list()
# Make sure all nodes are starting out blank
for node in self._nodes.values():
node.reset()
# Start Timers
self._monitor_timer = rospy.Timer(self.sample_rate, self._collect_data)
self._publisher_timer = rospy.Timer(self.update_rate,
self._publish_data)
self._graphupdate_timer = rospy.Timer(self.update_rate,
self._update_node_list)
def _configure_internal_topics(self, pub_names):
"""
Create subscriptions to all the topics that we externally publish.
"""
i = 0
for name in pub_names:
resolved_name = rospy.resolve_name(name)
rospy.loginfo("configuring internal subscriber [%s]",
resolved_name)
try:
real_msg_class, _, _ = rostopic.get_topic_class(resolved_name)
except rostopic.ROSTopicException:
raise rospy.ROSInitException(
"cannot proxy subscription [%s]: unknown topic type" %
resolved_name)
i += 1
topic_class = classobj('t_passthrough_%s' % i,
(rospy.msg.AnyMsg, ), {
'_type': real_msg_class._type,
'_md5sum': real_msg_class._md5sum,
})
self.subs_internal[resolved_name] = rospy.Subscriber(
name, topic_class)
rospy.loginfo("proxying %s", resolved_name)
def _initializeKeypoints(self, param_name):
"""!
This is an internal function to look up and process the list of keypoints
that is provided for the robot. It converts them to a list of
geometry_msgs/Point. All parameters are assumed within the robot's namespace
(which is also within the node's namespace)
@param param_name The parameter to lookup, without the robot's name.
@return The keypoints as a list of geometry_msgs/Point.
@throw rospy.InitException Thrown if the keypoints parameter is not
found.
"""
keypoint_param = ParameterLookup.lookup(param_name)
# The specifications are imported as a list of lists, so iterate
# through the outer one and convert the inner into the message
# format.
keypoints = []
for keypoint_list in keypoint_param:
# Verify that each point has three entitites for the 3 dimensions.
if len(keypoint_list) != 3:
error_string = 'Keypoints must have all three dimensions: [x, y, z]'
rospy.logfatal(error_string)
raise rospy.ROSInitException(error_string)
keypoint_message = Point()
keypoint_message.x = keypoint_list[0]
keypoint_message.y = keypoint_list[1]
keypoint_message.z = keypoint_list[2]
keypoints.append(keypoint_message)
return keypoints
def fetch_definition(self):
get_def = rospy.ServiceProxy('memory/definitions/get', GetDefinition)
get_def.wait_for_service()
try:
res = get_def(self.DEF_FILE)
if not res.success:
msg = "Can't fetch belt definition file. Shutting down."
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
else:
rospy.logdebug("Belt definition file fetched.")
return res.path
except rospy.ServiceException as exc:
msg = "Exception when fetching belt definition file. Shutting down.\n {}".format(str(exc))
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
def _configure_internal_services(self, service_names):
"""
Create rospy handles to all of the services that we are
configured to be able to call.
"""
i = 0
for name in service_names:
rospy.loginfo("configuring service %s", name)
resolved_name = rospy.resolve_name(name)
rospy.wait_for_service(name, timeout=60.)
type_ = rosservice.get_service_type(resolved_name)
if type_ is None:
raise rospy.ROSInitException(
"cannot proxy service [%s]: unknown type" % resolved_name)
i += 1
# for efficiency, we generate a passthrough service
# definition that does not do any serializatoin on the
# request or response. This requires more work because the
# instantiated class has to have the correct properties.
real_service_class = roslib.message.get_service_class(type_)
real_req = real_service_class._request_class
real_resp = real_service_class._response_class
request_d = {
'_type': real_req._type,
'_md5sum': real_req._md5sum,
'_full_text': real_req._full_text,
}
response_d = {
'_type': real_resp._type,
'_md5sum': real_resp._md5sum,
'_full_text': real_resp._full_text,
}
service_class = classobj(
's_passthrough_%s' % i, (object, ), {
'_type':
real_service_class._type,
'_md5sum':
real_service_class._md5sum,
'_request_class':
classobj('passthrough_request_%s' % i,
(PassthroughServiceMessage, ), request_d),
'_response_class':
classobj('passthrough_response_%s' % i,
(PassthroughServiceMessage, ), response_d),
})
self.services[resolved_name] = rospy.ServiceProxy(name,
service_class,
persistent=True)
rospy.loginfo("proxying %s", resolved_name)
def __init__(self):
self._planner_client = actionlib.SimpleActionClient(
"planner_request", PlannerRequestAction)
self._planner_client.wait_for_server()
if rospy.is_shutdown():
raise rospy.ROSInitException()
self._goals = [(1, 2), (3, 4)]
self._rate = rospy.Rate(10)
def __init__(self):
self._client = actionlib.SimpleActionClient("planner_request",
PlannerRequestAction)
self._client.wait_for_server()
self._sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped,
self._point_cb)
self._lock = threading.RLock()
if rospy.is_shutdown():
raise rospy.ROSInitException()
def updatePlanningGroup(self, next_index):
if next_index >= len(self.planning_groups_keys):
self.current_planning_group_index = 0
elif next_index < 0:
self.current_planning_group_index = len(self.planning_groups_keys) - 1
else:
self.current_planning_group_index = next_index
next_planning_group = None
try:
next_planning_group = self.planning_groups_keys[self.current_planning_group_index]
except IndexError:
msg = 'Check if you started movegroups. Exiting.'
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
rospy.loginfo("Changed planning group to " + next_planning_group)
self.plan_group_pub.publish(next_planning_group)
def lookup(parameter):
"""
@brief Look up an essential parameter.
If this parameter is not found, the user is warned and an
Exception is raised.
@param parameter The parameter to look up.
@return The value of the parameter from the server.
@throw rospy.InitException Thrown if the parameter is not found.
"""
if not rospy.has_param(param_name=parameter):
error_string = 'Parameter not found for {param}'.format(
param=parameter)
rospy.logfatal(msg=error_string)
raise rospy.ROSInitException(error_string)
# This is only reached if the parameter exists, so look it up.
result = rospy.get_param(param_name=parameter)
return result
def __init__(self, name=None):
self._NAME = name or "rosgrapher"
# Force singleton by not allowing name to be remapped
if not re.findall("^/*(.+)$", rospy.get_name())[0] == self._NAME:
raise rospy.ROSInitException(
"Node '%s' of type rosprofiler/rosgrapher should only use the name '%s' to avoid being run multiple times." % (rospy.get_name(), self._NAME))
self._master = rosgraph.Master(self._NAME)
self._publisher = rospy.Publisher('/topology', Graph, queue_size=10, latch=True)
self._poller_timer = None
# Message Sequence Number
self._seq = 1
# Previous values for comparison
self._old_nodes = dict()
self._old_topics = dict()
def __init__(self, name=None, interval=2, verbose="false"):
self._NAME = name or "rosgrapher"
self._POLL_INTERVAL = int(interval)
self._VERBOSE_MODE = verbose == "true"
# Force singleton by not allowing name to be remapped
if not re.findall("^/*(.+)$", rospy.get_name())[0] == self._NAME:
raise rospy.ROSInitException(
"Node '%s' of type rosprofiler/rosgrapher should only use the name '%s' to avoid being run multiple times." % (rospy.get_name(), self._NAME))
self._master = rosgraph.Master(self._NAME)
self._publisher = rospy.Publisher('/topology', Graph, queue_size=10, latch=True)
self._poller_timer = None
self._service = rospy.Service('/topology', GetTopologyGraph, self._service_callback)
# Message Sequence Number
self._seq = 1
# Previous values for comparison
self._old_nodes = dict()
self._old_topics = dict()
def __init__(self):
# General swarm information
self.vehs = rospy.get_param('/vehs')
self.n = len(self.vehs)
rospy.loginfo('Using vehicles: {}'.format(self.vehs))
# Load desired formations
self.send_gains = rospy.get_param('~send_gains', False)
formation_group = rospy.get_param('~formation_group')
self.formations = rospy.get_param('~{}'.format(formation_group))
self.manageAdjmat()
if self.formations['agents'] != self.n:
rospy.logerr('Mismatch between number of vehicles ({}) '
'and formation group agents ({})'.format(
self.n, self.formations['agents']))
rospy.signal_shutdown('Incorrect formation')
raise rospy.ROSInitException()
fnames = ', '.join([f['name'] for f in self.formations['formations']])
rospy.loginfo('Using formation group \'{}\': {}'.format(formation_group, fnames))
self.formidx = -1 # for cycling through formations in the group
# Behavior selector and flight status management
self.status = NOT_FLYING
self.pubEvent = rospy.Publisher(
'/globalflightmode', QuadFlightMode, queue_size=1, latch=True)
self.srv = rospy.Service("change_mode", MissionModeChange, self.srvCB)
# Publishers
self.formationPub = rospy.Publisher(
'/formation', Formation, queue_size=10)
self.pubMarker = rospy.Publisher(
"/highbay", MarkerArray, queue_size=1, latch=True)
# Handle centralized assignment --- only for comparison
self.central_assignment = rospy.get_param('~central_assignment', False)
self.assignment_dt = rospy.get_param('~central_assignment_dt', 0.75)
if self.central_assignment:
rospy.logwarn('Generating centralized assignment')
# last assignment
self.P = None
# ground truth state of each vehicle
self.poses = [None]*self.n
for idx,veh in enumerate(self.vehs):
rospy.Subscriber('/{}/world'.format(veh), PoseStamped,
lambda msg, i=idx: self.poseCb(msg, i), queue_size=1)
self.tim_sendassign = rospy.Timer(rospy.Duration(self.assignment_dt),
self.sendAssignmentCb)
self.pub_assignment = rospy.Publisher('/central_assignment',
UInt8MultiArray, queue_size=1)
# Safety bounds for visualization
self.xmax = rospy.get_param('/room_bounds/x_max', 0.0)
self.xmin = rospy.get_param('/room_bounds/x_min', 1.0)
self.ymax = rospy.get_param('/room_bounds/y_max', 0.0)
self.ymin = rospy.get_param('/room_bounds/y_min', 1.0)
self.zmin = rospy.get_param('/room_bounds/z_min', 0.0)
self.zmax = rospy.get_param('/room_bounds/z_max', 1.0)
self.genEnvironment()
#!/usr/bin/env python
import rospy
import actionlib
from stacker import Stacker
if __name__ == '__main__':
rospy.init_node('stacker', log_level=rospy.DEBUG)
node_name = rospy.get_name()
rospy.logwarn('Node name: ' + node_name)
opcua_endpoint = rospy.get_param('~opcua_endpoint')
if not rospy.has_param('~opcua_server_namespace'):
raise rospy.ROSInitException(
'Parameter "opcua_server_namespace" must be specified in accordance with OPCU-UA'
'Model. Example: /Airalab/Stacker_goods')
opcua_server_namespace = rospy.get_param('~opcua_server_namespace')
if 'ns=' not in opcua_server_namespace: # use only string type nodeId
raise rospy.ROSInitException(
'Parameter "opcua_server_namespace" template: "ns=<int>;s=/<VendorName>/<ObjectName>"')
if not rospy.has_param('opcua_client_node'):
rospy.logwarn('Using default ROS OPC-UA Client node path: /opcua/opcua_client')
rospy.logwarn('You can specify it in parameter \'opcua_client_node\'')
opcua_client_node = rospy.get_param('opcua_client_node', '/opcua/opcua_client')
if not rospy.has_param('~direction'):
raise rospy.ROSInitException(
'Parameter "direction" must be specified "forward", "fw" or "backward", "bw"')
direction = rospy.get_param('~direction')
def __init__(self, file):
super(BeltParser, self).__init__()
rospy.logdebug("Parsing belt definition...")
root = ET.parse(file).getroot()
required = ["max_range", "angle", "precision", "scale_responsive"]
# parse params
if required and root.find("params") is None:
msg = "Can't parse belt definition file: a 'params' element is required. Shutting down."
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
self.Params = {
p.attrib["type"]: {c.tag: float(c.text)
for c in p}
for p in root.find("params")
}
rospy.loginfo(self.Params)
for p in required:
for s in self.Params:
if p not in self.Params[s]:
msg = "Can't parse belt definition: a '{}' element is required in the parameters. Shutting down."\
.format(p)
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
# parse sensors
if root.find("sensors") is None:
msg = "Can't parse belt definition: a 'sensors' element is required. Shutting down."
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
sensors = []
for sensor in root.find("sensors"):
if "id" not in sensor.attrib:
rospy.logerr(
"Can't parse sensor definition: a 'id' attribute is required. Skipping this sensor."
)
continue
required = ["x", "y", "a"]
for p in required:
if sensor.find(p) is None:
rospy.logerr(
"Can't parse sensor definition: a '{}' element is required. Skipping this sensor."
.format(p))
if "type" not in sensor.attrib:
rospy.logerr(
"Can't parse sensor definition: a 'type' attribute is required. Skipping this sensor."
)
continue
if "layer" not in sensor.attrib:
rospy.logerr(
"Can't parse sensor definition: a 'layer' attribute is required. Skipping this sensor."
)
continue
if sensor.attrib["type"] not in self.Params:
rospy.logerr(
"Can't parse sensor definition: {} sensor type is not defined. Skipping this sensor."
.format(sensor.attrib["type"]))
continue
sensors.append({
"id": sensor.attrib["id"],
"x": float(sensor.find("x").text),
"y": float(sensor.find("y").text),
"a": float(sensor.find("a").text),
"type": sensor.attrib["type"],
"layer": sensor.attrib["layer"]
})
if not sensors:
rospy.logwarn("No sensor found in belt definition.")
rospy.logdebug("{} sensors found in belt definition".format(
len(sensors)))
self.Sensors = {s["id"]: s for s in sensors}
rospy.logwarn(
'Deprecated state code: %d, set state to undefined (-1)' %
self.state)
self.state = -1
rospy.logdebug('New plant state: ' + str(self.state))
time.sleep(1)
if __name__ == '__main__':
rospy.init_node('plant', log_level=rospy.DEBUG)
node_name = rospy.get_name()
opcua_endpoint = rospy.get_param('~opcua_endpoint')
if not rospy.has_param('~opcua_server_namespace'):
raise rospy.ROSInitException(
'Parameter "opcua_server_namespace" must be specified in accordance with OPCU-UA'
'Model. Example: /Airalab/Plant1')
opcua_server_namespace = rospy.get_param('~opcua_server_namespace')
if 'ns=' not in opcua_server_namespace: # use only string type nodeId
raise rospy.ROSInitException(
'Parameter "opcua_server_namespace" template: "ns=<int>;s=/<VendorName>/<PlantName>"'
)
if not rospy.has_param('opcua_client_node'):
rospy.logwarn(
'Using default ROS OPC-UA Client node path: /opcua/opcua_client')
rospy.logwarn('You can specify it in parameter \'opcua_client_node\'')
opcua_client_node = rospy.get_param('opcua_client_node',
'/opcua/opcua_client')
unload_time = rospy.get_param('~unload_time', 2000)
def run(self):
# this check is needed, as sometimes there are issues with simulated time
while rospy.Time.now() == rospy.Time(0) and not rospy.is_shutdown():
rospy.logwarn('TaskManager: waiting on time')
rospy.sleep(0.005)
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
# check to make sure all dependencies are ready
if not self._wait_until_ready(self._timeout):
raise rospy.ROSInitException()
# forming bond with safety client, if enabled
if self._safety_enabled and not self._safety_client.form_bond():
raise IARCFatalSafetyException(
'TaskManager: could not form bond with safety client')
while not rospy.is_shutdown():
# main loop
with self._lock:
if self._safety_enabled and self._safety_client.is_fatal_active(
):
raise IARCFatalSafetyException('TaskManager: Safety Fatal')
if self._safety_enabled and self._safety_client.is_safety_active(
):
rospy.logerr('TaskManager: Activating safety response')
self._safety_responder.activate_safety_response()
return
if self._task is None and self._action_server.has_new_task():
# new task is available
try:
request = self._action_server.get_new_task()
# use provided handler to check that request is valid
new_task = self._handler.check_request(request)
if new_task is not None:
self._task = new_task
self._action_server.set_accepted()
else:
rospy.logerr('TaskManager: new task is invalid')
self._action_server.set_rejected()
except Exception as e:
rospy.logfatal(
'TaskManager: Exception getting new task')
raise
if self._task is not None and self._action_server.task_canceled(
):
# there is a task running, but it is canceled
try:
success = self._task.cancel()
if not success:
rospy.logwarn(
'TaskManager: task refusing to cancel')
if self._force_cancel:
rospy.logwarn(
'TaskManager: forcing task cancel')
self._action_server.set_canceled()
self._task = None
elif success:
self._action_server.set_canceled()
self._task = None
except Exception as e:
rospy.logfatal('TaskManager: Error canceling task')
raise
if self._task is not None:
# can get next command and task state
try:
state, command = self._task.get_desired_command()
except Exception as e:
rospy.logerr(
'TaskManager: Error getting command from task. Aborting task'
)
rospy.logerr(str(e))
self._action_server.set_aborted()
self._task = None
try:
task_state = self._handler.handle(command, state)
except Exception as e:
# this is a fatal error, as the handler should be able to
# handle commands of valid tasks without raising exceptions
rospy.logfatal(
'TaskManager: Error handling task command')
rospy.logfatal(str(e))
raise
if task_state == TaskHandledState.ABORTED:
rospy.logerr('TaskManager: aborting task')
self._action_server.set_aborted()
self._task = None
elif task_state == TaskHandledState.DONE:
rospy.loginfo(
'TaskManager: task has completed cleanly')
self._action_server.set_succeeded()
self._task = None
elif task_state == TaskHandledState.FAILED:
rospy.logerr('TaskManager: Task failed')
self._action_server.set_failed()
self._task = None
elif task_state == TaskHandledState.OKAY:
rospy.loginfo_throttle(1, 'TaskManager: task running')
else:
raise IARCFatalSafetyException(
'TaskManager: invalid task handled state returned from handler: '
+ str(task_state))
self._update_rate.sleep()
def __init__(self):
rospy.init_node('imu_node')
self.pub_euler = rospy.Publisher('sensors/imu/euler',
Vector3Stamped,
queue_size=1)
self.pub_diagnostics = rospy.Publisher('sensors/imu/diagnostics',
DiagnosticStatus,
queue_size=1)
self.pub_imu = rospy.Publisher('sensors/imu/data', Imu, queue_size=1)
self.srv_save_calibration = rospy.Service(
'sensors/imu/save_calibration', SaveImuCalibration,
self.save_calibration)
self.srv_load_calibration = rospy.Service(
'sensors/imu/load_calibration', LoadImuCalibration,
self.load_calibration)
try:
mode_name = rospy.get_param('sensors/bno055/mode')
except KeyError:
rospy.logerr(
'Could not read mode parameter, defaulting to IMU mode.')
mode_name = 'IMU'
if mode_name == 'IMU':
mode = BNO055.OPERATION_MODE_IMUPLUS
elif mode_name == 'NDOF':
mode = BNO055.OPERATION_MODE_NDOF
else:
rospy.logerr('Invalid mode parameter, defaulting to IMU mode.')
mode_name = 'IMU'
mode = BNO055.OPERATION_MODE_IMUPLUS
try:
self.bno = BNO055.BNO055(rst='P9_12')
except RuntimeError:
rospy.logfatal('Unsupported hardware, intiating clean shutdown.')
rospy.signal_shutdown('')
return
if not self.bno.begin(mode):
rospy.logfatal(
'Failed to initialise BNO055! Is the sensor connected?')
raise rospy.ROSInitException(
'Failed to initialise BNO055! Is the sensor connected?')
self.status, self.self_test, self.error = self.bno.get_system_status()
rospy.logdebug("System status: %s", self.status)
rospy.logdebug("Self test result (0x0F is normal): 0x%02X",
self.self_test)
if self.status == 0x01:
rospy.logwarn(
"System status: 0x%02X\n (see datasheet section 4.3.59).",
self.status)
self.sw_v, \
self.bootloader_v, \
self.accelerometer_id, \
self.magnetometer_id, \
self.gyro_id = self.bno.get_revision()
rospy.logdebug(("Software version: %s\n"
"Bootloader version: %s\n"
"Accelerometer ID: 0x%02X\n"
"Magnetometer ID: 0x%02X\n"
"Gyroscope ID: 0x%02X\n"), self.sw_v,
self.bootloader_v, self.accelerometer_id,
self.accelerometer_id, self.gyro_id)
rospy.loginfo('Initialized in %s mode.', mode_name)
self.bno.set_axis_remap(BNO055.AXIS_REMAP_Y, BNO055.AXIS_REMAP_X,
BNO055.AXIS_REMAP_Z,
BNO055.AXIS_REMAP_POSITIVE,
BNO055.AXIS_REMAP_POSITIVE,
BNO055.AXIS_REMAP_POSITIVE)
rospy.logdebug('IMU axis config is {0}'.format(
self.bno.get_axis_remap()))
self.talker()
def initializeReplay(global_frame_id, robot_list):
"""!
@brief Load the TF tree from the provided bag file.
This initialization takes a bag file, as provided by the user on the parameter
server and reads the entire TF tree into a buffer for use in replaying.
@param global_frame_id The global refernce frame used by Gazebo, often 'map'
@param robot_list The list of robots in the simulation. Used to determine the start
and end times that have valid TF transforms for all robots.
@return A tuple containing:
1. The tf buffer
2. A rospy.Time of the start time of the bag file
3. A rospy.Time of the end time of the bag file
4. A rospy.Duration of the period used during processing
@throw rospy.ROSInitException Thrown if the bag file can't be opened or found.
"""
rospy.loginfo('Loading bag file...')
# Determine where the bag file is located via parameter server.
bag_file = ParameterLookup.lookup(parameter='~bag_file')
if 'example_replay.bag' in bag_file:
rospy.logwarn(
msg='You might be using the example bag file. Did you mean to provide your own using the bag_file parameter?')
# Open the bag file. This might throw an error, so catch it and send a warning
# to the user.
try:
bag = rosbag.Bag(f=bag_file)
except Exception as ex:
error_string = 'Unable to open {bag}: {error}'.format(
bag=bag_file, error=ex)
rospy.logfatal(msg=error_string)
raise rospy.ROSInitException(error_string)
# If this point is reached, the bag file is loaded and ready.
# Now look up the simulated rate with the same method as the bag parameter.
rate = ParameterLookup.lookupWithDefault(
parameter='~simulated_rate', default=30.0)
period = rospy.Duration(secs=(1.0 / rate))
# Now, parse the bag file into a tf buffer
start_time = rospy.Time(bag.get_start_time())
end_time = rospy.Time(bag.get_end_time())
tf_buffer = tf2_py.BufferCore((end_time - start_time))
for topic, msg, _ in bag.read_messages(topics=['/tf', '/tf_static']):
for msg_tf in msg.transforms:
if topic == '/tf_static':
tf_buffer.set_transform_static(msg_tf, 'default_authority')
else:
tf_buffer.set_transform(msg_tf, 'default_authority')
# Now that everything is loaded, we don't need the bag file.
bag.close()
# Find the first start_time that has a valid transform for every single robot.
good_start_time = False
# This only finishes if every robot has a viable transform at this time step
while not good_start_time and start_time < end_time:
good_start_time = True
for robot in robot_list:
(is_good, _) = tf_buffer.can_transform_core(
global_frame_id, robot.getFullFrame(), start_time)
good_start_time = good_start_time and is_good
if not good_start_time:
start_time += period
# If the start time is now greater than the end time, then there were no
# time frames that had valid transforms for every robot. This is a problem.
if not start_time < end_time:
error_string = 'Unable to find valid transforms for all robots in bag file.'
rospy.logfatal(error_string)
raise rospy.exceptions.ROSInitException(error_string)
# Now do the same thing for the end time, but decrement instead. The end time
# is only a gate on when the whole simulation should end, so it does not have to
# be a whole number of increments from start time. Therefore, just start at the
# end of the bag file.
good_end_time = False
# You technically need to watch for the end > start since there may be a specific
# setup that only has frame interpolation in a very narrow time range and if period
# is larger than that range, end_time is liable to never exist within that range.
while not good_end_time and end_time > start_time:
good_end_time = True
for robot in robot_list:
(is_good, _) = tf_buffer.can_transform_core(
global_frame_id, robot.getFullFrame(), end_time)
good_end_time = good_end_time and is_good
if not good_end_time:
end_time -= period
if not end_time > start_time:
error_string = 'Unable to find valid transforms for all robots in bag file.'
rospy.logfatal(error_string)
raise rospy.exceptions.ROSInitException(error_string)
return (tf_buffer, start_time, end_time, period)
# Try to init ROS node or fail out immediately to avoid blocking the UI.
from socket import error as socket_error
import rosgraph
import rospy
try:
rosgraph.Master('/rostopic').getPid()
except socket_error:
raise rospy.ROSInitException(
'Cannot load QGIS ROS. ROS Master was not found.')
else:
rospy.init_node('qgis_ros_toolbox')
from functools import partial
from pathlib import Path
import os
from PyQt5 import uic, QtCore, QtWidgets
from qgis.core import QgsProject
import rospy
from ..core import TranslatorRegistry
FORM_CLASS, _ = uic.loadUiType(
str(Path(os.path.dirname(__file__)) / 'data_loader_widget.ui'))
class DataLoaderWidget(QtWidgets.QWidget, FORM_CLASS):
def __init__(self, parent=None):
'''Occurs on init, even if dialog is not shown.'''
super(DataLoaderWidget, self).__init__(parent)
state = self.opcua.read_data(self.opcua_ns + '/StateTR' + str(i+1))
rospy.logdebug( 'New conveyor state: ' + str(state) )
if state in range(0, 9):
self.state[i] = state
else:
rospy.logwarn('Unexpected state: ' + str(state) + ' of rotary conveyor ' + str(i+1))
self.state[i] = -1
rospy.sleep(1)
if __name__ == '__main__':
rospy.init_node('conveyor')
opcua_endpoint = rospy.get_param('~opcua_endpoint')
if not rospy.has_param('~opcua_server_namespace'):
raise rospy.ROSInitException(
'Parameter "opcua_server_namespace" must be specified in accordance with OPCU-UA'
'Model. Example: "ns=3;/Airalab/Conveyor"')
opcua_server_namespace = rospy.get_param('~opcua_server_namespace')
if not rospy.has_param('opcua_client_node'):
rospy.logwarn('Using default ROS OPC-UA Client node path: /opcua/opcua')
rospy.logwarn('You can specify it in parameter \'opcua_node\'')
opcua_client_node = rospy.get_param('opcua_client_node', '/opcua/opcua_client')
tr_load_time = rospy.get_param('tr_load_time', 2000)
tr_unload_time = rospy.get_param('tr_unload_time', 2000)
tr_timeout = rospy.get_param('tr_timeout', 5000)
tl_unload_time = rospy.get_param('tl_unload_time', 2000)
Conveyor(opcua_client_node, opcua_endpoint, opcua_server_namespace, tr_load_time, tr_unload_time, tr_timeout, tl_unload_time).spin()
