def __init__(self): self.brake_change_threshold = rospy.get_param('~brake_change_threshold', 0) self.steer_change_threshold = rospy.get_param('~steer_change_threshold', 0) self.arduino_cmd_msg = Arduino() self.arduino_pub = rospy.Publisher('arduino_cmd', Arduino) rate = rospy.get_param('~rate', 10) rospy.loginfo('Sending messages to the Arduino board every %dms' % int(1000.0/rate)) # monitor the last time we received a button_state_emergency message self.last_watchdog = None self.arduino_sub = rospy.Subscriber('button_state_emergency', Bool, self.arduinoCB) self.throttle_sub = setupSub('throttle', Float64, self.throttleCB) self.brake_sub = setupSub('brake_angle', Float64, self.brakeCB) self.steer_sub = setupSub('steer_angle', Float64, self.steerCB) self.diag_updater = DIAG.Updater() self.diag_updater.setHardwareID('none') # a frequency diagnostic: make sure we are sending the arduino_cmd message at the # appropriate frequency fs_param = DIAG.FrequencyStatusParam({'min': rate, 'max': rate}, 0.1, 5) self.fs_diag = DIAG.FrequencyStatus(fs_param) self.diag_updater.add(self.fs_diag) # watchdog diagnostic: reports an error when the board does not respond self.diag_updater.add( DIAG.FunctionDiagnosticTask('board watchdog', self.watchdog_diag) ) # emergency monitor self.emergency_state = False self.diag_updater.add( DIAG.FunctionDiagnosticTask('emergency button', self.emergency_diag) ) self.timer = rospy.Timer(rospy.Duration(1.0/rate), self.timer_cb)
def __init__(self, node_name): """ Parameters: :str node_name: ROS node name :RoboclawControl rbc_ctl: RoboclawControl obj. controlling hardware :int loop_rate: Integer rate in Hz of the main loop """ self._node_name = node_name self._rbc_ctls = [] # Populated by the connect() method # Records the values of the last speed command self._p_cmd_time = rospy.get_rostime() self._p_cmd_m1_qpps = 0 self._p_cmd_m2_qpps = 0 self._p_cmd_accel = 0 self._p_cmd_max_secs = 0 # Serialize access to cmd variables self._speed_cmd_lock = threading.RLock() # Setup encoder parameters self._ticks_pr_meter = 4342.2 self._base_width = 0.315 self._p_enc_left = 0 self._p_enc_right = 0 self._p_enc_time = rospy.get_rostime() # Setup position parameters self._x = 0 self._y = 0 self._th = 0.0 # Set up the Publishers self._stats_pub = rospy.Publisher(rospy.get_param( "~stats_topic", DEFAULT_STATS_TOPIC), Stats, queue_size=1) self.tf_pub = tf2_ros.TransformBroadcaster() # Set up odometry publisher self._odom_pub = rospy.Publisher("odom", Odometry, queue_size=50) # Set up odometry-baselink broadcaster self._odom_broadcaster = tf2_ros.TransformBroadcaster() self._wheels_broadcaster = tf2_ros.TransformBroadcaster() # Set up the Diagnostic Updater self._diag_updater = diagnostic_updater.Updater() self._diag_updater.setHardwareID(node_name) self._diag_updater.add("Read Diagnostics", self._publish_diagnostics) # Set up the SpeedCmd Subscriber rospy.Subscriber( rospy.get_param("~speed_cmd_topic", DEFAULT_SPEED_CMD_TOPIC), SpeedCmd, self._speed_cmd_callback) # For logdebug self.p_m1_val = 0 self.p_m2_val = 0
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.send_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s" % message_class_str) self.lock = Lock() self.launched_time = rospy.Time.now() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("LowspeedStreamer", self.diagnosticCallback) self.send_num = 0 self.last_send_time = rospy.Time(0) self.last_input_received_time = rospy.Time(0) self.last_send_time_pub = rospy.Publisher("~last_send_time", Time) self.last_input_received_time_pub = rospy.Publisher( "~last_input_received_time", Time) self.to_port = rospy.get_param("~to_port", 1024) self.to_ip = rospy.get_param("~to_ip", "127.0.0.1") self.send_rate = rospy.get_param("~send_rate", 1) self.event_driven = rospy.get_param("~event_driven", False) self.latest_message = None self.socket_client = socket(AF_INET, SOCK_DGRAM) self.send_format = msgToStructFormat(self.send_message()) self.sub = rospy.Subscriber("~input", self.send_message, self.messageCallback) if not self.event_driven: self.send_timer = rospy.Timer(rospy.Duration(1 / self.send_rate), self.sendTimerCallback) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def __init__(self): rospy.init_node('safety_pose') self.last_msg = [] self.localization_pub = rospy.Publisher('localization_active', Bool, queue_size=1, latch=True) self.pub = rospy.Publisher('out', PoseWithCovarianceStamped, queue_size=1) self.active = rospy.get_param('~active', True) self.timeout = rospy.get_param('~timeout', 1.0) self.active_index = 0 self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("pose sources") self.topics = [] for i, data in enumerate(rospy.get_param('~in', [])): topic = data['topic'] covariance = data.get('cov', None) _type = topic_type.get(data.get('type', None), None) if _type is None: if covariance is None: _type = PoseWithCovarianceStamped else: _type = PoseStamped self.last_msg.append(rospy.Time.now()) self.topics.append(topic) rospy.Subscriber(topic, _type, self.got_pose_from(_type, i, cov=covariance)) self.updater.add(topic, self.diagnostics(i)) self.updater.add('active source', self.active_diagnostics) rospy.Timer(rospy.Duration(1), self.update_diagnostics) rospy.Timer(rospy.Duration(self.timeout), self.update_localization) self.srv = Server(SafeOdomConfig, self.reconfigure) rospy.spin()
def __init__(self, topics, params): """ Constructor @param topics: dictionary mapping topic names to publisher handles valid topic names are {"cloud", "scan0", "scan1", "scan2", "scan3"} @type topics: dict {topic_name:publisher_handle} @param params: dictionary mapping parameter names to values. Where applicable, the parameters must be in device units (e.g. ticks) @type params: dict {ros_parameter_string:value} """ self.params = params self.topics = topics # LaserScan messages (numbered 0-3, 0 is the lowest scan plane) # rewire for numpy serialization on publish self.scans = [numpy_msg(LaserScan)(), numpy_msg(LaserScan)(), numpy_msg(LaserScan)(), numpy_msg(LaserScan)()] self._init_scans() # PointCloud2 message # rewire for numpy serialization on publish self.point_cloud = numpy_msg(PointCloud2)() self._init_point_cloud() # put variables into the namespace to prevent # attribute exceptions when the class is abused self.header = None self.x = None self.y = None self.z = None self.echo = None self.layer = None self.flags = None self.echo_w = None self.h_angle = None self.rads_per_tick = None self.pc_data = None self.last_start_time = None self.rads_per_tick = None self.seq_num = -1 self.time_delta = rospy.Duration() self.n_points = 0 # timestamp smoothing self.smoothtime = None self.smoothtime_prev = None self.recv_time_prev = None self.known_delta_t = rospy.Duration(256.0/self.params['scan_frequency']) self.time_smoothing_factor = self.params['time_smoothing_factor'] self.time_error_threshold = self.params['time_error_threshold'] self.total_err = 0.0 #integrate errors self.header = {}.fromkeys(self.header_keys) # For diagnostics: self.diag_updater = diagnostic_updater.Updater() self.diag_updater.setHardwareID('none') # scan_frequency is in ticks and the rate is 256 ticks per second self.freq_bound = {'min': (self.params['scan_frequency']/256.0), 'max': (self.params['scan_frequency']/256.0)} fs_params = diagnostic_updater.FrequencyStatusParam(self.freq_bound, 0.1, 5) self.fs_diag = diagnostic_updater.HeaderlessTopicDiagnostic('ldmrs', self.diag_updater, fs_params)
def rosSetup(self): """ Creates and setups ROS components """ self.cinfo = camera_info_manager.CameraInfoManager( cname=self.camera_model, url=self.camera_info_url, namespace=rospy.get_name()) self.cinfo.loadCameraInfo() # Mirar de cambiar por ImageTransport self.compressed_image_publisher = rospy.Publisher( "%scompressed" % rospy.get_namespace(), CompressedImage, self, queue_size=10) self.caminfo_publisher = rospy.Publisher("%scamera_info" % rospy.get_namespace(), CameraInfo, self, queue_size=10) # Sets the url self.url = 'http://%s/axis-cgi/mjpg/video.cgi?streamprofile=%s&camera=%d&fps=%d&compression=%d' % ( self.hostname, self.profile, self.camera_number, self.fps, self.compression) rospy.loginfo('Axis:rosSetup: Camera %s (%s:%d): url = %s, PTZ %s' % (self.camera_model, self.hostname, self.camera_number, self.url, self.ptz)) self.encodedstring = base64.encodestring(self.username + ":" + str(self.password))[:-1] self.auth = "Basic %s" % self.encodedstring # Diagnostic Updater self.diagnostics_updater = diagnostic_updater.Updater() self.diagnostics_updater.setHardwareID( "%s-%s:%s" % (self.camera_model, self.camera_id, self.hostname)) self.diagnostics_updater.add("Video stream updater", self.getStreamDiagnostic) #self.diagnostics_updater.add("Video stream frequency", self.getStreamFrequencyDiagnostic) if self.fps == 0: freq_bounds = {'min': 0.5, 'max': 100} else: freq_bounds = {'min': self.fps, 'max': self.fps} self.image_pub_freq = diagnostic_updater.HeaderlessTopicDiagnostic( "%scompressed" % rospy.get_namespace(), self.diagnostics_updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.01, 1)) if self.ptz: # sets the ptz interface self.ptz_interface.rosSetup() self.diagnostics_updater.add("Ptz state updater", self.ptz_interface.getStateDiagnostic) # Creates a periodic callback to publish the diagnostics at desired freq self.diagnostics_timer = rospy.Timer(rospy.Duration(1.0), self.publishDiagnostics)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("HighspeedReceiver", self.diagnosticCallback) self.latch = rospy.get_param("~latch", True) self.pesimistic = rospy.get_param("~pesimistic", False) self.fragment_packets_torelance = rospy.get_param("~fragment_packets_torelance", 20) self.timestamp_overwrite_topics = rospy.get_param("~timestamp_overwrite_topics", []) self.publish_only_if_updated_topics = rospy.get_param("~publish_only_if_updated_topics", []) self.prev_seq_ids = {} self.receive_port = rospy.get_param("~receive_port", 16484) self.receive_ip = rospy.get_param("~receive_ip", "localhost") self.topic_prefix = rospy.get_param("~topic_prefix", "/from_fc") if not self.topic_prefix.startswith("/"): self.topic_prefix = "/" + self.topic_prefix if self.topic_prefix == "/": self.topic_prefix = "" self.publishers = publishersFromMessage(self.message_class, self.topic_prefix, latch=self.latch) self.packet_size = rospy.get_param("~packet_size", 1400) #2Hz self.launched_time = rospy.Time.now() self.last_received_time = rospy.Time(0) self.last_received_time_pub = rospy.Publisher("~last_received_time", Time) self.last_published_seq_id = -1 self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback) self.packets_queue = Queue()
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("HighspeedReceiver", self.diagnosticCallback) self.pesimistic = rospy.get_param("~pesimistic", False) self.receive_port = rospy.get_param("~receive_port", 16484) self.receive_ip = rospy.get_param("~receive_ip", "localhost") self.topic_prefix = rospy.get_param("~topic_prefix", "/from_fc") if not self.topic_prefix.startswith("/"): self.topic_prefix = "/" + self.topic_prefix if self.topic_prefix == "/": self.topic_prefix = "" self.publishers = publishersFromMessage(self.message_class, self.topic_prefix) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.bind((self.receive_ip, self.receive_port)) self.packet_size = rospy.get_param("~packet_size", 1000) #2Hz self.packets = [] self.launched_time = rospy.Time.now() self.last_received_time = rospy.Time(0) self.last_received_time_pub = rospy.Publisher("~last_received_time", Time) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def rosSetup(self): """ Creates and setups ROS components """ # Diagnostic Updater self.diagnostics_updater = diagnostic_updater.Updater() self.diagnostics_updater.setHardwareID("%s-%s:%s" % (self.camera_model, self.camera_id, self.hostname) ) self.diagnostics_updater.add("Video stream updater", self.getStreamDiagnostic) #self.diagnostics_updater.add("Video stream frequency", self.getStreamFrequencyDiagnostic) if self.fps == 0: freq_bounds = {'min':0.5, 'max':100} else: freq_bounds = {'min':self.fps, 'max':self.fps} self.image_pub_freq = diagnostic_updater.HeaderlessTopicDiagnostic("%scompressed"%rospy.get_namespace(), self.diagnostics_updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.01, 1)) if self.ptz: # sets the ptz interface self.ptz_interface.rosSetup() self.diagnostics_updater.add("Ptz state updater", self.ptz_interface.getStateDiagnostic) # Creates a periodic callback to publish the diagnostics at desired freq self.diagnostics_timer = rospy.Timer(rospy.Duration(1.0), self.publishDiagnostics)
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCS") try: self.receive_message = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s" % message_class_str) self.lock = Lock() self.launched_time = rospy.Time.now() self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") self.diagnostic_updater.add("LowspeedReceiver", self.diagnosticCallback) self.received_num = 0 self.receive_port = rospy.get_param("~receive_port", 1024) self.receive_ip = rospy.get_param("~receive_ip", "127.0.0.1") self.receive_buffer = rospy.get_param("~receive_buffer_size", 250) self.socket_server = socket(AF_INET, SOCK_DGRAM) self.socket_server.bind((self.receive_ip, self.receive_port)) self.receive_format = msgToStructFormat(self.receive_message()) self.pub = rospy.Publisher("~output", self.receive_message) self.last_received_time = rospy.Time(0) self.last_received_time_pub = rospy.Publisher("~last_received_time", Time) self.last_publish_output_time = rospy.Time(0) self.last_publish_output_time_pub = rospy.Publisher( "~last_publish_output_time", Time) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def __init__(self): self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("bebop diagnostic") clss = [AutopilotState, AutopilotAlert, SensorState, BatteryState, WifiState, OverheatingState, MagnetormeterState] self.bebop_states = [cls(self.updater) for cls in clss] rospy.Timer(rospy.Duration(1), self.update_diagnostics)
def __init__(self): """ Init Communication and define ROS publishers """ # define ros publishers self.rcu_status_pub = rospy.Publisher("~rcu_status", ChargerState, queue_size=10) self.rcu_battery_pub = rospy.Publisher("~battery_state", BatteryState, queue_size=10) self.diag_updater = diagnostic_updater.Updater() # init connection self.comm_interface = rospy.get_param("~communication_interface", "modbus") self.init_connection(self.comm_interface) # RCU Status update self.status_update() # RCU diagnostic_updater self.diag_updater.add("Device Temperature", self.check_device_temperature) self.diag_updater.add("Coil Temperature", self.check_coil_temperature) self.diag_updater.setHardwareID(rospy.get_name()) self.enable_charging_srv = rospy.Service('~enable_charging', Trigger, self.enable_charging) self.disable_charging_srv = rospy.Service('~disable_charging', Trigger, self.disable_charging)
def __init__(self): rospy.init_node("gps_node") ## publish self.pub_nav_fix = rospy.Publisher('gps/fix', NavSatFix, queue_size=1) self.pub_nav_stat = rospy.Publisher('gps/stat', NavSatStatus, queue_size=1) ## diagnostics updater self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("gps") gdt = GpsDiagnosticTask() self.updater.add(gdt) freq_bounds = {'min':18., 'max':22.} # If you update these values, the self.nav_fix_freq = diagnostic_updater.TopicDiagnostic("/gps/fix", self.updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.02, 10), diagnostic_updater.TimeStampStatusParam(min_acceptable = -1, max_acceptable = 1)) ## threading self.lock = threading.Lock() ## random random.seed()
def __init__(self): message_class_str = rospy.get_param("~message", "jsk_network_tools/FC2OCSLargeData") try: self.message_class = get_message_class(message_class_str) except: raise Exception("invalid topic type: %s"%message_class_str) self.lock = Lock() self.launched_time = rospy.Time.now() self.packet_interval = None self.diagnostic_updater = diagnostic_updater.Updater() self.diagnostic_updater.setHardwareID("none") # register function to publish diagnostic self.diagnostic_updater.add("HighspeedStreamer", self.diagnosticCallback) self.send_port = rospy.get_param("~to_port", 16484) self.send_ip = rospy.get_param("~to_ip", "localhost") self.last_send_time = rospy.Time(0) self.last_input_received_time = rospy.Time(0) self.last_send_time_pub = rospy.Publisher("~last_send_time", Time) self.last_input_received_time_pub = rospy.Publisher( "~last_input_received_time", Time) self.send_num = 0 #self.packet_interval = rospy.get_param("~packet_interval", 0.001) self.bandwidth = rospy.get_param("~bandwidth", 280 * 1000 * 1000) self.rate = rospy.get_param("~send_rate", 2) #2Hz self.socket_client = socket(AF_INET, SOCK_DGRAM) self.packet_size = rospy.get_param("~packet_size", 1000) #2Hz subscriber_info = subscribersFromMessage(self.message_class()) self.messages = {} self.subscribe(subscriber_info) self.counter = 0 self.send_timer = rospy.Timer(rospy.Duration(1.0 / self.rate), self.sendTimerCallback) self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10), self.diagnosticTimerCallback)
def main(): rospy.init_node('Diagnostic_updater') updater = diagnostic_updater.Updater() updater.setHardwareID('MARCH IVc') # Frequency checks CheckInputDevice('/march/input_device/alive', Alive, updater, 5) # control checks check_current_movement_values = CheckJointValues('march/joint_states', JointState) updater.add('Control position values', check_current_movement_values.position_diagnostics) updater.add('Control velocity values', check_current_movement_values.velocity_diagnostics) updater.add('Control effort values', check_current_movement_values.effort_diagnostics) # IMC state check CheckImcStatus(updater) # Gait information CheckGaitStatus(updater) updater.force_update() while not rospy.is_shutdown(): rospy.sleep(0.1) updater.update()
def __init__(self, loop_time, serial_port_comm): threading.Thread.__init__(self) self.loop_time = loop_time self.driver_shutdown = False self.current_flags_sync = threading.Lock() self.current_flags_dict = None self.current_flags_updated = False serial_port_comm.add_flags_observer(self) self.joint_state_msg = JointState() self.joint_state_msg.name = [] self.joint_state_msg.name.append("gripper_claws") self.joint_states_publisher = rospy.Publisher('joint_states', JointState, queue_size=10) self.updater = diagnostic_updater.Updater() self.updater.setHardwareID( "Weiss Robotics Gripper IEG 76-030 V1.02 SN 000106") self.updater.add("Position and flags updater", self.produce_diagnostics) freq_bounds = {'min': 0.5, 'max': 2} # It publishes the messages and simultaneously makes diagnostics for the topic "joint_states" using a FrequencyStatus and TimeStampStatus self.pub_freq_time_diag = diagnostic_updater.DiagnosedPublisher( self.joint_states_publisher, self.updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10), diagnostic_updater.TimeStampStatusParam())
def __init__(self): super().__init__(NODE_NAME) self.updater = diagnostic_updater.Updater(node=self, period=PERIOD) self.updater.setHardwareID(HARDWARE_ID) joint_names = get_joint_names_from_service(self) # Frequency checks CheckInputDevice(self, "/march/input_device/alive", Alive, self.updater, 4) # Control checks check_joint_states = CheckJointValues(self, "/march/joint_states", JointState) self.updater.add("Control position values", check_joint_states.position_diagnostics) self.updater.add("Control velocity values", check_joint_states.velocity_diagnostics) self.updater.add("Control effort values", check_joint_states.effort_diagnostics) # MotorController state check CheckMotorControllerStatus(self, self.updater, joint_names) # Gait information CheckGaitStatus(self, self.updater) # PDB checks CheckPDBStatus(self, self.updater)
def __init__(self): """ Init Communication and define ROS publishers """ # define ros publishers self.rcu_status_pub = rospy.Publisher("~rcu_status", ChargerState, queue_size=10) # self.rcu_battery_pub = rospy.Publisher("~battery_state", BatteryState, queue_size=10) # init connection self.comm_interface = rospy.get_param("~communication_interface", "modbus") self.init_connection(self.comm_interface) # RCU diagnostic_updater self.diag_updater = diagnostic_updater.Updater() self.diag_updater.setHardwareID(rospy.get_name()) if 'modbus' == self.comm_interface or 'dummy' == self.comm_interface: self.diag_updater.add("Device Temperature", self.check_device_temperature) self.diag_updater.add("Coil Temperature", self.check_coil_temperature) else: rospy.logwarn(f'coil temerature and device temperature are not available for communication interface {self.comm_interface}') # RCU Status update self.status_update() self.start_charging_srv = rospy.Service('~start_charging', Trigger, self.start_charging) self.stop_charging_srv = rospy.Service('~stop_charging', Trigger, self.stop_charging)
def __init__(self): rospy.init_node('base_controller') # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) self.port = rospy.get_param("~port", "/dev/ttyACM0") self.baud = int(rospy.get_param("~baud", 57600)) self.timeout = rospy.get_param("~timeout", 0.5) self.base_frame = rospy.get_param("~base_frame", 'base_link') # Overall loop rate: should be faster than fastest sensor rate self.rate = int(rospy.get_param("~base_controller_rate", 20)) r = rospy.Rate(self.rate) self.use_base_controller = rospy.get_param("~use_base_controller", False) self.diag_updater = diagnostic_updater.Updater() self.diag_updater.setHardwareID(self.port) self.diag_updater.add("Serial Port Status", self.update_diagnostics) freq_bounds = {'min': self.rate, 'max': self.rate} self.odom_pub_freq_updater = diagnostic_updater.HeaderlessTopicDiagnostic( "Odometry", self.diag_updater, diagnostic_updater.FrequencyStatusParam(freq_bounds)) # Initialize a Twist message self.cmd_vel = Twist() # A cmd_vel publisher so we can stop the robot when shutting down self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Initialize the controlller self.controller = Arduino(self.port, self.baud, self.timeout) # Make the connection self.controller.connect() rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") # Reserve a thread lock mutex = thread.allocate_lock() # Initialize the base controller if used if self.use_base_controller: self.myBaseController = BaseController(self.controller, self.base_frame, self.rate) # Start polling the sensors and base controller while not rospy.is_shutdown(): self.diag_updater.update() if self.use_base_controller: mutex.acquire() self.myBaseController.poll() self.odom_pub_freq_updater.tick() mutex.release() r.sleep()
def __init__(self): self.last_bestpos = None self.last_inspvax = None rospy.Subscriber("novatel_data/bestpos", BESTPOS, self.bestpos_callback) rospy.Subscriber("novatel_data/inspvax", INSPVAX, self.inspvax_callback) self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("none") self.updater.add("Novatel SPAN", self.produce_diagnostics)
def __init__(self, hostname): self.hostname = hostname self.ignore_fans = rospy.get_param('~ignore_fans', False) rospy.loginfo("Ignore fanspeed warnings: %s" % self.ignore_fans) self.updater = DIAG.Updater() self.updater.setHardwareID("none") self.updater.add('%s Sensor Status' % self.hostname, self.monitor) self.timer = rospy.Timer(rospy.Duration(1), self.timer_cb)
def init_diagnostics(self): updater = diagnostic_updater.Updater() _id = rospy.get_param('~id', '?') updater.setHardwareID('Pozyx %s' % _id) freq_bounds = {'min': self.rate * 0.8, 'max': self.rate * 1.2} freq_param = diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10) stamp_param = diagnostic_updater.TimeStampStatusParam() self.pose_pub_stat = diagnostic_updater.DiagnosedPublisher( self.pose_pub, updater, freq_param, stamp_param) if self.enable_raw_sensors: updater.add("Sensor calibration", self.update_sensor_diagnostic) updater.add("Localization", self.update_localization_diagnostic) updater.add("Resets", self.update_reset_diagnostic) rospy.Timer(rospy.Duration(1), lambda evt: updater.update())
def __init__(self): # Point cloud details self._last_pc_header = None self._pc_lock = Lock() self._pc_sub = rospy.Subscriber(PointCloudDiagnostic.POINT_CLOUD_TOPIC, PointCloud2, self._on_point_cloud) # Create the diagnostic updater self._updater = diagnostic_updater.Updater() self._updater.setHardwareID("none") self._updater.add("delay", self._create_diagnostic) # Set the loop rate of this updater (Hz) self._loop_rate = 20
def __init__(self, timer_duration): self.diagnostic_updater = diagnostic_updater.Updater() self.timer_kwargs = dict( period=timer_duration, callback=self.timer_callback, oneshot=False, ) if (LooseVersion(pkg_resources.get_distribution('rospy').version) >= LooseVersion('1.12.0')): # on >=kinetic, it raises ROSTimeMovedBackwardsException # when we use rosbag play --loop. self.timer_kwargs['reset'] = True self.timer = None
def listener(self): pub = rospy.Publisher('emlid_gps', NavSatFix, queue_size=100) rate = rospy.Rate(50) updater = diagnostic_updater.Updater() updater.setHardwareID('Device : Reach') updater.add('Reach status', self.reach_diagnostics.run_diagnostics) freq_bounds = {'min': 5, 'max': 10} pub_freq = diagnostic_updater.HeaderlessTopicDiagnostic( 'emlid_gps', updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10)) # try to establish connection with the TCP server of emlid if self.use_serial: self.init_serial() else: self.init_sockets() # if connection is successful begin collecting data and parsing them while not rospy.is_shutdown(): # try getting data on the channel try: data = self.readline() if data: self.last_data_tstamp = time.time() self.handle_gps_msg(data) else: # if data is not there wait for some time then timeout self.timeout_counter = time.time() - self.last_data_tstamp if self.timeout_counter > self.timeout_threshold: rospy.loginfo('Connection timed out. Closing') self.shutdown() sys.exit() except socket.error as (code, msg): self.shutdown() rospy.loginfo('Exiting with MSG: %s' % (msg)) sys.exit() if self.flag_new_data is True: self.flag_new_data = False pub.publish(self.gps_data) pub_freq.tick() updater.update() rate.sleep()
def __init__(self): self.host_ = rospy.get_param('~reach_rs_host_or_ip', 'reach.local') self.port_ = rospy.get_param('~reach_rs_port') self.frameId_ = rospy.get_param('~reach_rs_frame_id', 'reach_rs') self.fixTimeout_ = rospy.get_param('~fix_timeout', 0.5) self.socket_ = socket.socket() self.driver_ = enway_reach_rs_driver.driver.RosNMEADriver() self.diagnostics_ = diagnostic_updater.Updater() self.diagnostics_.setHardwareID('Emlid Reach RS') self.diagnostics_.add('Receiver Status', self.addDiagnotics) self.connectionStatus_ = 'Not connected' self.lastFix_ = None
def __init__(self): duration = rospy.get_param('~duration', 60) # Get sanity target topic and node names topics = rospy.get_param('~topics', []) nodes = rospy.get_param('~nodes', []) # Set diagnostic updater for each topic and node self.updater = diagnostic_updater.Updater() self.updater.setHardwareID("none") for topic_name in topics: self.updater.add( topic_name, lambda stat, tn=topic_name: self.check_topic(stat, tn)) for node_name in nodes: self.updater.add( node_name, lambda stat, nn=node_name: self.check_node(stat, nn)) # Timer to call updater self.timer = rospy.Timer(rospy.Duration(duration), self.check_sanity)
def __init__(self): host = rospy.get_param('~reach_rs_host_or_ip', 'reach.local') port = rospy.get_param('~reach_rs_port') self.address = (host, port) self.socket = None self.frame_id = rospy.get_param('~reach_rs_frame_id', 'reach_rs') self.fix_timeout = rospy.get_param('~fix_timeout', 0.5) self.driver = enway_reach_rs_driver.driver.RosNMEADriver() self.diagnostics = diagnostic_updater.Updater() self.diagnostics.setHardwareID('Emlid Reach RS') self.diagnostics.add('Receiver Status', self.add_diagnotics) self.connected = False self.connection_status = 'not connected' self.last_fix = None
def __init__( self, node_name='pipeline_node', param_prefix='', gst_plugin_paths=[], # list of strings gst_plugin_ros_packages=[], # list of tuples (package_name,sub_dir) gst_plugins_required=[] # list of strings ): super().__init__(node_name) self.registry = Gst.Registry() self.registry.connect('plugin-added', self.plugin_added) self.node_name = node_name # pull additional info from parameter server (paths, packages, plugins) = self.fetch_params(param_prefix) self.gst_plugin_paths = gst_plugin_paths + paths self.gst_plugin_ros_packages = gst_plugin_ros_packages + packages self.gst_plugins_required = gst_plugins_required + plugins # unpack ros packages into paths for (pack, subdir) in self.gst_plugin_ros_packages: plugin_path = os.path.join(get_package_prefix(pack), subdir) self.gst_plugin_paths.append(plugin_path) self.get_logger().debug('additional gst plugin paths are ' + str(paths)) self.get_logger().debug('required gst plugins are ' + str(plugins)) self.add_plugin_paths(self.gst_plugin_paths) self.check_plugins(self.gst_plugins_required) self.updater = diagnostic_updater.Updater(self) self.updater.setHardwareID("Gst1.0") self.updater.add("Pipe Status", self.diagnostic_task) self.pipe = Gst.Pipeline.new("rospipe") self.bus = self.pipe.get_bus() self.bus.add_signal_watch() self.bus.connect('message::error', self.on_error) self.bus.connect('message::state-changed', self.on_status_changed) self.bus.connect('message::eos', self.on_eos) self.bus.connect('message::info', self.on_info) self.updater.force_update() self.diagnostic_timer = self.create_timer(0.5, self.updater.update)
def __init__(self): self.period = rospy.get_param('~period', 0.02) self.minPubPeriod = rospy.get_param('~min_pub_period', None) if self.minPubPeriod < self.period * 2: rospy.loginfo( "Warning: you attempted to set the min_pub_period" + " to a smaller value (%.3f)" % (self.minPubPeriod) + " than twice the period (2 * %.3f)." % (self.period) + " This is not allowed, hence I am setting the min_pub_period" + " to %.3f." % (self.period * 2)) self.minPubPeriod = self.period * 2 # encoders are identified by their index (i.e. on which port they are # plugged on the phidget) self.left = 0 self.right = 1 self.countBufs = {self.left: CountBuffer(), self.right: CountBuffer()} self.lastPub = None self._mutex = threading.RLock() self.initPhidget() self.encoder.setEnabled(self.left, True) self.encoder.setEnabled(self.right, True) self.pub = rospy.Publisher('encoder_counts', EncoderCounts) # diagnostics self.diag_updater = DIAG.Updater() self.diag_updater.setHardwareID('none') f = {'min': 1.0 / self.minPubPeriod} fs_params = DIAG.FrequencyStatusParam(f, 0.25, 1) self.pub_diag = DIAG.HeaderlessTopicDiagnostic('encoder_counts', self.diag_updater, fs_params) #TODO: add a diagnostic task to monitor the connection with the phidget self.forcePub(None)