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
"""
# 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 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('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 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 __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
# 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()
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")
bounds = diagnostic_updater.CompositeDiagnosticTask("Bound check")
bounds.addTask(lower_battery)
bounds.addTask(upper_battery)
updater.add(bounds)
updater.broadcast(0, "Initializing...")
pub_battery = rospy.Publisher("bb1_motor_driver",
std_msgs.msg.Bool,
queue_size=10)
freq_bounds = {'min': 9, 'max': 11}
pub_battery_freq = diagnostic_updater.HeaderlessTopicDiagnostic(
"motor_driver", updater,
diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10))
pub_battery_freq.addTask(bounds)
pub_battery_freq.addTask(upper_temperature)
# 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"):
rospy.logerr(
"The Bound check task was not found when trying to remove it.")
while not rospy.is_shutdown():
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)