def main():
t = 0.0
A = 10.0 # Sine wave amplitude [-]
F = 1.3 # Sine wave frequency [Hz]
A2 = 1.2
F2 = 100
B = 5.0 # Sine wave offset
sigma_delta_step = 0.3
sigma_delta_value = 0
client = LognplotTcpClient()
client.connect()
dt = 0.0001 # 10 kHz
n_samples = 2000
while True:
samples = []
samples2 = []
samples3 = []
samples4 = []
t0 = t
# Generate samples:
for _ in range(n_samples):
omega = 2 * math.pi * F
omega2 = 2 * math.pi * F2
sample = A * math.sin(omega * t) + B + A2 * math.cos(omega2 * t)
sample2 = A * math.sin(omega * t) + B + A2 * math.cos(
omega2 * t) + 9
# Track sample with binary output:
if sigma_delta_value < sample:
sample3 = 1.0
sigma_delta_value += sigma_delta_step
else:
sample3 = 0.0
sigma_delta_value -= sigma_delta_step
samples.append(sample)
samples2.append(sample2)
samples3.append(sample3)
samples4.append(float(random.randint(0, 1)))
# Increment time:
t += dt
gen = random_bits()
samples5 = [float(next(gen)) for _ in range(n_samples)]
print(f"Sending {len(samples)} samples")
client.send_samples("Trace1", t0, dt, samples)
client.send_samples("Trace2", t0, dt, samples2)
client.send_samples("SIGMA-DELTA", t0, dt, samples3)
client.send_samples("RANDOM", t0, dt, samples4)
client.send_samples("TEXT_BITS", t0, dt, samples5)
client.send_text("Log", t0, f"Log at {t0:.3}")
client.send_text("Log2", t0, f"Another log at {t0:.3}")
client.send_text("Other TXT", t0, f"Yet {t0:.3} blah")
time.sleep(n_samples * dt)
class RosToLogNPlot:
def __init__(self, lognplot_host, lognplot_port):
self._subscriptions = {}
self._lognplot_host = lognplot_host
self._lognplot_port = lognplot_port
def connect(self):
try:
self._client = LognplotTcpClient(hostname=self._lognplot_host,
port=self._lognplot_port)
self._client.connect()
except ConnectionRefusedError:
print("Error connecting to lognplot GUI!")
self._client = None
def is_connected(self):
return bool(self._client)
def run(self):
self.node = rclpy.create_node("ros_to_lognplot")
self.timer = self.node.create_timer(2.0, self._check_topics)
self.node.create_subscription(Log, "/rosout", self.on_ros_out_msg, 0)
rclpy.spin(self.node)
rclpy.shutdown()
def on_ros_out_msg(self, msg):
signal_name = f'/rosout/{msg.name}'
timestamp = time.time()
text = msg.msg
self.send_text(signal_name, timestamp, text)
def _check_topics(self):
""" Check which topics are present in the system, and subscribe to them all!
"""
topics = self.node.get_topic_names_and_types()
for topic_name, topic_type_name in topics:
if not self.is_subscribed(topic_name):
print("-", topic_name, "---", topic_type_name)
topic_type = load_type(topic_type_name[0])
self._subscribe_on_topic(topic_name, topic_type)
def is_subscribed(self, topic_name):
return topic_name in self._subscriptions
def _subscribe_on_topic(self, topic_name, topic_type):
assert topic_name not in self._subscriptions
def handler(msg):
timestamp = time.time()
self.process_message(topic_name, topic_type, timestamp, msg)
subscription = self.node.create_subscription(topic_type, topic_name,
handler,
qos_profile_sensor_data)
self._subscriptions[topic_name] = subscription
def process_message(self, topic_name, topic_type, timestamp, msg):
""" Process an incoming ROS message.
"""
self.process_value(topic_name, topic_type, timestamp, msg)
def process_value(self, full_name, value_type, timestamp, value):
if hasattr(value, "get_fields_and_field_types"):
for field_name, field_type in value.get_fields_and_field_types(
).items():
field_value = getattr(value, field_name)
full_field_name = f"{full_name}.{field_name}"
self.process_value(full_field_name, field_type, timestamp,
field_value)
else:
if isinstance(value, (float, np.float32, int)):
self.send_sample(full_name, timestamp, float(value))
elif isinstance(value, (list, np.ndarray)):
for element_index, element_value in enumerate(value):
element_name = f"{full_name}[{element_index}]"
element_type = None
self.process_value(element_name, element_type, timestamp,
element_value)
else:
# Great panic! What now?
# Ignore for now..
pass
def send_sample(self, signal_name: str, timestamp, value):
""" Emit a single sample to the lognplot GUI. """
if self._client:
self._client.send_sample(signal_name, timestamp, value)
def send_text(self, signal_name: str, timestamp, text):
""" Emit a single text to the lognplot GUI. """
if self._client:
self._client.send_text(signal_name, timestamp, text)