Merge pull request #6 from SemvdH/camera

Camera node to take pictures is done

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

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
+
+

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
+
+

get_usb_device_port.sh

@@ -0,0 +1 @@
+/bin/udevadm info --name=/dev/ttyACM0 | grep SERIAL_SHORT | cut -d = -f 2

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

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

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

src/camera/camera/camera_controller.py

@@ -0,0 +1,50 @@
+import rclpy
+from rclpy.node import Node
+
+from drone_services.srv import TakePicture
+import os
+from datetime import datetime
+
+# import cv2
+
+RES_4K_H = 3496
+RES_4K_W = 4656
+
+
+class CameraController(Node):
+    def __init__(self):
+        super().__init__('camera_controller')
+
+        self.get_logger().info("Camera controller started. Waiting for service call...")
+        self.srv = self.create_service(
+            TakePicture, 'drone/picture', self.take_picture_callback)
+
+    def take_picture_callback(self, request, response):
+        if (request.input_name == "default"):
+            self.get_logger().info("Taking picture with default filename")
+            now = datetime.now().strftime("droneimage_%Y-%m-%d_%H-%M-%S")
+            imagename = "/home/ubuntu/drone_img/" + now + ".jpg"
+            response.filename = imagename
+        else:
+            response.filename = request.input_name
+        os.system('fswebcam -r ' + RES_4K_W + 'x' + RES_4K_H + ' ' + response.filename)
+        self.get_logger().info("Picture saved as " + response.filename)
+
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    test_controller = CameraController()
+    rclpy.spin(test_controller)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    test_controller.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/camera/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>camera</name>
+    <version>0.0.0</version>
+    <description>Package for controlling the camera of the drone</description>
+    <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
+    <license>Apache License 2.0</license>
+
+    <exec_depend>rclpy</exec_depend>
+    <exec_depend>drone_services</exec_depend>
+
+    <test_depend>ament_copyright</test_depend>
+    <test_depend>ament_flake8</test_depend>
+    <test_depend>ament_pep257</test_depend>
+    <test_depend>python3-pytest</test_depend>
+
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>

src/camera/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/camera
+[install]
+install-scripts=$base/lib/camera

src/camera/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'camera'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='ubuntu',
+    maintainer_email='semmer99@gmail.com',
+    description='Package for controlling the camera of the drone',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'camera_controller = camera.camera_controller:main'
+        ],
+    },
+)

src/camera/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/camera/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/camera/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/drone_services/CMakeLists.txt

@@ -23,6 +23,8 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SetAttitude.srv"
   "srv/SetVelocity.srv"  
+  "srv/TakePicture.srv"
+  "srv/SetVehicleControl.srv"
 )
 
 if(BUILD_TESTING)

src/drone_services/srv/SetVehicleControl.srv

@@ -0,0 +1,3 @@
+int32 control # control bitmask
+---
+int8 status # status of operation

src/drone_services/srv/TakePicture.srv

@@ -0,0 +1,3 @@
+wstring input_name "default" # name of the input file
+---
+wstring filename # output file name

src/object_detection/src/lidar_reader.cpp

@@ -1,8 +1,4 @@
 #include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "object_detection/msg/lidar_reading.hpp"

src/object_detection/src/multiflex_reader.cpp

@@ -1,8 +1,4 @@
 #include <chrono>
-#include <functional>
-#include <memory>
-#include <string>
-#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "object_detection/msg/multiflex_reading.hpp"
@@ -11,6 +7,8 @@
 #include <terabee/ITerarangerMultiflex.hpp>
 #include <terabee/DistanceData.hpp>
 
+// #include <iostream>
+
 using terabee::DistanceData;
 
 using namespace std::chrono_literals;
@@ -42,9 +40,9 @@ public:
             return;
         }
 
-        if (!multiflex->configureNumberOfSensors(0x3f)) // check if all 6 distance sensors work
+        if (!multiflex->configureNumberOfSensors(0x7)) // check if all 6 distance sensors work
         {
-            RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!");
+            RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 3!");
             return;
         }
 
@@ -76,7 +74,8 @@ private:
         auto msg = object_detection::msg::MultiflexReading();
         for (size_t i = 0; i < data.size(); i++)
         {
-            msg.distance_data[i] = data.distance[i];
+            RCLCPP_INFO(this->get_logger(), "distance %f", data.distance[i]);
+            //            msg.distance_data[i] = data.distance[i];
         }
 
         // publish message

src/test_controls/test_controls/test_controller.py

@@ -16,7 +16,7 @@ class TestController(Node):
             self.get_logger().info('service not available, waiting again...')
         self.req = SetAttitude.Request()
 
-        self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nShift - down\nN - emergency stop\nEsc - exit")
+        self.get_logger().info("Controls:\nW - forward\nS - backward\nA - left\nD - right\nQ - rotate left\nE - rotate right\nSpace - up\nZ - down\nV - Down nudge\nF - Up nudge\nN - emergency stop\nEsc - exit")
 
     def spin(self):
         while rclpy.ok():
@@ -43,27 +43,27 @@ class TestController(Node):
             # self.get_logger().info('pressed ' + char)
             if key == 'w':
                 self.get_logger().info('forward')
-                self.send_request(pitch=-10.0, yaw=0.0,
+                self.send_request(pitch=-1.0, yaw=0.0,
                                   roll=0.0, thrust=0.0)
             if key == 's':
                 self.get_logger().info('backward')
-                self.send_request(pitch=10.0, yaw=0.0,
+                self.send_request(pitch=1.0, yaw=0.0,
                                   roll=0.0, thrust=0.0)
             if key == 'a':
                 self.get_logger().info('left')
                 self.send_request(pitch=0.0, yaw=0.0,
-                                  roll=-10.0, thrust=0.0)
+                                  roll=-1.0, thrust=0.0)
             if key == 'd':
                 self.get_logger().info('right')
                 self.send_request(pitch=0.0, yaw=0.0,
-                                  roll=10.0, thrust=0.0)
+                                  roll=1.0, thrust=0.0)
             if key == 'q':
                 self.get_logger().info('rotate left')
-                self.send_request(pitch=0.0, yaw=-10.0,
+                self.send_request(pitch=0.0, yaw=-1.0,
                                   roll=0.0, thrust=0.0)
             if key == 'e':
                 self.get_logger().info('rotate right')
-                self.send_request(pitch=0.0, yaw=10.0,
+                self.send_request(pitch=0.0, yaw=1.0,
                                   roll=0.0, thrust=0.0)
             if key == 'z':
                 self.get_logger().info('down')
@@ -73,10 +73,19 @@ class TestController(Node):
                 self.get_logger().info('up')
                 self.send_request(pitch=0.0, yaw=0.0,
                                   roll=0.0, thrust=0.05)
+            if key == 'v':
+                self.get_logger().info('down a little')
+                self.send_request(pitch=0.0, yaw=0.0,
+                                  roll=0.0, thrust=-0.01)
+            if key == 'f':
+                self.get_logger().info('up a little')
+                self.send_request(pitch=0.0, yaw=0.0,
+                                  roll=0.0, thrust=0.01)
             if key == 'n':
                 self.get_logger().info('stop')
                 self.send_request(pitch=0.0, yaw=0.0,
                                   roll=0.0, thrust=0.0)
+            
             # else:
             #     try:
             #         # known keys like spacebar, ctrl