add generated viewer node an update python script
This commit is contained in:
parent
76634ef316
commit
ccca372ef3
4
launch.bash
Normal file → Executable file
4
launch.bash
Normal file → Executable file
@ -1,4 +1,6 @@
|
||||
#!/bin/bash
|
||||
ros2 run ft_analytics can_viewer_node foxglove_bridge foxglove_bridge \
|
||||
ros2 run ft_analytics can_viewer_node
|
||||
|
||||
ros2 run foxglove_bridge foxglove_bridge \
|
||||
--ros-args -p send_buffer_limit:=1000000000
|
||||
|
||||
|
@ -1,5 +1,11 @@
|
||||
import re
|
||||
|
||||
def normalize_signal(name):
|
||||
s1 = re.sub('(.)([A-Z][a-z]+)', r'\1_\2', name)
|
||||
s2 = re.sub('([a-z0-9])([A-Z])', r'\1_\2', s1)
|
||||
s3 = s2.replace("__", "_")
|
||||
return s3.lower()
|
||||
|
||||
dbc_path = "/workspaces/ft_analytics/ros2_ws/src/ft_can_transceiver_lib/dbc/can/CANdbcFT25/CAN1MainFT25.dbc"
|
||||
|
||||
with open(dbc_path, "r") as f:
|
||||
@ -13,14 +19,18 @@ current = None
|
||||
for line in dbc_text.splitlines():
|
||||
m = msg_pattern.match(line)
|
||||
if m:
|
||||
msg_name = m.group(1)
|
||||
msg_name = normalize_signal(m.group(1))
|
||||
if msg_name == 'vector_independent_sig_msg' or msg_name == 'stw_param_set':
|
||||
current = None
|
||||
continue
|
||||
current = {"name": msg_name, "signals": []}
|
||||
messages.append(current)
|
||||
elif current:
|
||||
sg = sg_pattern.match(line)
|
||||
if sg:
|
||||
sig_name = normalize_signal(sg.group(1))
|
||||
current["signals"].append({
|
||||
"name": sg.group(1),
|
||||
"name": sig_name,
|
||||
"bit_length": int(sg.group(2))
|
||||
})
|
||||
|
||||
@ -57,11 +67,11 @@ publisher_init = []
|
||||
for msg in messages:
|
||||
msg_lc = msg["name"].lower()
|
||||
for sig in msg["signals"]:
|
||||
topic = get_topic(msg_lc, sig["name"])
|
||||
topic = get_topic(msg_lc, sig["name"].lower())
|
||||
if sig["bit_length"] == 1:
|
||||
publisher_init.append(f' bool_publishers_["{sig["name"]}"] = this->create_publisher<std_msgs::msg::Bool>("{topic}", 10);')
|
||||
publisher_init.append(f' bool_publishers_["{sig["name"].lower()}"] = this->create_publisher<std_msgs::msg::Bool>("{topic}", 10);')
|
||||
else:
|
||||
publisher_init.append(f' float_publishers_["{sig["name"]}"] = this->create_publisher<std_msgs::msg::Float32>("{topic}", 10);')
|
||||
publisher_init.append(f' float_publishers_["{sig["name"].lower()}"] = this->create_publisher<std_msgs::msg::Float32>("{topic}", 10);')
|
||||
|
||||
# --- CALLBACK BLOECKE ---
|
||||
callback_blocks = []
|
||||
@ -75,14 +85,14 @@ for msg in messages:
|
||||
block.append(f' {ros_msg_type(sig["bit_length"])} msg_{sig["name"].lower()};')
|
||||
block.append("")
|
||||
for sig in msg["signals"]:
|
||||
block.append(f' msg_{sig["name"].lower()}.data = frame_decoded.{sig["name"]};')
|
||||
block.append(f' msg_{sig["name"].lower()}.data = frame_decoded.{sig["name"].lower()};')
|
||||
block.append("")
|
||||
for sig in msg["signals"]:
|
||||
block.append(f' {pub_map(sig["bit_length"])}["{sig["name"]}"]->publish(msg_{sig["name"].lower()});')
|
||||
block.append("")
|
||||
log_parts = [f'{sig["name"]}=%{ros_data_type(sig["bit_length"])}' for sig in msg["signals"]]
|
||||
log_parts = [f'{sig["name"].lower()}=%{ros_data_type(sig["bit_length"])}' for sig in msg["signals"]]
|
||||
log_vars = ', '.join([f'msg_{sig["name"].lower()}.data' for sig in msg["signals"]])
|
||||
block.append(f' RCLCPP_INFO(this->get_logger(), "{msg["name"]}: {" ".join(log_parts)}", {log_vars});')
|
||||
block.append(f' RCLCPP_INFO(this->get_logger(), "{msg["name"].lower()}: {" ".join(log_parts)}", {log_vars});')
|
||||
block.append(" };")
|
||||
callback_blocks.append("\n".join(block))
|
||||
|
||||
@ -120,7 +130,7 @@ int main(int argc, char * argv[]) {
|
||||
'''
|
||||
|
||||
# --- AUSGABE ---
|
||||
with open("ros2_ws/src/ft_can_viewer/test/can_viewer_node.cpp", "w") as f:
|
||||
with open("ros2_ws/src/ft_can_viewer/src/can_viewer_node.cpp", "w") as f:
|
||||
f.write(header)
|
||||
f.write('\n'.join(publisher_init))
|
||||
f.write("\n\n")
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -1,9 +1,19 @@
|
||||
#!/usr/bin/env bash
|
||||
# 1 000 Frames, 10 ms Abstand
|
||||
for i in $(seq 0 999); do
|
||||
# Zufallswert 0-255 → zweistelliges Hex
|
||||
v=$(printf '%02X' $((RANDOM % 256)))
|
||||
# Byte0 Byte1 Byte2=[$v] rest = 0
|
||||
cansend slcan0 720#0001${v}00000000
|
||||
sleep 0.01
|
||||
done
|
||||
|
||||
DBC="/workspaces/ft_analytics/ros2_ws/src/ft_can_transceiver_lib/dbc/can/CANdbcFT25/CAN1MainFT25.dbc"
|
||||
IFACE="slcan0" # Dein CAN-Interface
|
||||
|
||||
grep '^BO_' "$DBC" | \
|
||||
sed -E 's/BO_ ([0-9]+) ([^:]+): ([0-9]+) .*/\1 \2 \3/' | \
|
||||
while read id name dlc; do
|
||||
(
|
||||
echo "Simuliere $name (ID $id) mit $dlc Bytes"
|
||||
while true; do
|
||||
payload=$(for i in $(seq 1 $dlc); do printf '%02X' $((RANDOM%256)); done)
|
||||
cansend $IFACE $(printf "%03X" $((id)))#${payload}
|
||||
sleep 0.05
|
||||
done
|
||||
) &
|
||||
done
|
||||
trap "kill 0" SIGINT
|
||||
wait
|
||||
|
Loading…
x
Reference in New Issue
Block a user