#screen 0 zsh -is eval 'ros2 run a4wd3 hw_node --ros-args --log-level info'
screen 0 zsh -is eval 'ros2 launch a4wd3 a4wd3_launch.py'
+screen 1 zsh -is eval 'docker_python2 /root/ros2_ws/src/a4wd3/scripts/oled_ssd1306.py "udpsrc ! rsvgdec ! videoconvert"'
target_link_libraries(hw_node i2c)
target_compile_features(hw_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
+add_executable(display src/display.cpp)
+ament_target_dependencies(display rclcpp sensor_msgs)
+
install(TARGETS hw_node
DESTINATION lib/${PROJECT_NAME})
+install(TARGETS display
+ DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME})
install(DIRECTORY params
parameters=[{"enable_odom_tf": False}],
output="screen",
),
+ Node(
+ package='a4wd3',
+ executable='display',
+ name='a4wd3_display',
+ output="screen",
+ ),
Node(
package='bno085_uart',
executable='bno085',
package='tf2_ros',
executable='static_transform_publisher',
name='tf_base_imu',
- arguments = ['--x', '0', '--y', '0.00', '--yaw', '0.0', '--frame-id', 'base_link', '--child-frame-id', 'imu'],
+ arguments = ['--x', '-0.02', '--y', '-0.013', '--roll', '3.1416', '--yaw', '0.0', '--frame-id', 'base_link', '--child-frame-id', 'imu'],
output="screen"
),
Node(
--- /dev/null
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import sys
+import struct
+import gi
+gi.require_version('Gst', '1.0')
+from gi.repository import GObject, Gst
+from time import sleep
+from pyshared.i2c import *
+
+SET_DISPLAY_ON=0xaf
+SET_DISPLAY_OFF=0xa4
+SET_MULTIPLEX_RATIO=0xa8
+SET_DISPLAY_OFFSET=0xd3
+SET_COM_PINS_HARDWARE_CONFIGURATION=0xda
+SET_NORMAL_DISPLAY=0xa6
+SET_INVERSE_DISPLAY=0xa7
+SET_DISPLAY_CLOCK_DIVIDE_RATIO=0xd5
+SET_MEMORY_ADDRESSING_MODE=0x20
+SET_COLUMN_ADDRESS=0x21
+SET_PAGE_ADDRESS=0x22
+SET_CONTRAST=0x81
+
+def binarize(px, thres=127):
+ return px > thres
+
+class ssd1306:
+ def __init__(self):
+ Gst.init(None)
+ self.addr=0x7a
+ self.width=128
+ self.height=64
+ self.initialize()
+
+ def play(self, src):
+ self.pipeline = Gst.parse_launch('%s ! decodebin ! videoconvert ! videoscale add-borders=true ! videorate average-period=1000000000 max-rate=4 ! video/x-raw,format=GRAY8,width=128,height=64 ! appsink' % (src))
+ appsink = self.pipeline.get_by_name('appsink0')
+ bus = self.pipeline.get_bus()
+ bus.add_signal_watch()
+ bus.connect('message', self.on_gst_message)
+ appsink.set_property('emit-signals', True)
+ appsink.connect('new-sample', self.new_appsink_buffer)
+ self.pipeline.set_state(Gst.State.PLAYING)
+ loop = GObject.MainLoop()
+ loop.run()
+
+ def new_appsink_buffer(self, appsink):
+ sample = appsink.emit("pull-sample")
+ buf = sample.get_buffer()
+ imgbuf = buf.extract_dup(0, buf.get_size())
+ pOled.ssd1306_data(list(imgbuf))
+ return Gst.FlowReturn.OK
+
+ def on_gst_message(self, bus, message):
+ if message.type == Gst.MessageType.EOS:
+ sys.exit(0)
+ elif message.type == Gst.MessageType.ERROR:
+ err, debug = message.parse_error()
+ print err, debug
+ sys.exit(1)
+ elif message.type == Gst.MessageType.WARNING:
+ err, debug = message.parse_warning()
+ print err, debug
+ sys.exit(1)
+
+ def ssd1306_cmd(self, ctrlbyte, databytes=[]):
+ data = chr(ctrlbyte) + "".join([chr(i) for i in databytes])
+ i2c_write_reg(self.addr, 0b00000000, data)
+
+ def initialize(self):
+ # Set MUX Ratio
+ self.ssd1306_cmd(SET_MULTIPLEX_RATIO, [63])
+
+ # Set Display Offset
+ self.ssd1306_cmd(SET_DISPLAY_OFFSET, [0])
+
+ # Set Display Start Line
+ self.ssd1306_cmd(0x40)
+
+ # Set Segment re-map 0xA0/0xA1
+ self.ssd1306_cmd(0xa1)
+
+ # Set COM Output Scan Direction 0xC0/0xC8
+ self.ssd1306_cmd(0xc8)
+
+ # Set COM Pins hardware configuration
+ self.ssd1306_cmd(SET_COM_PINS_HARDWARE_CONFIGURATION, [0x12])
+
+ # Disable Entire Display On
+ self.ssd1306_cmd(SET_DISPLAY_OFF)
+
+ # Set Normal Display
+ self.ssd1306_cmd(SET_NORMAL_DISPLAY)
+
+ # Set Osc Frequency
+ self.ssd1306_cmd(SET_DISPLAY_CLOCK_DIVIDE_RATIO, [0x80])
+
+ # Enable charge pump regulator
+ self.ssd1306_cmd(0x8d, [0x14])
+ self.ssd1306_cmd(0xd9, [0xf1])
+ self.ssd1306_cmd(0xdb, [0x40])
+
+ # Misc setup
+ self.ssd1306_cmd(SET_MEMORY_ADDRESSING_MODE, [0x00])
+ self.ssd1306_cmd(SET_CONTRAST, [0x7f])
+
+ # Display On
+ self.ssd1306_cmd(SET_DISPLAY_ON)
+
+ self.clear()
+
+ def ssd1306_data(self, img):
+ img = [ord(c) if type(c) == str else c for c in img]
+ thres = sum(img)/len(img)
+ print "Threshold=%d" % (thres)
+ if thres <= 31: thres=31
+ elif thres > 223: thres=223
+ data = []
+ for y in range(0, self.height, 8): # step with page increment
+ for x in range(0, self.width):
+ px = binarize(img[(y+0)*self.width + x], thres) << 0
+ px |= binarize(img[(y+1)*self.width + x], thres) << 1
+ px |= binarize(img[(y+2)*self.width + x], thres) << 2
+ px |= binarize(img[(y+3)*self.width + x], thres) << 3
+ px |= binarize(img[(y+4)*self.width + x], thres) << 4
+ px |= binarize(img[(y+5)*self.width + x], thres) << 5
+ px |= binarize(img[(y+6)*self.width + x], thres) << 6
+ px |= binarize(img[(y+7)*self.width + x], thres) << 7
+ data.append(chr(px))
+
+ self.ssd1306_cmd(SET_COLUMN_ADDRESS, [0x00, 0x7f])
+ self.ssd1306_cmd(SET_PAGE_ADDRESS, [0x00, 0x07])
+ i2c_write_reg(self.addr, 0b01000000, "".join(data))
+
+ def clear(self):
+ img = [0x00] * (128*64)
+ self.ssd1306_data(img)
+
+if __name__ == "__main__":
+ pOled = ssd1306()
+ pOled.play(sys.argv[1])
+++ /dev/null
-#!/usr/bin/env python
-# -*- coding: iso-8859-15 -*-
-
-import sys
-import struct
-import gi
-gi.require_version('Gst', '1.0')
-from gi.repository import GObject, Gst
-from time import sleep
-from pyshared.i2c import *
-
-SET_DISPLAY_ON=0xaf
-SET_DISPLAY_OFF=0xa4
-SET_MULTIPLEX_RATIO=0xa8
-SET_DISPLAY_OFFSET=0xd3
-SET_COM_PINS_HARDWARE_CONFIGURATION=0xda
-SET_NORMAL_DISPLAY=0xa6
-SET_INVERSE_DISPLAY=0xa7
-SET_DISPLAY_CLOCK_DIVIDE_RATIO=0xd5
-SET_MEMORY_ADDRESSING_MODE=0x20
-SET_COLUMN_ADDRESS=0x21
-SET_PAGE_ADDRESS=0x22
-SET_CONTRAST=0x81
-
-def binarize(px, thres=127):
- return px > thres
-
-class ssd1306:
- def __init__(self):
- Gst.init(None)
- self.addr=0x7a
- self.width=128
- self.height=64
- self.initialize()
-
- def play(self, src):
- self.pipeline = Gst.parse_launch('%s ! decodebin ! videoconvert ! videoscale add-borders=true ! videorate average-period=1000000000 max-rate=4 ! video/x-raw,format=GRAY8,width=128,height=64 ! appsink' % (src))
- appsink = self.pipeline.get_by_name('appsink0')
- bus = self.pipeline.get_bus()
- bus.add_signal_watch()
- bus.connect('message', self.on_gst_message)
- appsink.set_property('emit-signals', True)
- appsink.connect('new-sample', self.new_appsink_buffer)
- self.pipeline.set_state(Gst.State.PLAYING)
- loop = GObject.MainLoop()
- loop.run()
-
- def new_appsink_buffer(self, appsink):
- sample = appsink.emit("pull-sample")
- buf = sample.get_buffer()
- imgbuf = buf.extract_dup(0, buf.get_size())
- pOled.ssd1306_data(list(imgbuf))
- return Gst.FlowReturn.OK
-
- def on_gst_message(self, bus, message):
- if message.type == Gst.MessageType.EOS:
- sys.exit(0)
- elif message.type == Gst.MessageType.ERROR:
- err, debug = message.parse_error()
- print err, debug
- sys.exit(1)
- elif message.type == Gst.MessageType.WARNING:
- err, debug = message.parse_warning()
- print err, debug
- sys.exit(1)
-
- def ssd1306_cmd(self, ctrlbyte, databytes=[]):
- data = chr(ctrlbyte) + "".join([chr(i) for i in databytes])
- i2c_write_reg(self.addr, 0b00000000, data)
-
- def initialize(self):
- # Set MUX Ratio
- self.ssd1306_cmd(SET_MULTIPLEX_RATIO, [63])
-
- # Set Display Offset
- self.ssd1306_cmd(SET_DISPLAY_OFFSET, [0])
-
- # Set Display Start Line
- self.ssd1306_cmd(0x40)
-
- # Set Segment re-map 0xA0/0xA1
- self.ssd1306_cmd(0xa1)
-
- # Set COM Output Scan Direction 0xC0/0xC8
- self.ssd1306_cmd(0xc8)
-
- # Set COM Pins hardware configuration
- self.ssd1306_cmd(SET_COM_PINS_HARDWARE_CONFIGURATION, [0x12])
-
- # Disable Entire Display On
- self.ssd1306_cmd(SET_DISPLAY_OFF)
-
- # Set Normal Display
- self.ssd1306_cmd(SET_NORMAL_DISPLAY)
-
- # Set Osc Frequency
- self.ssd1306_cmd(SET_DISPLAY_CLOCK_DIVIDE_RATIO, [0x80])
-
- # Enable charge pump regulator
- self.ssd1306_cmd(0x8d, [0x14])
- self.ssd1306_cmd(0xd9, [0xf1])
- self.ssd1306_cmd(0xdb, [0x40])
-
- # Misc setup
- self.ssd1306_cmd(SET_MEMORY_ADDRESSING_MODE, [0x00])
- self.ssd1306_cmd(SET_CONTRAST, [0x7f])
-
- # Display On
- self.ssd1306_cmd(SET_DISPLAY_ON)
-
- self.clear()
-
- def ssd1306_data(self, img):
- img = [ord(c) if type(c) == str else c for c in img]
- thres = sum(img)/len(img)
- print "Threshold=%d" % (thres)
- if thres <= 31: thres=31
- elif thres > 223: thres=223
- data = []
- for y in range(0, self.height, 8): # step with page increment
- for x in range(0, self.width):
- px = binarize(img[(y+0)*self.width + x], thres) << 0
- px |= binarize(img[(y+1)*self.width + x], thres) << 1
- px |= binarize(img[(y+2)*self.width + x], thres) << 2
- px |= binarize(img[(y+3)*self.width + x], thres) << 3
- px |= binarize(img[(y+4)*self.width + x], thres) << 4
- px |= binarize(img[(y+5)*self.width + x], thres) << 5
- px |= binarize(img[(y+6)*self.width + x], thres) << 6
- px |= binarize(img[(y+7)*self.width + x], thres) << 7
- data.append(chr(px))
-
- self.ssd1306_cmd(SET_COLUMN_ADDRESS, [0x00, 0x7f])
- self.ssd1306_cmd(SET_PAGE_ADDRESS, [0x00, 0x07])
- i2c_write_reg(self.addr, 0b01000000, "".join(data))
-
- def clear(self):
- img = [0x00] * (128*64)
- self.ssd1306_data(img)
-
-if __name__ == "__main__":
- pOled = ssd1306()
- pOled.play(sys.argv[1])
--- /dev/null
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/battery_state.hpp"
+#include <sys/socket.h>
+#include <arpa/inet.h>
+
+using std::placeholders::_1;
+
+class A4wd3display : public rclcpp::Node {
+ public:
+ A4wd3display() : Node("A4WD3_display")
+ {
+ sub_bat = this->create_subscription<sensor_msgs::msg::BatteryState>("battery", 10, std::bind(&A4wd3display::battery_callback, this, _1));
+ sock = socket(AF_INET, SOCK_DGRAM, 0);
+ if (sock < 0) {
+ perror("socket");
+ exit(-1);
+ }
+ memset((char *) &addr, 0, sizeof(addr));
+ addr.sin_family = AF_INET;
+ addr.sin_port = htons(5004);
+ inet_aton("127.0.0.1", &addr.sin_addr);
+ }
+
+ private:
+ rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr sub_bat;
+ int sock;
+ struct sockaddr_in addr;
+
+ void battery_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg) const {
+ char svg[1024];
+ const char *svg_template = R"""(
+ <svg width="128" height="64" xmlns="http://www.w3.org/2000/svg">"
+ <rect width="128" height="64" x="0" y="0" style="fill:rgb(255,255,255)" />
+ <text x="0" y="15" font-family="monospace" xml:space="preserve">%5.2f V</text>
+ <text x="0" y="30" font-family="monospace" xml:space="preserve">%5.2f A</text>
+ <text x="0" y="45" font-family="monospace" xml:space="preserve"></text>
+ <text x="0" y="60" font-family="monospace" xml:space="preserve"></text>
+ </svg>
+ )""";
+
+ snprintf(svg, 1024, svg_template, msg->voltage, msg->current);
+ RCLCPP_DEBUG(this->get_logger(), "%s", svg);
+ if (sendto(sock, svg, strlen(svg), 0, (struct sockaddr *)&addr, sizeof(addr)) < 0 ) {
+ perror("sendto");
+ }
+ }
+};
+
+int main(int argc, char * argv[]) {
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared<A4wd3display>());
+ rclcpp::shutdown();
+ return 0;
+}
RCLCPP_ERROR(this->get_logger(), "Failed to read Odometry err=%d", ret);
return;
}
- uint8_t count;
- const int retcount = i2c_read_reg(0x50, 0xa2, 1, &count);
- RCLCPP_INFO(this->get_logger(), "Count: %d, ret: %d", count, retcount);
values[0].i = __bswap_32(values[0].i);
values[1].i = __bswap_32(values[1].i);
projects / a4wd3.git / commitdiff