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 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): 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, 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 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, 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 __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_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 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.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)
def setup_pubsub(self): freq_params = diagnostic_updater.FrequencyStatusParam( { 'min': self.diag_update_freq, 'max': self.diag_update_freq }, self.diag_freq_tolerance, self.diag_window_size) time_params = diagnostic_updater.TimeStampStatusParam( self.diag_min_delay, self.diag_max_delay) self.pub_fix = rospy.Publisher("~fix", NavSatFix, queue_size=1000) self.pub_spp_fix = diagnostic_updater.DiagnosedPublisher( rospy.Publisher("~spp_fix", NavSatFix, queue_size=1000), self.diag_updater, freq_params, time_params) self.pub_rtk_fix = diagnostic_updater.DiagnosedPublisher( rospy.Publisher("~rtk_fix", NavSatFix, queue_size=1000), self.diag_updater, freq_params, time_params) # self.pub_rtk = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~rtk_odom", Odometry, queue_size=1000), self.diag_updater, freq_params, time_params) self.pub_odom = diagnostic_updater.DiagnosedPublisher( rospy.Publisher("~odom", Odometry, queue_size=1000), self.diag_updater, freq_params, time_params) self.pub_time = diagnostic_updater.DiagnosedPublisher( rospy.Publisher("~time", TimeReference, queue_size=1000), self.diag_updater, freq_params, time_params) if self.publish_utm_rtk_tf or self.publish_rtk_child_tf: self.tf_br = tf2_ros.TransformBroadcaster() if self.publish_ephemeris: self.pub_eph = rospy.Publisher("~ephemeris", Ephemeris, queue_size=1000) if self.publish_observations: self.pub_obs = rospy.Publisher('~observations', Observations, queue_size=1000)
def __init__(self, component, name, error_threshold=10, rate=1.0, create_watchdog=False): self.component = component self.name = name # Determines the OK, WARN and ERROR status flags self.error_threshold = error_threshold # Create a diagnostics updater self.diag_updater = diagnostic_updater.Updater() # Set the period from the rate self.diag_updater.period = 1.0 / rate # Set the hardware ID to name + pin try: self.diag_updater.setHardwareID(self.name + '_pin_' + str(self.component.pin)) except: self.diag_updater.setHardwareID(self.name) # Create a frequency monitor that tracks the publishing frequency for this component if self.component.rate > 0: if "Servo" in str(self.component): freq_bounds = diagnostic_updater.FrequencyStatusParam( { 'min': self.component.device.joint_update_rate, 'max': self.component.device.joint_update_rate }, 0.3, 5) self.freq_diag = diagnostic_updater.HeaderlessTopicDiagnostic( self.component.name + '_freq', self.diag_updater, freq_bounds) else: freq_bounds = diagnostic_updater.FrequencyStatusParam( { 'min': self.component.rate, 'max': self.component.rate }, 0.3, 5) self.freq_diag = diagnostic_updater.HeaderlessTopicDiagnostic( self.component.name + '_freq', self.diag_updater, freq_bounds) # Create an error monitor that tracks timeouts, serial exceptions etc self.error_diag = diagnostic_updater.FunctionDiagnosticTask( self.name, self.get_error_rate) self.diag_updater.add(self.error_diag) # Create a watchdog diagnostic to monitor the connection status if create_watchdog: self.watchdog_diag = diagnostic_updater.FunctionDiagnosticTask( self.name, self.get_connection_status) self.diag_updater.add(self.watchdog_diag) # Counters used for diagnostics self.reads = 0 self.total_reads = 0 self.errors = 0 self.error_rates = [0.0] # Connection status for device self.watchdog = False
# (FrequencyStatus only, for topics that do not contain a header) or a # TopicDiagnostic (FrequencyStatus and TimestampStatus, for topics that # do contain a header). # # Some values are passed to the constructor as pointers. If these values # are changed, the FrequencyStatus/TimestampStatus will start operating # with the new values. # # Refer to diagnostic_updater.FrequencyStatusParam and # diagnostic_updater.TimestampStatusParam documentation for details on # what the parameters mean: freq_bounds = {'min': 0.5, 'max': 2} # If you update these values, the # HeaderlessTopicDiagnostic will use the new values. pub1_freq = diagnostic_updater.HeaderlessTopicDiagnostic( "topic1", updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10)) # Note that TopicDiagnostic, HeaderlessDiagnosedPublisher, # HeaderlessDiagnosedPublisher and DiagnosedPublisher all descend from # CompositeDiagnosticTask, so you can add your own fields to them using # the addTask method. # # Each time pub1_freq is updated, lower will also get updated and its # output will be merged with the output from pub1_freq. pub1_freq.addTask(lower) # (This wouldn't work if lower was stateful). # If we know that the state of the node just changed, we can force an # immediate update. updater.force_update() # We can remove a task by refering to its name.
'''Republishes an odometry topic to 'odom' and broadcasts the corresponding transform to the base_link frame. This is useful to select which of multiple odometry sources becomes the main odometry ('odom') for the navigation stack and the rest of the high level code. ''' rospy.init_node('odom_republish') odo_pub = rospy.Publisher('odom', Odometry) odomBroadcaster = TransformBroadcaster() diag_updater = DIAG.Updater() diag_updater.setHardwareID('none') diag_param_fs = DIAG.FrequencyStatusParam({'min': 5}, 0.1, 5) diag_task_fs = DIAG.FrequencyStatus(diag_param_fs) diag_updater.add(diag_task_fs) initial = True px = 0 py = 0 pz = 0 def odoCallBack(msg): global initial global px global py global pz if initial==True: px = msg.pose.pose.position.x py = msg.pose.pose.position.y pz = msg.pose.pose.position.z
def __init__(self): rospy.init_node('follow_controllers') cmd_topic = rospy.get_param('~cmd_topic', 'cmd_vel_input') use_mocap = rospy.get_param("~use_mocap", True) self.active_controller_pub = rospy.Publisher( 'active_controller', UInt8, latch=True, queue_size=None) self.controllers = FollowControllers( v1_model_path=rospy.get_param('~v1_model_path'), v2_model_path=rospy.get_param('~v2_model_path'), v3_model_path=rospy.get_param('~v3_model_path')) self.should_publish_all_controllers = rospy.get_param('~enable_debug_pubs') self.active_controller = Controller(rospy.get_param('~controller')) self.srv = Server(ControllersConfig, self.callback) video = message_filters.Subscriber('video', CompressedImage) vo_odom_drone = message_filters.Subscriber('vo_odom_drone', Odometry) subs = [video, vo_odom_drone] if use_mocap: mocap_odom_drone = message_filters.Subscriber('mocap_odom_drone', Odometry) mocap_odom_head = message_filters.Subscriber('mocap_odom_head', Odometry) subs += [mocap_odom_drone, mocap_odom_head] self.controller_pub = {controller: rospy.Publisher( 'output_{name}'.format(name=controller.name), Twist, queue_size=1) for controller in Controller if controller != Controller.idle} self.cmd_pub = rospy.Publisher(cmd_topic, Twist, queue_size=1) message_filters.ApproximateTimeSynchronizer( subs, queue_size=10, slop=0.1).registerCallback(self.update) self.last_controller_duration = {} updater = diagnostic_updater.Updater() updater.setHardwareID("ML") updater.add("Controllers", self.controllers_diagnostics) def update_diagnostics(event): updater.update() rospy.Timer(rospy.Duration(1), update_diagnostics) freq_bounds = {'min': 4, 'max': 6} self.pub_freq = diagnostic_updater.HeaderlessTopicDiagnostic( cmd_topic, updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10)) if use_mocap: self.start_pose = PoseStamped() self.start_pose.header.frame_id = 'World' self.start_pose.pose.position = Point( rospy.get_param('~start_x', 0), rospy.get_param('~start_y', 0), rospy.get_param('~start_z', 1)) start_yaw = rospy.get_param('~start_yaw', 0) self.start_pose.pose.orientation.w = np.cos(start_yaw * 0.5) self.start_pose.pose.orientation.z = np.sin(start_yaw * 0.5) #self.goto = actionlib.SimpleActionClient('fence_control', GoToPoseAction) #self.goto.wait_for_server() else: self.goto = None self.start_pose = None # self.target_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=1) rospy.Subscriber('joy', Joy, self.stop, queue_size=1) rospy.loginfo("Ready to start")
def main(): rclpy.init() node = rclpy.create_node('diagnostic_updater_example') # The Updater class advertises to /diagnostics, and has a # ~diagnostic_period parameter that says how often the diagnostics # should be published. updater = diagnostic_updater.Updater(node) # The diagnostic_updater.Updater class will fill out the hardware_id # field of the diagnostic_msgs.msg.DiagnosticStatus message. You need to # use the setHardwareID() method to set the hardware ID. # # The hardware ID should be able to identify the specific device you are # working with. If it is not appropriate to fill out a hardware ID in # your case, you should call setHardwareID("none") to avoid warnings. # (A warning will be generated as soon as your node updates with no # non-OK statuses.) updater.setHardwareID('none') # Or... updater.setHardwareID('Device-%i-%i' % (27, 46)) # Diagnostic tasks are added to the Updater. They will later be run when # the updater decides to update. # As opposed to the C++ API, there is only one add function. It can take # several types of arguments: # - add(task): where task is a DiagnosticTask # - add(name, fn): add a DiagnosticTask embodied by a name and function updater.add('Function updater', dummy_diagnostic) dc = DummyClass() updater.add('Method updater', dc.produce_diagnostics) # Internally, updater.add converts its arguments into a DiagnosticTask. # Sometimes it can be useful to work directly with DiagnosticTasks. Look # at FrequencyStatus and TimestampStatus in update_functions for a # real-life example of how to make a DiagnosticTask by deriving from # DiagnosticTask. # Alternatively, a FunctionDiagnosticTask is a derived class from # DiagnosticTask that can be used to create a DiagnosticTask from # a function. This will be useful when combining multiple diagnostic # tasks using a CompositeDiagnosticTask. lower = diagnostic_updater.FunctionDiagnosticTask('Lower-bound check', check_lower_bound) upper = diagnostic_updater.FunctionDiagnosticTask('Upper-bound check', check_upper_bound) # If you want to merge the outputs of two diagnostic tasks together, you # can create a CompositeDiagnosticTask, also a derived class from # DiagnosticTask. For example, we could combine the upper and lower # bounds check into a single DiagnosticTask. bounds = diagnostic_updater.CompositeDiagnosticTask('Bound check') bounds.addTask(lower) bounds.addTask(upper) # We can then add the CompositeDiagnosticTask to our Updater. When it is # run, the overall name will be the name of the composite task, i.e., # "Bound check". The summary will be a combination of the summary of the # lower and upper tasks (see DiagnosticStatusWrapper.mergeSummary for # details on how the merging is done). The lists of key-value pairs will be # concatenated. updater.add(bounds) # You can broadcast a message in all the DiagnosticStatus if your node # is in a special state. updater.broadcast(b'0', 'Doing important initialization stuff.') pub1 = node.create_publisher(std_msgs.msg.Bool, 'topic1', 10) sleep(2) # It isn't important if it doesn't take time. # Some diagnostic tasks are very common, such as checking the rate # at which a topic is publishing, or checking that timestamps are # sufficiently recent. FrequencyStatus and TimestampStatus can do these # checks for you. # # Usually you would instantiate them via a HeaderlessTopicDiagnostic # (FrequencyStatus only, for topics that do not contain a header) or a # TopicDiagnostic (FrequencyStatus and TimestampStatus, for topics that # do contain a header). # # Some values are passed to the constructor as pointers. If these values # are changed, the FrequencyStatus/TimestampStatus will start operating # with the new values. # # Refer to diagnostic_updater.FrequencyStatusParam and # diagnostic_updater.TimestampStatusParam documentation for details on # what the parameters mean: freq_bounds = {'min': 0.5, 'max': 2} # If you update these values, the # HeaderlessTopicDiagnostic will use the new values. pub1_freq = (diagnostic_updater.HeaderlessTopicDiagnostic( 'topic1', updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10))) # Note that TopicDiagnostic, HeaderlessDiagnosedPublisher, # HeaderlessDiagnosedPublisher and DiagnosedPublisher all descend from # CompositeDiagnosticTask, so you can add your own fields to them using # the addTask method. # # Each time pub1_freq is updated, lower will also get updated and its # output will be merged with the output from pub1_freq. pub1_freq.addTask(lower) # (This wouldn't work if lower was stateful). # If we know that the state of the node just changed, we can force an # immediate update. updater.force_update() # We can remove a task by refering to its name. if not updater.removeByName('Bound check'): node.get_logger().error( 'The Bound check task was not found when trying to remove it.') while rclpy.ok(): msg = std_msgs.msg.Bool() sleep(0.1) # Calls to pub1 have to be accompanied by calls to pub1_freq to keep # the statistics up to date. msg.data = False pub1.publish(msg) pub1_freq.tick() # We can call updater.update whenever is convenient. It will take care # of rate-limiting the updates. updater.update() rclpy.spin_once(node, timeout_sec=1)
def __init__(self): rospy.init_node("piksi_ros") self.send_observations = False self.serial_number = None self.last_baseline = None self.last_vel = None self.last_pos = None self.last_dops = None self.last_rtk_pos = None self.last_spp_time = 0 self.last_rtk_time = 0 self.fix_mode = -1 self.rtk_fix_mode = 2 self.read_piksi_settings_info() self.piksi_settings = tree() self.proj = None self.disconnect_piksi() self.read_params() self.setup_comms() self.odom_msg = Odometry() self.odom_msg.header.frame_id = self.rtk_frame_id self.odom_msg.child_frame_id = self.child_frame_id self.odom_msg.pose.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) self.odom_msg.pose.covariance[0] = self.rtk_h_accuracy**2 self.odom_msg.pose.covariance[7] = self.rtk_h_accuracy**2 self.odom_msg.pose.covariance[14] = self.rtk_v_accuracy**2 self.odom_msg.twist.covariance[ 0] = self.odom_msg.pose.covariance[0] * 4 self.odom_msg.twist.covariance[ 7] = self.odom_msg.pose.covariance[7] * 4 self.odom_msg.twist.covariance[ 14] = self.odom_msg.pose.covariance[14] * 4 self.odom_msg.pose.covariance[21] = COV_NOT_MEASURED self.odom_msg.pose.covariance[28] = COV_NOT_MEASURED self.odom_msg.pose.covariance[35] = COV_NOT_MEASURED self.odom_msg.twist.covariance[21] = COV_NOT_MEASURED self.odom_msg.twist.covariance[28] = COV_NOT_MEASURED self.odom_msg.twist.covariance[35] = COV_NOT_MEASURED self.transform = TransformStamped() self.transform.header.frame_id = self.utm_frame_id self.transform.child_frame_id = self.rtk_frame_id self.transform.transform.rotation = Quaternion(x=0, y=0, z=0, w=1) # Used to publish transform from rtk_frame_id -> child_frame_id self.base_link_transform = TransformStamped() self.base_link_transform.header.frame_id = self.rtk_frame_id self.base_link_transform.child_frame_id = self.child_frame_id self.base_link_transform.transform.rotation = Quaternion(x=0, y=0, z=0, w=1) self.diag_updater = diagnostic_updater.Updater() self.heartbeat_diag = diagnostic_updater.FrequencyStatus( diagnostic_updater.FrequencyStatusParam( { 'min': self.diag_heartbeat_freq, 'max': self.diag_heartbeat_freq }, self.diag_freq_tolerance, self.diag_window_size)) self.diag_updater.add("Piksi status", self.diag) self.diag_updater.add(self.heartbeat_diag) self.setup_pubsub()
def __init__(self, device): rospy.init_node('pozyx_driver', anonymous=True) updater = diagnostic_updater.Updater() self.ps = deque(maxlen=20) self.es = deque(maxlen=20) self.remote_id = rospy.get_param('~remote_id', None) self.base_frame_id = rospy.get_param('~base_frame_id', 'base_link') debug = rospy.get_param('~debug', False) self.enable_orientation = rospy.get_param('~enable_orientation', True) self.enable_position = rospy.get_param('~enable_position', True) self.enable_raw_sensors = rospy.get_param('~enable_sensors', True) las = rospy.get_param("~linear_acceleration_stddev", 0.0) avs = rospy.get_param("~angular_velocity_stddev", 0.0) mfs = rospy.get_param("~magnetic_field_stddev", 0.0) os = rospy.get_param("~orientation_stddev", 0.0) ps = rospy.get_param('~position_stddev', 0.0) self.pose_cov = cov([ps] * 3 + [os] * 3) self.orientation_cov = cov([os] * 3) self.angular_velocity_cov = cov([avs] * 3) self.magnetic_field_cov = cov([mfs] * 3) self.linear_acceleration_cov = cov([las] * 3) _a = rospy.get_param('~algorithm', 'uwb_only') _d = rospy.get_param('~dimension', '3d') rate = rospy.get_param('~rate', 1.0) # Hz self.algorithm = _algorithms.get(_a, px.POZYX_POS_ALG_UWB_ONLY) self.dimension = _dimensions.get(_d, px.POZYX_3D) baudrate = rospy.get_param('~baudrate', 115200) # height of device, required in 2.5D positioning self.height = rospy.get_param('~height', 0.0) # mm p = None while not p and not rospy.is_shutdown(): try: p = px.PozyxSerial(device, baudrate=baudrate, print_output=debug) except SystemExit: rospy.sleep(1) if not p: return self.pozyx = PozyxProxy(p, self.remote_id) self.pose_pub = rospy.Publisher('pose', PoseStamped, queue_size=1) self.pose_cov_pub = rospy.Publisher('pose_cov', PoseWithCovarianceStamped, queue_size=1) self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1) self.mag_pub = rospy.Publisher('mag', MagneticField, queue_size=1) self.frame_id = rospy.get_param('~anchors/frame_id') anchors = [] if rospy.has_param('~anchors/positions'): anchors_data = rospy.get_param('~anchors/positions') anchors = [ px.DeviceCoordinates(dev_id, 1, px.Coordinates(x, y, z)) for [dev_id, x, y, z] in anchors_data ] self.check_visible_devices() rospy.sleep(0.1) self.set_anchors(anchors) rospy.sleep(0.1) self.check_config(anchors) rospy.sleep(0.1) self.tfBuffer = tf2_ros.Buffer() self.tf = tf2_ros.TransformListener(self.tfBuffer) if self.enable_orientation: try: t = self.tfBuffer.lookup_transform(self.frame_id, 'utm', rospy.Time(), rospy.Duration(5.0)) # r = t.transform.rotation # self.rotation = [r.x, r.y, r.z, r.w] self.rotation = quaternion_from_euler(0, 0, 0) # = r # self.rotation = quaternion_multiply(r, self.rotation) rospy.loginfo('rotation map-> pozyx %s', self.rotation) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logerr('Can not tranform from utm to map frame') self.enable_orientation = False sr = px.SingleRegister() self.pozyx.setOperationMode(0, self.remote_id) # work as a tag self.pozyx.getWhoAmI(sr, self.remote_id) _id = '0x%.2x' % sr.data[0] rospy.set_param('~id', _id) updater.setHardwareID('Pozyx %s' % _id) self.pozyx.getFirmwareVersion(sr, self.remote_id) rospy.set_param('~firmware', register2version(sr)) self.pozyx.getHardwareVersion(sr, self.remote_id) rospy.set_param('~hardware', register2version(sr)) ni = px.NetworkID() self.pozyx.getNetworkId(ni) rospy.set_param('~uwb/network_id', str(ni)) s = px.UWBSettings() self.pozyx.getUWBSettings(s, self.remote_id) rospy.set_param('~uwb/channel', s.channel) rospy.set_param('~uwb/bitrate', s.parse_bitrate()) rospy.set_param('~uwb/prf', s.parse_prf()) rospy.set_param('~uwb/plen', s.parse_plen()) rospy.set_param('~uwb/gain', s.gain_db) self.pozyx.getOperationMode(sr, self.remote_id) rospy.set_param('~uwb/mode', 'anchor' if (sr.data[0] & 1) == 1 else 'tag') self.pozyx.getSensorMode(sr, self.remote_id) rospy.set_param('~sensor_mode', sensor_mode(sr)) self_test = self.check_self_test() if not all(self_test.values()): rospy.logwarn('Failed Self Test %s', self_test) else: rospy.loginfo('Passed Self Test %s', self_test) freq_bounds = {'min': rate * 0.8, 'max': 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) updater.add("Sensor calibration", self.update_sensor_diagnostic) updater.add("Localization", self.update_localization_diagnostic) rospy.on_shutdown(self.cleanup) continuous = rospy.get_param('~continuous', False) rospy.Timer(rospy.Duration(1), lambda evt: updater.update()) rospy.Subscriber('set_anchors', Anchors, self.has_updated_anchors) if continuous: if self.enable_position: ms = int(1000.0 / rate) # self.pozyx.setUpdateInterval(200) msr = px.SingleRegister(ms, size=2) self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr, self.remote_id) self.pozyx.setPositionAlgorithm(self.algorithm, self.dimension) rospy.sleep(0.1) else: msr = px.SingleRegister(0, size=2) self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr, self.remote_id) if self.enable_position or self.enable_raw_sensors: Updater(self, ms * 0.001) # PositionUpdater(self, ms / 1000.0) # if self.enable_raw_sensors: # IMUUpdater(self) # # position = px.Coordinates() # # while not rospy.is_shutdown(): # # try: # # self.pozyx.checkForFlag(bm.POZYX_INT_STATUS_POS, 0.2) # # self.pozyx.getCoordinates(position) # # x = np.array([position.x, position.y, position.z])/1000.0 # # self.publish_pose(x) # # except PozyxException as e: # # rospy.logerr(e.message) # # rospy.sleep(1) rospy.spin() else: rospy.Timer(rospy.Duration(1 / rate), self.update) rospy.spin()