diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 2ce1dddf..6743b427 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -11,7 +11,8 @@ "/usr/include/**", "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/rtls_driver/include/**", "/mnt/Homework/Avans/AFSTUDEERSTAGE/positioning_systems_api/serial_communication/include/**", - "/mnt/Homework/Avans/AFSTUDEERSTAGE/terabee_api/include/**" + "/mnt/Homework/Avans/AFSTUDEERSTAGE/terabee_api/include/**", + "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**" ], "name": "ROS", "intelliSenseMode": "gcc-x64", diff --git a/get_build.sh b/get_build.sh new file mode 100755 index 00000000..4686bb99 --- /dev/null +++ b/get_build.sh @@ -0,0 +1,7 @@ +#!/bin/bash +echo "updating git..." +./fgit.sh +echo "building package..." +colcon build --packages-select px4_connection +#echo "launching controller..." +#ros2 run px4_connection px4_controller diff --git a/src/drone_controls/CMakeLists.txt b/src/drone_controls/CMakeLists.txt new file mode 100644 index 00000000..cc182294 --- /dev/null +++ b/src/drone_controls/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.5) +project(drone_controls) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/drone_controls/package.xml b/src/drone_controls/package.xml new file mode 100644 index 00000000..5d264637 --- /dev/null +++ b/src/drone_controls/package.xml @@ -0,0 +1,20 @@ + + + + drone_controls + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt new file mode 100644 index 00000000..d63e092b --- /dev/null +++ b/src/drone_services/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) +project(drone_services) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SetAttitude.srv" + "srv/SetVelocity.srv" +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/drone_services/package.xml b/src/drone_services/package.xml new file mode 100644 index 00000000..af4dc178 --- /dev/null +++ b/src/drone_services/package.xml @@ -0,0 +1,23 @@ + + + + drone_services + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + + rclcpp + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/drone_services/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv new file mode 100644 index 00000000..52957c0c --- /dev/null +++ b/src/drone_services/srv/SetAttitude.srv @@ -0,0 +1,9 @@ +#all angles are in degrees +float32 yaw +float32 pitch +float32 roll + +#thrust between -1 and 1 +float32 thrust +--- +int8 status diff --git a/src/drone_services/srv/SetVelocity.srv b/src/drone_services/srv/SetVelocity.srv new file mode 100644 index 00000000..0d85cba9 --- /dev/null +++ b/src/drone_services/srv/SetVelocity.srv @@ -0,0 +1,9 @@ +#all speeds are in meters/second +float32 x_speed +float32 y_speed +float32 z_speed + +#angle is in degrees +float32 angle +--- +int8 status diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index d8f70b06..1df63c2d 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -1,5 +1,10 @@ +/** + * @file height_reader.cpp + * @author Sem van der Hoeven (sem.hoeven@gmail.com) + * @brief Uses the Terabee API to read the Terabee Evo Mini + * height sensor data and publishes it on a /drone/height topic + */ #include -#include #include "rclcpp/rclcpp.hpp" #include "height/msg/height_data.hpp" @@ -13,85 +18,84 @@ using namespace std::chrono_literals; using terabee::DistanceData; using terabee::ITerarangerEvoMini; -std::ostream &operator<<(std::ostream &os, const DistanceData &d) -{ - os << "["; - for (size_t i = 0; i < d.distance.size(); i++) - { - os << d.distance[i] << ", "; - } - os << "\b\b" - << " ]"; - return os; -} - class HeightReader : public rclcpp::Node { public: - HeightReader() : rclcpp::Node("height_reader") - { - rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - descriptor.description = "serial port for the USB port of the height sensor"; - - this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); - - factory = terabee::ITerarangerFactory::getFactory(); - evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); - - if (!evo_mini) + HeightReader() : rclcpp::Node("height_reader") { - RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); - return; + rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + descriptor.description = "serial port for the USB port of the height sensor"; + + this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); + + factory = terabee::ITerarangerFactory::getFactory(); + evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); + + if (!evo_mini) + { + RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); + return; + } + + evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); + + if (!evo_mini->initialize()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); + + timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); + publisher_ = this->create_publisher("drone/height", 10); } - evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); - - if (!evo_mini->initialize()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); - return; - } - - RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); - - timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); - publisher_ = this->create_publisher("drone/height", 10); - } - private: - void read_height() - { - auto msg = height::msg::HeightData(); - - float min = 255; - terabee::DistanceData distance_data = evo_mini->getDistance(); - for (size_t i = 0; i < distance_data.size(); i++) + /** + * @brief reads the height value from the Terabee Evo Mini sensor and publishes a + * + */ + void read_height() { - msg.heights[i] = distance_data.distance[i]; - if (distance_data.distance[i] < min && distance_data.distance[i] >= 0) - { - min = distance_data.distance[i]; - } + auto msg = height::msg::HeightData(); + + // high initial minimal value + float min = 255; + terabee::DistanceData distance_data = evo_mini->getDistance(); + // add readings and calculate mimimum value + for (size_t i = 0; i < distance_data.size(); i++) + { + msg.heights[i] = distance_data.distance[i]; + if (distance_data.distance[i] < min && distance_data.distance[i] >= 0) + { + min = distance_data.distance[i]; + } + } + // add minimum value and publish message + msg.min_height = min; + publisher_->publish(msg); + + RCLCPP_INFO(this->get_logger(), "publishing message with min distance %f", msg.min_height); } - msg.min_height = min; - publisher_->publish(msg); - RCLCPP_INFO(this->get_logger(),"publishing message with min distance %f",msg.min_height); - } + // timer for publishing height readings + rclcpp::TimerBase::SharedPtr timer_; + // publisher for height readings + rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - - std::unique_ptr factory; - std::unique_ptr evo_mini; + // factory for height sensor + std::unique_ptr factory; + // height sensor object pointer + std::unique_ptr evo_mini; }; int main(int argc, char **argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); - return 0; + return 0; } diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp index eaa98bd8..265d35a2 100644 --- a/src/object_detection/src/lidar_reader.cpp +++ b/src/object_detection/src/lidar_reader.cpp @@ -18,104 +18,81 @@ using terabee::TowerDistanceData; using namespace std::chrono_literals; -/* This example creates a subclass of Node and uses std::bind() to register a - * member function as a callback from the timer. */ - -std::ostream &operator<<(std::ostream &os, const TowerDistanceData &d) -{ - os << "["; - for (size_t i = 0; i < d.distance.size(); i++) - { - os << i << " " << d.distance[i] << (d.mask[i] ? " , " : " , "); - } - os << "\b\b" - << " ]"; - return os; -} - -std::ostream &operator<<(std::ostream &os, const ImuData &d) -{ - os << "["; - for (size_t i = 0; i < d.data.size(); i++) - { - os << d.data[i] << ", "; - } - os << "\b\b" - << " ]"; - return os; -} - class LidarReader : public rclcpp::Node { public: - LidarReader() - : Node("lidar_reader") - { - - rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - descriptor.description = "serial port for the USB port of the LIDAR"; - - this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor); - - // publisher_ = this->create_publisher("drone/object_detection", 10); - timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); - - ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc); - - factory = terabee::ITerarangerFactory::getFactory(); - tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string()); - - if (!tower) + LidarReader() + : Node("lidar_reader") { - RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo"); - return; - } + // get serial port of sensor through parameter + rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + descriptor.description = "serial port for the USB port of the LIDAR"; + this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor); - tower->setImuMode(mode); + // create publisher and bind timer to publish function + publisher_ = this->create_publisher("drone/object_detection", 10); + timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); - if (!tower->initialize()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo"); - return; + factory = terabee::ITerarangerFactory::getFactory(); + tower = factory->createTerarangerTowerEvo(this->get_parameter("lidar_serial_port").as_string()); + + if (!tower) // check if the object could be created + { + RCLCPP_ERROR(this->get_logger(), "Failed to create TerarangerTowerEvo"); + return; + } + + // set lidar to measure including IMU + tower->setImuMode(ITerarangerTowerEvo::QuaternionLinearAcc); + + if (!tower->initialize()) // check if communication with the sensor works + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize TerarangerTowerEvo"); + return; + } } - } private: - void read_lidar_data() - { - //TODO publish message with all data from lidar - // std::cout << "Distance = " << tower->getDistance() << std::endl; - // std::cout << "IMU = " << tower->getImuData() << std::endl; + // publisher for lidar data + rclcpp::Publisher::SharedPtr publisher_; + // timer for publishing readings + rclcpp::TimerBase::SharedPtr timer_; - // auto msg = object_detection::msg::LidarReading(); + // terabee tower evo variables + std::unique_ptr tower; + std::unique_ptr factory; - // msg.sensor_1 = tower->getDistance().distance.at(0); - // msg.sensor_2 = tower->getDistance().distance.at(2); - // msg.sensor_3 = tower->getDistance().distance.at(4); - // msg.sensor_4 = tower->getDistance().distance.at(6); + /** + * @brief Reads the data from the LIDAR distance modules and the IMU, and publishes it onto the drone/object_detection topic + * + */ + void read_lidar_data() + { + auto msg = object_detection::msg::LidarReading(); - // ImuData imu_data = tower->getImuData(); + // read distance data from all sensors + msg.sensor_1 = tower->getDistance().distance.at(0); + msg.sensor_2 = tower->getDistance().distance.at(2); + msg.sensor_3 = tower->getDistance().distance.at(4); + msg.sensor_4 = tower->getDistance().distance.at(6); - // for (size_t i = 0; i < imu_data.data.size(); i++) - // { - // msg.imu_data.push_back(imu_data.data[i]); - // } - // // publisher_->publish(msg); - // RCLCPP_INFO(this->get_logger(), "Publishing message"); - } + // read data from built-in IMU + ImuData imu_data = tower->getImuData(); + for (size_t i = 0; i < imu_data.data.size(); i++) + { + msg.imu_data.push_back(imu_data.data[i]); + } - // rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; - - // terabee tower evo variables - std::unique_ptr tower; - std::unique_ptr factory; + // publish message + publisher_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Publishing message"); + } }; int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/src/object_detection/src/multiflex_reader.cpp b/src/object_detection/src/multiflex_reader.cpp index b92b3db6..f2e92272 100644 --- a/src/object_detection/src/multiflex_reader.cpp +++ b/src/object_detection/src/multiflex_reader.cpp @@ -18,64 +18,76 @@ using namespace std::chrono_literals; class MultiflexReader : public rclcpp::Node { public: - MultiflexReader() - : Node("multiflex_reader") - { - rcl_interfaces::msg::ParameterDescriptor serial_port_descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - serial_port_descriptor.description = "Serial port of the USB port that the multiflex PCB is connected to."; - this->declare_parameter("multiflex_serial_port", "/dev/ttyACM0", serial_port_descriptor); - - factory = terabee::ITerarangerFactory::getFactory(); - multiflex = factory->createTerarangerMultiflex(this->get_parameter("multiflex_serial_port").as_string()); - - if (!multiflex) + MultiflexReader() + : Node("multiflex_reader") { - RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex"); - return; + // get serial port of sensor through parameter + rcl_interfaces::msg::ParameterDescriptor serial_port_descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + serial_port_descriptor.description = "Serial port of the USB port that the multiflex PCB is connected to."; + this->declare_parameter("multiflex_serial_port", "/dev/ttyACM0", serial_port_descriptor); + + // create the sensor object + factory = terabee::ITerarangerFactory::getFactory(); + multiflex = factory->createTerarangerMultiflex(this->get_parameter("multiflex_serial_port").as_string()); + + if (!multiflex) // check if the object can be created + { + RCLCPP_ERROR(this->get_logger(), "Failed to create multiflex"); + return; + } + + if (!multiflex->initialize()) // check if communication with the sensor works + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize multiflex"); + return; + } + + if (!multiflex->configureNumberOfSensors(0x3f)) // check if all 6 distance sensors work + { + RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Multiflex initialized"); + + publisher_ = this->create_publisher("drone/object_detection", 10); + timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this)); } - if (!multiflex->initialize()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize multiflex"); - return; - } - - if (!multiflex->configureNumberOfSensors(0x3f)) - { - RCLCPP_ERROR(this->get_logger(), "Failed to set the number of sensors to 6!"); - return; - } - - RCLCPP_INFO(this->get_logger(), "Multiflex initialized"); - - - publisher_ = this->create_publisher("drone/object_detection", 10); - timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this)); - } - private: - void read_multiflex_data() - { - terabee::DistanceData data = multiflex->getDistance(); - auto msg = object_detection::msg::MultiflexReading(); - for (size_t i = 0; i < data.size(); i++) + // factory and object for multiflex sensor + std::unique_ptr factory; + std::unique_ptr multiflex; + + // timer and publisher for publishing message onto topic + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + + /** + * @brief Reads distance data from the sensor and publishes it onto the drone/object_detection topic + * + */ + void read_multiflex_data() { - msg.distance_data[i] = data.distance[i]; + // get distance reading + terabee::DistanceData data = multiflex->getDistance(); + + // populate message with readings + auto msg = object_detection::msg::MultiflexReading(); + for (size_t i = 0; i < data.size(); i++) + { + msg.distance_data[i] = data.distance[i]; + } + + // publish message + publisher_->publish(msg); } - publisher_->publish(msg); - } - - std::unique_ptr factory; - std::unique_ptr multiflex; - - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; }; int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/src/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt new file mode 100644 index 00000000..638d46f7 --- /dev/null +++ b/src/px4_connection/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.5) +project(px4_connection) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_ros_com REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(drone_services REQUIRED) +find_package(std_srvs REQUIRED) + +include_directories(include) +add_executable(heartbeat src/heartbeat.cpp) +ament_target_dependencies( + heartbeat + rclcpp + px4_ros_com + px4_msgs +) + +add_executable(px4_controller src/px4_controller.cpp) +ament_target_dependencies( + px4_controller + rclcpp + px4_ros_com + px4_msgs + drone_services + std_srvs +) + +target_include_directories(heartbeat PUBLIC + $ + $) + +target_include_directories(px4_controller PUBLIC + $ + $) + +install(TARGETS heartbeat px4_controller + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + # set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + # set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/px4_connection/include/attitude_msg_code.hpp b/src/px4_connection/include/attitude_msg_code.hpp new file mode 100644 index 00000000..2a54b920 --- /dev/null +++ b/src/px4_connection/include/attitude_msg_code.hpp @@ -0,0 +1,8 @@ +#ifndef ATTITUDE_MSG_CODE_HPP +#define ATTITUDE_MSG_CODE_HPP + +#define ATTITUDE_STATUS_OK 0 +#define ATTITUDE_STATUS_ERROR 1 + + +#endif \ No newline at end of file diff --git a/src/px4_connection/launch/px4_controller_heardbeat_launch.py b/src/px4_connection/launch/px4_controller_heardbeat_launch.py new file mode 100644 index 00000000..d0b733ca --- /dev/null +++ b/src/px4_connection/launch/px4_controller_heardbeat_launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="px4_connection", + executable="heartbeat" + ), + Node( + package="px4_connection", + executable="px4_controller" + ) + + ]) diff --git a/src/px4_connection/package.xml b/src/px4_connection/package.xml new file mode 100644 index 00000000..604e89b2 --- /dev/null +++ b/src/px4_connection/package.xml @@ -0,0 +1,24 @@ + + + + px4_connection + 0.0.0 + Package to communicate with PX4 through the XRCE-DDS bridge + ubuntu + Apache License 2.0 + + ament_cmake + + rclcpp + px4_ros_com + px4_msgs + drone_services + std_srvs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp new file mode 100644 index 00000000..0376e8dd --- /dev/null +++ b/src/px4_connection/src/heartbeat.cpp @@ -0,0 +1,62 @@ +/** + * @file heartbeat.cpp + * @author Sem van der Hoeven (sem.hoeven@gmail.com) + * @brief Heartbeat node that keeps the connection between the flight computer + * and PX4 flight controller alive by sending OffboardControl messages + */ +#include +#include "rclcpp/rclcpp.hpp" +#include + +using namespace std::chrono_literals; + +class HeartBeat : public rclcpp::Node +{ +public: + HeartBeat() : Node("heartbeat") + { + // create a publisher on the offboard control mode topic + offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + // create timer to send heartbeat messages (offboard control) every 100ms + timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this)); + start_time = this->get_clock()->now().seconds(); + RCLCPP_INFO(this->get_logger(), "done initializing at %d seconds. Sending heartbeat...", start_time); + } + +private: + // publisher for offboard control mode messages + rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + // timer for sending the heartbeat + rclcpp::TimerBase::SharedPtr timer_; + // start time of node in seconds + double start_time; + + /** + * @brief Publish offboard control mode messages as a heartbeat. + * Only the attitude is enabled, because that is how the drone will be controlled. + * + */ + void send_heartbeat() + { + // set message to enable attitude + auto msg = px4_msgs::msg::OffboardControlMode(); + msg.position = false; + msg.velocity = false; + msg.acceleration = false; + msg.attitude = true; + msg.body_rate = false; + msg.actuator = false; + + // get timestamp and publish message + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + offboard_control_mode_publisher_->publish(msg); + } +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp new file mode 100644 index 00000000..9f28763c --- /dev/null +++ b/src/px4_connection/src/px4_controller.cpp @@ -0,0 +1,355 @@ +/** + * @file px4_controller.cpp + * @author Sem van der Hoeven (sem.hoeven@gmail.com) + * @brief Controller node to contol the PX4 using attitude or trajectory setpoints. + * It subscribes to the /drone/set_attitude topic to receive control commands + */ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +#define D_SPEED(x) -x - 9.81 + +using namespace std::chrono_literals; + +class PX4Controller : public rclcpp::Node +{ +public: + PX4Controller() : Node("px4_controller") + { + + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); + + // create a publisher on the vehicle attitude setpoint topic + vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); + vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); + trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); + // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + + vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1)); + + set_attitude_service_ = this->create_service("drone/set_attitude", std::bind(&PX4Controller::set_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + // create timer to send vehicle attitude setpoints every second + timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); + + start_time_ = this->get_clock()->now().seconds(); + RCLCPP_INFO(this->get_logger(), "finished initializing at %f seconds.", start_time_); + } + +private: + rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; + rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; + rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + rclcpp::Subscription::SharedPtr vehicle_attitude_subscription_; + + rclcpp::Service::SharedPtr set_attitude_service_; + rclcpp::Service::SharedPtr disarm_service_; + + // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + + rclcpp::TimerBase::SharedPtr timer_; + double start_time_; + bool has_sent_status = false; + bool flying = false; + bool armed = false; + bool has_swithed = false; + int setpoint_count = 0; + float thrust = 0.0; + bool ready_to_fly = false; + float cur_yaw = 0; + bool new_setpoint = false; // for printing new q_d when a new setpoint has been received + + float last_setpoint[3] = {0, 0, 0}; + float last_angle = 0; + float last_thrust = 0; + + float base_q[4] = {0, 0, 0, 0}; + int base_q_amount = 0; + + // result quaternion + std::array q = {0, 0, 0, 0}; + + void set_setpoint( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + if (armed) + { + if (request->yaw == 0 && request->pitch == 0 && request->roll == 0 && request->thrust == 0) + { + last_setpoint[0] = degrees_to_radians(request->yaw); + last_setpoint[1] = degrees_to_radians(request->pitch); + last_setpoint[2] = degrees_to_radians(request->roll); + last_thrust = request->thrust; + RCLCPP_INFO(this->get_logger(), "STOPPING MOTORS"); + } + else + { + last_setpoint[0] += degrees_to_radians(request->yaw); + last_setpoint[1] += degrees_to_radians(request->pitch); + last_setpoint[2] += degrees_to_radians(request->roll); + last_thrust += request->thrust; + if (last_thrust > 1) + last_thrust = 1; + else if (last_thrust < 0) + last_thrust = 0; + } + + RCLCPP_INFO(this->get_logger(), "got values: yaw:%f pitch:%f roll:%f thrust:%f", request->yaw, request->pitch, request->roll, request->thrust); + RCLCPP_INFO(this->get_logger(), "New setpoint: yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); + new_setpoint = true; + + response->status = 0; + } + else + { + last_thrust = 0; + last_setpoint[0] = 0; + last_setpoint[1] = 0; + last_setpoint[2] = 0; + response->status = 1; + } + } + + /** + * @brief disarms the drone when a call to the disarm service is made + * + * @param request_header the header for the service request + * @param request the request (empty) + * @param response the response (empty) + */ + void handle_disarm_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + RCLCPP_INFO(this->get_logger(), "Got disarm request..."); + + if (armed) + { + armed = false; + publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + } + } + + void send_trajectory_setpoint() + { + + auto msg = px4_msgs::msg::TrajectorySetpoint(); + if (this->get_clock()->now().seconds() - start_time_ < 10) + { + msg.acceleration[0] = 0; + msg.acceleration[1] = 0; + msg.acceleration[2] = D_SPEED(10); + msg.yawspeed = 0; + msg.yaw = -3.14; + } + else + { + if (!has_swithed) + { + RCLCPP_INFO(this->get_logger(), "waiting for service input..."); + has_swithed = true; + } + + msg.acceleration[0] = last_setpoint[0]; + msg.acceleration[1] = last_setpoint[1]; + msg.acceleration[2] = D_SPEED(last_setpoint[2]); + msg.yawspeed = 0.5; + } + + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + + trajectory_setpoint_publisher->publish(msg); + } + + void send_attitude_setpoint() + { + + // set message to enable attitude + auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); + + // if (this->get_clock()->now().seconds() - start_time_ < 30) + // { + // msg.yaw_body = last_setpoint[0]; + // msg.pitch_body = last_setpoint[1]; + // msg.roll_body = last_setpoint[2]; + // move up? + msg.thrust_body[0] = 0; // north + msg.thrust_body[1] = 0; // east + msg.thrust_body[2] = -last_thrust; + + calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); + // RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", q.at(0), q.at(1), q.at(2), last_thrust); + // } + // else + // { + // if (flying) + // { + // publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0, 0); + // flying = false; + // RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds"); + // } + // } + + // set quaternion + msg.q_d[0] = q.at(0); + msg.q_d[1] = base_q[1] + q.at(1); + msg.q_d[2] = base_q[2] + q.at(2); + msg.q_d[3] = base_q[3] + q.at(3); + + if (new_setpoint) + { + RCLCPP_INFO(this->get_logger(), "q_d: [%f, %f, %f, %f]", msg.q_d[0], msg.q_d[1], msg.q_d[2], msg.q_d[3]); + new_setpoint = false; + } + + msg.yaw_sp_move_rate = 0; + msg.reset_integral = false; + msg.fw_control_yaw_wheel = false; + + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + vehicle_setpoint_publisher_->publish(msg); + } + + /** + * @brief Send setpoints to PX4. First, 20 setpoints will be sent before offboard mode will be engaged and the drone will be armed. + * + */ + void send_setpoint() + { + // increase amount of setpoints sent + setpoint_count++; + + ready_to_fly = setpoint_count == 20; + // after 20 setpoints, send arm command to make drone actually fly + if (ready_to_fly) + { + // switch to offboard mode and arm + + // set to offboard mode + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); + RCLCPP_INFO(this->get_logger(), "Set to offboard mode"); + // arm the drone + this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0); + + RCLCPP_INFO(this->get_logger(), "Arm command sent"); + flying = true; + armed = true; + } + + send_attitude_setpoint(); + } + + void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) + { + /* + average q: + - 0.7313786745071411 + - 0.005042835604399443 + - 0.0002370707516092807 + - 0.6819528937339783 + */ + if (!armed) + { + if (base_q_amount > 2) + { + base_q[1] = (base_q[1] + msg->q[1]) / 2; + base_q[2] = (base_q[2] + msg->q[2]) / 2; + base_q[3] = (base_q[3] + msg->q[3]) / 2; + } else { + base_q[1] = msg->q[1]; + base_q[2] = msg->q[2]; + base_q[3] = msg->q[3]; + base_q_amount++; + } + + RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]); + } + } + + /** + * @brief Publish vehicle commands + * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) + * @param param1 Command parameter 1 + * @param param2 Command parameter 2 + */ + void publish_vehicle_command(uint16_t command, float param1, float param2) + { + px4_msgs::msg::VehicleCommand msg{}; + msg.param1 = param1; + msg.param2 = param2; + msg.command = command; + msg.target_system = 1; + msg.target_component = 1; + msg.source_system = 1; + msg.source_component = 1; + msg.from_external = true; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + vehicle_command_publisher_->publish(msg); + } + + /** + * @brief calculates a quaternion based on the given input values + * + * @param ptr array of result quaternion. Must be of size 4. + * @param heading desired heading (yaw) in radians. + * @param attitude desired attitude (pitch) in radians. + * @param bank desired bank (roll) in radians. + */ + static void calculate_quaternion(std::array &ptr, float heading, float attitude, float bank) + { + + float c1 = cos(heading / 2); + float c2 = cos(attitude / 2); + float c3 = cos(bank / 2); + float s1 = sin(heading / 2); + float s2 = sin(attitude / 2); + float s3 = sin(bank / 2); + + float c1c2 = c1 * c2; + float s1s2 = s1 * s2; + + ptr.at(0) = c1c2 * c3 - s1s2 * s3; // w + ptr.at(1) = c1c2 * s3 + s1s2 * c3; // x + ptr.at(2) = s1 * c2 * c3 + c1 * s2 * s3; // y + ptr.at(3) = c1 * s2 * c3 - s1 * c2 * s3; // z + } + + /** + * @brief converts degrees to radians + * + * @param deg angle in degrees + * @return float new angle in radians + */ + static float degrees_to_radians(float deg) + { + return deg * (M_PI / 180.0); + } +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::Node::SharedPtr node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/px4_msgs b/src/px4_msgs index 4db0a3f1..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit 4db0a3f14ea81b9de7511d738f8ad9bd8ae5b3ad +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4 diff --git a/src/test_controls/package.xml b/src/test_controls/package.xml new file mode 100644 index 00000000..9904e9d7 --- /dev/null +++ b/src/test_controls/package.xml @@ -0,0 +1,21 @@ + + + + test_controls + 0.0.0 + test controls + ubuntu + Apache License 2.0 + + rclpy + drone_services + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/test_controls/resource/test_controls b/src/test_controls/resource/test_controls new file mode 100644 index 00000000..e69de29b diff --git a/src/test_controls/setup.cfg b/src/test_controls/setup.cfg new file mode 100644 index 00000000..5dc0dac5 --- /dev/null +++ b/src/test_controls/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/test_controls +[install] +install-scripts=$base/lib/test_controls diff --git a/src/test_controls/setup.py b/src/test_controls/setup.py new file mode 100644 index 00000000..a521d2fb --- /dev/null +++ b/src/test_controls/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'test_controls' + +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='test controls', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'test_controller = test_controls.test_controller:main' + ], + }, +) diff --git a/src/test_controls/test/test_copyright.py b/src/test_controls/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/src/test_controls/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/test_controls/test/test_flake8.py b/src/test_controls/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/test_controls/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/test_controls/test/test_pep257.py b/src/test_controls/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/test_controls/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/test_controls/test_controls/__init__.py b/src/test_controls/test_controls/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/test_controls/test_controls/test_controller.py b/src/test_controls/test_controls/test_controller.py new file mode 100644 index 00000000..9ed15c26 --- /dev/null +++ b/src/test_controls/test_controls/test_controller.py @@ -0,0 +1,118 @@ +import rclpy +from rclpy.node import Node + +from sshkeyboard import listen_keyboard_manual +import asyncio + +from drone_services.srv import SetAttitude + + +class TestController(Node): + + def __init__(self): + super().__init__('test_controller') + self.cli = self.create_client(SetAttitude, 'drone/set_attitude') + while not self.cli.wait_for_service(timeout_sec=1.0): + 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") + + def spin(self): + while rclpy.ok(): + asyncio.run(listen_keyboard_manual(on_press=self.on_press)) + rclpy.spin_once(self, timeout_sec=0.1) + + def send_request(self, yaw, pitch, roll, thrust): + self.req.yaw = yaw + self.req.pitch = pitch + self.req.roll = roll + self.req.thrust = thrust + self.get_logger().info('set request to %f %f %f %f' % (yaw, pitch, roll, thrust)) + self.future = self.cli.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message on service') + return self.future.result() + + def on_release(self, key): + # self.get_logger().info('released ' + str(key)) + pass + + def on_press(self, key): + try: + # self.get_logger().info('pressed ' + char) + if key == 'w': + self.get_logger().info('forward') + self.send_request(pitch=-10.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, + 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) + if key == 'd': + self.get_logger().info('right') + self.send_request(pitch=0.0, yaw=0.0, + roll=10.0, thrust=0.0) + if key == 'q': + self.get_logger().info('rotate left') + self.send_request(pitch=0.0, yaw=-10.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, + roll=0.0, thrust=0.0) + if key == 'z': + self.get_logger().info('down') + self.send_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.05) + if key == 'space': + self.get_logger().info('up') + self.send_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.05) + 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 + # name = key.name + # vk = key.value.vk + # except AttributeError: + # # unknown keys like headphones skip song button + # name = 'UNKNOWN' + # vk = key.vk + # # self.get_logger().info('pressed {} ({})'.format(name, vk)) + # if vk == 32: + # self.get_logger().info('up') + # self.send_request(pitch=0.0, yaw=0.0, roll=0.0, thrust=0.05) + # if vk == 65505: + # self.get_logger().info('down') + # self.send_request(pitch=0.0, yaw=0.0, + # roll=0.0, thrust=-0.05) + + except Exception as e: + self.get_logger().error(str(e)) + raise e + + +def main(args=None): + rclpy.init(args=args) + + test_controller = TestController() + + test_controller.spin() + + # 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()