diff --git a/drone_image.jpg b/drone_image.jpg new file mode 100644 index 00000000..b1ef45bf Binary files /dev/null and b/drone_image.jpg differ 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/image.jpg b/image.jpg new file mode 100644 index 00000000..309ea383 Binary files /dev/null and b/image.jpg differ 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/camera/camera/__init__.py b/src/camera/camera/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py new file mode 100644 index 00000000..e589c9e9 --- /dev/null +++ b/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() diff --git a/src/camera/package.xml b/src/camera/package.xml new file mode 100644 index 00000000..e9065b1e --- /dev/null +++ b/src/camera/package.xml @@ -0,0 +1,21 @@ + + + + camera + 0.0.0 + Package for controlling the camera of the drone + ubuntu + Apache License 2.0 + + rclpy + drone_services + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/src/camera/resource/camera b/src/camera/resource/camera new file mode 100644 index 00000000..e69de29b diff --git a/src/camera/setup.cfg b/src/camera/setup.cfg new file mode 100644 index 00000000..19b8cf92 --- /dev/null +++ b/src/camera/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/camera +[install] +install-scripts=$base/lib/camera diff --git a/src/camera/setup.py b/src/camera/setup.py new file mode 100644 index 00000000..18ec34f5 --- /dev/null +++ b/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' + ], + }, +) diff --git a/src/camera/test/test_copyright.py b/src/camera/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/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' diff --git a/src/camera/test/test_flake8.py b/src/camera/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/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) diff --git a/src/camera/test/test_pep257.py b/src/camera/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/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' diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index d63e092b..00b7c0fa 100644 --- a/src/drone_services/CMakeLists.txt +++ b/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) diff --git a/src/drone_services/srv/SetVehicleControl.srv b/src/drone_services/srv/SetVehicleControl.srv new file mode 100644 index 00000000..0be842e9 --- /dev/null +++ b/src/drone_services/srv/SetVehicleControl.srv @@ -0,0 +1,3 @@ +int32 control # control bitmask +--- +int8 status # status of operation \ No newline at end of file diff --git a/src/drone_services/srv/TakePicture.srv b/src/drone_services/srv/TakePicture.srv new file mode 100644 index 00000000..e043283c --- /dev/null +++ b/src/drone_services/srv/TakePicture.srv @@ -0,0 +1,3 @@ +wstring input_name "default" # name of the input file +--- +wstring filename # output file name \ No newline at end of file diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp index 265d35a2..85faa854 100644 --- a/src/object_detection/src/lidar_reader.cpp +++ b/src/object_detection/src/lidar_reader.cpp @@ -1,8 +1,4 @@ #include -#include -#include -#include -#include #include "rclcpp/rclcpp.hpp" #include "object_detection/msg/lidar_reading.hpp" diff --git a/src/object_detection/src/multiflex_reader.cpp b/src/object_detection/src/multiflex_reader.cpp index f2e92272..a134f2a5 100644 --- a/src/object_detection/src/multiflex_reader.cpp +++ b/src/object_detection/src/multiflex_reader.cpp @@ -1,8 +1,4 @@ #include -#include -#include -#include -#include #include "rclcpp/rclcpp.hpp" #include "object_detection/msg/multiflex_reading.hpp" @@ -11,6 +7,8 @@ #include #include +// #include + 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 diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py index 9ed15c26..312780bb 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/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 diff --git a/thefuck.txt b/thefuck.txt new file mode 100644 index 00000000..e69de29b diff --git a/web-cam-shot.jpg b/web-cam-shot.jpg new file mode 100644 index 00000000..00180c9c Binary files /dev/null and b/web-cam-shot.jpg differ