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