From 31f3959de44d665a15422a45c1a2fd30e40828f7 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Tue, 9 May 2023 12:48:18 +0200
Subject: [PATCH] add services

---
 drone_scripts/TerabeeLog.log            |  0
 drone_scripts/find_id.sh                | 32 +++++++++++++++++++++++++
 drone_scripts/start_height_sensor.sh    | 10 ++++++++
 drone_scripts/start_lidar.sh            | 10 ++++++++
 get_usb_device_port.sh                  |  1 +
 services/drone_find_usb_devices.service | 12 ++++++++++
 services/drone_height_sensor.service    | 14 +++++++++++
 services/drone_lidar.service            | 14 +++++++++++
 src/px4_msgs                            |  2 +-
 9 files changed, 94 insertions(+), 1 deletion(-)
 create mode 100644 drone_scripts/TerabeeLog.log
 create mode 100755 drone_scripts/find_id.sh
 create mode 100755 drone_scripts/start_height_sensor.sh
 create mode 100755 drone_scripts/start_lidar.sh
 create mode 100644 get_usb_device_port.sh
 create mode 100644 services/drone_find_usb_devices.service
 create mode 100644 services/drone_height_sensor.service
 create mode 100644 services/drone_lidar.service

diff --git a/drone_scripts/TerabeeLog.log b/drone_scripts/TerabeeLog.log
new file mode 100644
index 00000000..e69de29b
diff --git a/drone_scripts/find_id.sh b/drone_scripts/find_id.sh
new file mode 100755
index 00000000..3b9e4e2b
--- /dev/null
+++ b/drone_scripts/find_id.sh
@@ -0,0 +1,32 @@
+#!/bin/bash
+
+lidarserial=00000000001A
+heightserial=369A378F3439
+
+find_by_id(){
+    v=${1%:*}; p=${1#*:}  # split vid:pid into 2 vars
+    v=${v#${v%%[!0]*}}; p=${p#${p%%[!0]*}}  # strip leading zeros
+    grep -il "^PRODUCT=$v/$p" /sys/bus/usb/devices/*:*/uevent |
+    sed s,uevent,, |
+    xargs -r grep -r '^DEVNAME=' --include uevent
+}
+ids=$(find_by_id 0483:5740)
+echo $ids
+
+for device in $ids
+do
+	id=$(echo $device | cut -d "=" -f 2)
+	serial=$(/bin/udevadm info --name=/dev/$id | grep SERIAL_SHORT | cut -d = -f 2)
+	echo "device "$id" has serial number "$serial
+	if [ $serial = $lidarserial ]
+	then
+		echo "LIDAR on port" $id
+		echo $id > /home/ubuntu/drone_conf/lidar.conf
+	elif [ $serial = $heightserial ]
+	then
+		echo "height sensor on port" $id
+		echo $id > /home/ubuntu/drone_conf/height.conf
+	else
+		echo "unknown serial number" $serial
+	fi
+done
diff --git a/drone_scripts/start_height_sensor.sh b/drone_scripts/start_height_sensor.sh
new file mode 100755
index 00000000..8c02f0a9
--- /dev/null
+++ b/drone_scripts/start_height_sensor.sh
@@ -0,0 +1,10 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+SERIAL=$(cat /home/ubuntu/drone_conf/height.conf)
+echo "serial port is "$SERIAL
+echo ros2 run height height_reader --ros-args -p height_serial_port:=/dev/${SERIAL}
+ros2 run height height_reader --ros-args -p height_serial_port:=/dev/${SERIAL} | tee /home/ubuntu/drone_log/height.log
+
+
diff --git a/drone_scripts/start_lidar.sh b/drone_scripts/start_lidar.sh
new file mode 100755
index 00000000..af29f65f
--- /dev/null
+++ b/drone_scripts/start_lidar.sh
@@ -0,0 +1,10 @@
+#!/bin/bash
+
+. /home/ubuntu/source_ros2.sh
+
+SERIAL=$(cat /home/ubuntu/drone_conf/lidar.conf)
+echo "serial port is "$SERIAL
+echo ros2 run object_detection lidar_reader --ros-args -p lidar_serial_port:=/dev/${SERIAL}
+ros2 run object_detection lidar_reader --ros-args -p lidar_serial_port:=/dev/${SERIAL} | tee /home/ubuntu/drone_log/lidar.log
+
+
diff --git a/get_usb_device_port.sh b/get_usb_device_port.sh
new file mode 100644
index 00000000..8e119e7e
--- /dev/null
+++ b/get_usb_device_port.sh
@@ -0,0 +1 @@
+/bin/udevadm info --name=/dev/ttyACM0 | grep SERIAL_SHORT | cut -d = -f 2
diff --git a/services/drone_find_usb_devices.service b/services/drone_find_usb_devices.service
new file mode 100644
index 00000000..cf4ed424
--- /dev/null
+++ b/services/drone_find_usb_devices.service
@@ -0,0 +1,12 @@
+[Unit]
+Description=Service to start the script that sets the serial ports for the USB sensors
+
+[Service]
+Type=simple
+Restart=on-failure
+User=ubuntu
+ExecStart=/home/ubuntu/drone_scripts/find_id.sh
+WorkingDirectory=/home/ubuntu
+
+[Install]
+WantedBy=multi-user.target
diff --git a/services/drone_height_sensor.service b/services/drone_height_sensor.service
new file mode 100644
index 00000000..d13bead9
--- /dev/null
+++ b/services/drone_height_sensor.service
@@ -0,0 +1,14 @@
+[Unit]
+Description=Height sensor for drone running in ROS 2
+After=drone_find_usb_devices.service
+
+[Service]
+Type=simple
+Restart=on-failure
+User=ubuntu
+ExecStart=/home/ubuntu/drone_scripts/start_height_sensor.sh
+Environment="HOME=root"
+WorkingDirectory=/home/ubuntu
+
+[Install]
+WantedBy=multi-user.target
diff --git a/services/drone_lidar.service b/services/drone_lidar.service
new file mode 100644
index 00000000..71af1114
--- /dev/null
+++ b/services/drone_lidar.service
@@ -0,0 +1,14 @@
+[Unit]
+Description=Height sensor for drone running in ROS 2
+After=drone_find_usb_devices.service
+
+[Service]
+Type=simple
+Restart=on-failure
+User=ubuntu
+ExecStart=/home/ubuntu/drone_scripts/start_lidar.sh
+Environment="HOME=root"
+WorkingDirectory=/home/ubuntu
+
+[Install]
+WantedBy=multi-user.target
diff --git a/src/px4_msgs b/src/px4_msgs
index b64ef047..ffc3a4cd 160000
--- a/src/px4_msgs
+++ b/src/px4_msgs
@@ -1 +1 @@
-Subproject commit b64ef0475c1d44605688f4770899fe453d532be4
+Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266