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