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)