diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 6743b427..b8851067 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -12,7 +12,8 @@ "/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/**", - "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**" + "/home/ubuntu/ros2_ws/build/px4_msgs/rosidl_generator_cpp/px4_msgs/msg/**", + "/home/ubuntu/ros2_ws/src/px4_connection/include/**" ], "name": "ROS", "intelliSenseMode": "gcc-x64", diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 00b7c0fa..130f0f65 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -22,9 +22,14 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetAttitude.srv" + "srv/SetTrajectory.srv" "srv/SetVelocity.srv" "srv/TakePicture.srv" "srv/SetVehicleControl.srv" + "srv/ArmDrone.srv" + "srv/DisarmDrone.srv" + "srv/ControlRelais.srv" + "msg/DroneControlMode.msg" ) if(BUILD_TESTING) diff --git a/src/drone_services/msg/DroneArmStatus.msg b/src/drone_services/msg/DroneArmStatus.msg new file mode 100644 index 00000000..117ae05f --- /dev/null +++ b/src/drone_services/msg/DroneArmStatus.msg @@ -0,0 +1 @@ +bool armed false \ No newline at end of file diff --git a/src/drone_services/msg/DroneControlMode.msg b/src/drone_services/msg/DroneControlMode.msg new file mode 100644 index 00000000..92669b9e --- /dev/null +++ b/src/drone_services/msg/DroneControlMode.msg @@ -0,0 +1,5 @@ +# control mode of the drone +# 1: Attitude +# 2: Velocity +# 3: Position +int8 control_mode \ No newline at end of file diff --git a/src/drone_services/msg/DroneRouteStatus.msg b/src/drone_services/msg/DroneRouteStatus.msg new file mode 100644 index 00000000..bcf2a7c0 --- /dev/null +++ b/src/drone_services/msg/DroneRouteStatus.msg @@ -0,0 +1,2 @@ +int32 current_setpoint_index +float32[5] current_setpoint # x,y,z,angle,take_picture \ No newline at end of file diff --git a/src/drone_services/msg/DroneStatus.msg b/src/drone_services/msg/DroneStatus.msg new file mode 100644 index 00000000..0489be52 --- /dev/null +++ b/src/drone_services/msg/DroneStatus.msg @@ -0,0 +1,5 @@ +float32 battery_percentage +float32 cpu_usage +int32 route_setpoint # -1 if no route +wstring control_mode +bool armed \ No newline at end of file diff --git a/src/drone_services/srv/ArmDrone.srv b/src/drone_services/srv/ArmDrone.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/src/drone_services/srv/ArmDrone.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file diff --git a/src/drone_services/srv/ControlRelais.srv b/src/drone_services/srv/ControlRelais.srv new file mode 100644 index 00000000..805fcd89 --- /dev/null +++ b/src/drone_services/srv/ControlRelais.srv @@ -0,0 +1,5 @@ +#to turn the relais on or off +bool relais1_on false +bool relais2_on false +--- +int8 bits # relais 1 = bit 0, relais 2 is bit 1 \ No newline at end of file diff --git a/src/drone_services/srv/DisarmDrone.srv b/src/drone_services/srv/DisarmDrone.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/src/drone_services/srv/DisarmDrone.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file diff --git a/src/drone_services/srv/SetAttitude.srv b/src/drone_services/srv/SetAttitude.srv index 52957c0c..03a30ec9 100644 --- a/src/drone_services/srv/SetAttitude.srv +++ b/src/drone_services/srv/SetAttitude.srv @@ -1,3 +1,5 @@ +#service to set attitude setpoints for manual attitude control + #all angles are in degrees float32 yaw float32 pitch @@ -6,4 +8,4 @@ float32 roll #thrust between -1 and 1 float32 thrust --- -int8 status +bool success diff --git a/src/drone_services/srv/SetTrajectory.srv b/src/drone_services/srv/SetTrajectory.srv new file mode 100644 index 00000000..17058ee1 --- /dev/null +++ b/src/drone_services/srv/SetTrajectory.srv @@ -0,0 +1,7 @@ +# control mode velocity or position, refer to drone_services/msg/DroneControlMode.msg +int8 control_mode +# x, y, z values +float32[3] values +float32 yaw +--- +bool success \ No newline at end of file diff --git a/src/drone_services/srv/SetVehicleControl.srv b/src/drone_services/srv/SetVehicleControl.srv index c0dd2411..3ab33ea3 100644 --- a/src/drone_services/srv/SetVehicleControl.srv +++ b/src/drone_services/srv/SetVehicleControl.srv @@ -1,3 +1,4 @@ +# service to set control mode for heartbeat messages # bitmask for control: # bit 0: actuator # bit 1: body rate @@ -6,6 +7,10 @@ # bit 4: velocity # bit 5: position +# used control modes: +# 4: attitude and thrust +# 16: velocity +# 32: position int32 control # control bitmask --- -int8 status # status of operation \ No newline at end of file +bool success # if operation succeeded \ No newline at end of file diff --git a/src/drone_services/srv/SetVelocity.srv b/src/drone_services/srv/SetVelocity.srv index 0d85cba9..6602ab3e 100644 --- a/src/drone_services/srv/SetVelocity.srv +++ b/src/drone_services/srv/SetVelocity.srv @@ -6,4 +6,4 @@ float32 z_speed #angle is in degrees float32 angle --- -int8 status +bool success diff --git a/src/drone_status/drone_status/__init__.py b/src/drone_status/drone_status/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/drone_status/drone_status/drone_status.py b/src/drone_status/drone_status/drone_status.py new file mode 100644 index 00000000..ab8046af --- /dev/null +++ b/src/drone_status/drone_status/drone_status.py @@ -0,0 +1,63 @@ +import rclpy +from rclpy.node import Node + +from drone_services.msg import DroneStatus +from drone_services.msg import DroneControlMode +from drone_services.msg import DroneArmStatus +from drone_services.msg import DroneRouteStatus +from px4_msgs.msg import BatteryStatus +from px4_msgs.msg import Cpuload + +CONTROL_MODE_ATTITUDE = 1 +CONTROL_MODE_VELOCITY = 2 +CONTROL_MODE_POSITION = 3 + +class DroneStatus(Node): + def __init__(self): + super().__init__('drone_status') + #publish to drone/status topic + self.publisher = self.create_publisher(DroneStatus, '/drone/status', 10) + self.control_mode_subscriber = self.create_subscription(DroneControlMode, '/drone/control_mode', self.control_mode_callback, 10) + self.arm_status_subscriber = self.create_subscription(DroneArmStatus, '/drone/arm_status', self.arm_status_callback, 10) + self.route_status_subscriber = self.create_subscription(DroneRouteStatus, '/drone/route_status', self.route_status_callback, 10) + self.battery_status_subscriber = self.create_subscription(BatteryStatus, '/fmu/out/battery_status', self.battery_status_callback, 10) + self.cpu_load_subscriber = self.create_subscription(Cpuload, '/fmu/out/cpuload', self.cpu_load_callback, 10) + #publish every 0.5 seconds + self.timer = self.create_timer(0.5, self.publish_status) + self.armed = False + self.control_mode = "attitude" + self.battery_percentage = 100.0 + self.cpu_usage = 0.0 + self.route_setpoint = 0 + + def publish_status(self): + msg = DroneStatus() + msg.armed = self.armed + msg.control_mode = self.control_mode + msg.battery_percentage = self.battery_percentage + msg.cpu_usage = self.cpu_usage + msg.route_setpoint = self.route_setpoint + self.publisher.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.status) + + def control_mode_callback(self,msg): + if msg.control_mode == CONTROL_MODE_ATTITUDE: + self.control_mode = "attitude" + elif msg.control_mode == CONTROL_MODE_VELOCITY: + self.control_mode = "velocity" + elif msg.control_mode == CONTROL_MODE_POSITION: + self.control_mode = "position" + else: + self.control_mode = "unknown" + + def arm_status_callback(self,msg): + self.armed = msg.armed + + def route_status_callback(self,msg): + self.route_setpoint = msg.current_setpoint_index + + def battery_status_callback(self, msg): + self.battery_percentage = msg.remaining * 100.0 + + def cpu_load_callback(self, msg): + self.cpu_usage = msg.load \ No newline at end of file diff --git a/src/drone_status/package.xml b/src/drone_status/package.xml new file mode 100644 index 00000000..96b4dd15 --- /dev/null +++ b/src/drone_status/package.xml @@ -0,0 +1,21 @@ + + + + drone_status + 0.0.0 + Package for combining several data points from the drone into a single topic + ubuntu + Apache License 2.0 + rclpy + drone_services + px4_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/src/drone_status/resource/drone_status b/src/drone_status/resource/drone_status new file mode 100644 index 00000000..e69de29b diff --git a/src/drone_status/setup.cfg b/src/drone_status/setup.cfg new file mode 100644 index 00000000..40ac3e8a --- /dev/null +++ b/src/drone_status/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/drone_status +[install] +install-scripts=$base/lib/drone_status diff --git a/src/drone_status/setup.py b/src/drone_status/setup.py new file mode 100644 index 00000000..8057ea5c --- /dev/null +++ b/src/drone_status/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'drone_status' + +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='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/src/drone_status/test/test_copyright.py b/src/drone_status/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/src/drone_status/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/drone_status/test/test_flake8.py b/src/drone_status/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/drone_status/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/drone_status/test/test_pep257.py b/src/drone_status/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/drone_status/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/px4_connection/CMakeLists.txt b/src/px4_connection/CMakeLists.txt index 265fe1e0..f41b9cb8 100644 --- a/src/px4_connection/CMakeLists.txt +++ b/src/px4_connection/CMakeLists.txt @@ -31,6 +31,7 @@ ament_target_dependencies( drone_services px4_ros_com px4_msgs + drone_services ) add_executable(px4_controller src/px4_controller.cpp) diff --git a/src/px4_connection/include/drone_control_modes.h b/src/px4_connection/include/drone_control_modes.h new file mode 100644 index 00000000..16594db6 --- /dev/null +++ b/src/px4_connection/include/drone_control_modes.h @@ -0,0 +1,20 @@ +#ifndef DRONE_CONTROL_MODES_H +#define DRONE_CONTROL_MODES_H + +#define CONTROL_MODE_ATTITUDE 1 +#define CONTROL_MODE_VELOCITY 2 +#define CONTROL_MODE_POSITION 3 + +#define CONTROL_MODE_MIN CONTROL_MODE_ATTITUDE +#define CONTROL_MODE_MAX CONTROL_MODE_POSITION + +#define CONTROL_ACTUATOR_POS 0 +#define CONTROL_BODY_RATE_POS 1 +#define CONTROL_ATTITUDE_POS 2 +#define CONTROL_ACCELERATION_POS 3 +#define CONTROL_VELOCITY_POS 4 +#define CONTROL_POSITION_POS 5 + + + +#endif \ No newline at end of file diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 1563dfc8..8aafbf10 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -9,13 +9,11 @@ #include "rclcpp/rclcpp.hpp" #include #include +#include + +#include "drone_control_modes.h" + -#define CONTROL_ACTUATOR_POS 0 -#define CONTROL_BODY_RATE_POS 1 -#define CONTROL_ATTITUDE_POS 2 -#define CONTROL_ACCELERATION_POS 3 -#define CONTROL_VELOCITY_POS 4 -#define CONTROL_POSITION_POS 5 using namespace std::chrono_literals; @@ -24,10 +22,12 @@ class HeartBeat : public rclcpp::Node public: HeartBeat() : Node("heartbeat") { - // disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + vehicle_control_mode_service_ = this->create_service("/drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + RCLCPP_INFO(this->get_logger(), "initialized service /drone/set_vehicle_control"); + // create a publisher on the drone control mode topic + drone_control_mode_publisher_ = this->create_publisher("/drone/control_mode", 10); // create a publisher on the offboard control mode topic - offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + 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(); @@ -37,14 +37,17 @@ public: private: // publisher for offboard control mode messages rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; + // publisher for the control mode of the drone + rclcpp::Publisher::SharedPtr drone_control_mode_publisher_; // timer for sending the heartbeat rclcpp::TimerBase::SharedPtr timer_; + // service to change vehicle_mode + rclcpp::Service::SharedPtr vehicle_control_mode_service_; + // start time of node in seconds double start_time; // which level of control is needed, only attitude control by default int control_mode = 1 << CONTROL_ATTITUDE_POS; - // service to change vehicle_mode - rclcpp::Service::SharedPtr vehicle_control_mode_service_; /** * @brief Publish offboard control mode messages as a heartbeat. * Only the attitude is enabled, because that is how the drone will be controlled. @@ -52,34 +55,69 @@ private: */ void send_heartbeat() { - // set message to enable attitude based on control mode variable 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; - + + msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false; + msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false; + msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false; + msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false; + msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false; + msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false; // get timestamp and publish message msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; offboard_control_mode_publisher_->publish(msg); } +/** + * @brief Publish the current control mode of the drone onto the 'drone/control_mode' topic + * + */ + void publish_new_control_mode() + { + auto msg = drone_services::msg::DroneControlMode(); + if (this->control_mode & (1 << CONTROL_ATTITUDE_POS)) + { + msg.control_mode = CONTROL_MODE_ATTITUDE; + RCLCPP_INFO(this->get_logger(), "set control mode to attitude"); + } + else if (this->control_mode & (1 << CONTROL_VELOCITY_POS)) + { + msg.control_mode = CONTROL_MODE_VELOCITY; + RCLCPP_INFO(this->get_logger(), "set control mode to velocity"); + } + else if (this->control_mode & (1 << CONTROL_POSITION_POS)) + { + msg.control_mode = CONTROL_MODE_POSITION; + RCLCPP_INFO(this->get_logger(), "set control mode to position"); + } + RCLCPP_INFO(this->get_logger(), "publishing new control mode %d", msg.control_mode); + drone_control_mode_publisher_->publish(msg); + } + + /** + * @brief Handle a request to change the vehicle control mode + * + * @param request_header the header of the service request + * @param request the request that was sent to the service + * @param response the reponse that the service sends back to the client. + */ + void handle_vehicle_control_change( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) { - if (request->control < 0 || request->control > CONTROL_POSITION_POS) + if (request->control < 0 || request->control > (1 << CONTROL_POSITION_POS)) + { + response->success = false; + } + else { - response->status = 1; - } else { this->control_mode = request->control; RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode); - response->status = 0; + publish_new_control_mode(); + response->success = true; } - } }; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 9f28763c..fbad0885 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -1,7 +1,7 @@ /** * @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. + * @brief Controller node to contol the PX4 using attitude or trajectory setpoints. * It subscribes to the /drone/set_attitude topic to receive control commands */ @@ -15,12 +15,21 @@ #include #include #include +#include #include +#include +#include +#include +#include +#include #include -#define D_SPEED(x) -x - 9.81 +#include "drone_control_modes.h" + +#define DEFAULT_YAW_SPEED 0.6 // default turning speed in radians per second +#define D_SPEED(x) -x - 9.81 // a speed up of x m/s has to be ajusted for the earths gravity using namespace std::chrono_literals; @@ -37,13 +46,18 @@ public: 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); + arm_status_publisher_ = this->create_publisher("/drone/arm_status", 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)); + vehicle_local_position_subscription_ = this->create_subscription("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1)); + control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_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)); + set_attitude_service_ = this->create_service("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + set_trajectory_service_ = this->create_service("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_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)); + arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_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)); @@ -55,17 +69,23 @@ private: rclcpp::Publisher::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher::SharedPtr vehicle_command_publisher_; + rclcpp::Publisher::SharedPtr arm_status_publisher_; + rclcpp::Subscription::SharedPtr vehicle_attitude_subscription_; + rclcpp::Subscription::SharedPtr vehicle_local_position_subscription_; + rclcpp::Subscription::SharedPtr control_mode_subscription_; rclcpp::Service::SharedPtr set_attitude_service_; - rclcpp::Service::SharedPtr disarm_service_; + rclcpp::Service::SharedPtr set_trajectory_service_; + rclcpp::Service::SharedPtr disarm_service_; + rclcpp::Service::SharedPtr arm_service_; // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; rclcpp::TimerBase::SharedPtr timer_; double start_time_; bool has_sent_status = false; - bool flying = false; + bool user_in_control = false; // if user has taken over control bool armed = false; bool has_swithed = false; int setpoint_count = 0; @@ -75,16 +95,42 @@ private: 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 last_angle = 0; // angle in radians (-PI .. PI) + float last_thrust = 0; // default 10% thrust for when the drone gets armed + + float velocity[3] = {0, 0, 0}; + float position[3] = {0, 0, 0}; float base_q[4] = {0, 0, 0, 0}; int base_q_amount = 0; + char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control + + bool start_trajectory = false; + const float omega = 0.3; // angular speed of the POLAR trajectory + const float K = 0; // [m] gain that regulates the spiral pitch + + float rho_0 = 0; + float theta_0 = 0; + const float p0_z = -0.0; + float p0_x = rho_0 * cos(theta_0); + float p0_y = rho_0 * sin(theta_0); + float des_x = p0_x, des_y = p0_y, des_z = p0_z; + float dot_des_x = 0.0, dot_des_y = 0.0; + float gamma = M_PI_4; + + float X; + float Y; + + float local_x = 0; + float local_y = 0; + + uint32_t discrete_time_index = 0; + // result quaternion std::array q = {0, 0, 0, 0}; - void set_setpoint( + void handle_attitude_setpoint( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -113,9 +159,8 @@ private: 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; + response->success = true; } else { @@ -123,7 +168,44 @@ private: last_setpoint[0] = 0; last_setpoint[1] = 0; last_setpoint[2] = 0; - response->status = 1; + response->success = false; + } + } + + void handle_trajectory_setpoint( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + if (!(request->control_mode == CONTROL_MODE_VELOCITY || request->control_mode == CONTROL_MODE_POSITION)) + { + RCLCPP_INFO(this->get_logger(), "Got invalid trajectory control mode: %d", request->control_mode); + response->success = false; + } + else + { + RCLCPP_INFO(this->get_logger(), "Got new trajectory setpoint with control mode: %d", request->control_mode); + if (request->control_mode == CONTROL_MODE_VELOCITY) + { + for (int i = 0; i < 3; i++) + { + velocity[i] += request->values[i]; + } + RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]); + } + else if (request->control_mode == CONTROL_MODE_POSITION) + { + position[0] += request->values[0]; + position[1] += request->values[1]; + position[2] -= request->values[2]; // height is negative + + RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]); + } + + last_angle += request->yaw; + new_setpoint = true; + RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle); + response->success = true; } } @@ -136,47 +218,66 @@ private: */ void handle_disarm_request( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { RCLCPP_INFO(this->get_logger(), "Got disarm request..."); if (armed) { armed = false; + user_in_control = 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; + auto msg = drone_services::msg::DroneArmStatus(); + msg.armed = false; + arm_status_publisher_->publish(msg); + + response->success = true; } 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; + RCLCPP_ERROR(this->get_logger(), "Got disarm request but drone was not armed!"); + response->success = false; } + } - msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + /** + * @brief arms the drone when a call to the arm service is made + * + * @param request_header the header for the service request + * @param request the request (empty) + * @param response the response (empty) + */ + void handle_arm_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + RCLCPP_INFO(this->get_logger(), "Got arm request..."); - trajectory_setpoint_publisher->publish(msg); + if (!armed) + { + 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"); + this->last_thrust = -0.1; // spin motors at 10% thrust + armed = true; + + auto msg = drone_services::msg::DroneArmStatus(); + msg.armed = true; + arm_status_publisher_->publish(msg); + + response->success = true; + } + else + { + RCLCPP_ERROR(this->get_logger(), "Arm request received but the drone is already armed!"); + response->success = false; + } } void send_attitude_setpoint() @@ -185,41 +286,18 @@ private: // 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; @@ -228,33 +306,102 @@ private: vehicle_setpoint_publisher_->publish(msg); } + void send_velocity_setpoint() + { + auto msg = px4_msgs::msg::TrajectorySetpoint(); + + msg.velocity[0] = velocity[0]; + msg.velocity[1] = velocity[1]; + msg.velocity[2] = -velocity[2]; + for (int i = 0; i < 3; i++) + { + msg.position[i] = NAN; + } + + publish_trajectory_setpoint(msg); + } + + void send_position_setpoint() + { + auto msg = px4_msgs::msg::TrajectorySetpoint(); + + RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z); + RCLCPP_INFO(this->get_logger(),"local position: %f %f", local_x, local_y); + msg.position = {local_x, local_y, des_z}; + msg.velocity = {dot_des_x, dot_des_y, 0.0}; + msg.yaw = gamma; //-3.14; // [-PI:PI] + + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + trajectory_setpoint_publisher->publish(msg); + } + + void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg) + { + msg.yaw = last_angle; + msg.yawspeed = DEFAULT_YAW_SPEED; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + trajectory_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++; + // the spiral, in polar coordinates (theta, rho), is given by + // theta = theta_0 + omega*t + // rho = rho_0 + K*theta + float theta = theta_0 + omega * 0.1 * discrete_time_index; + float rho = rho_0 + K * theta; - ready_to_fly = setpoint_count == 20; - // after 20 setpoints, send arm command to make drone actually fly - if (ready_to_fly) + // from polar to cartesian coordinates + des_x = rho * cos(theta); + des_y = rho * sin(theta); + des_z = position[2]; // the z position can be set to the received height + + // velocity computation + float dot_rho = K * omega; + dot_des_x = dot_rho * cos(theta) - rho * sin(theta) * omega; + dot_des_y = dot_rho * sin(theta) + rho * cos(theta) * omega; + // desired heading direction + gamma = atan2(dot_des_y, dot_des_x); + + if (!user_in_control) { - // 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; + // RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint"); + send_attitude_setpoint(); + } + else + { + if (current_control_mode == CONTROL_MODE_ATTITUDE) + { + // RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); + send_attitude_setpoint(); + } + else + { + if (!new_setpoint) + { + return; + } + if (current_control_mode == CONTROL_MODE_VELOCITY) + { + // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); + send_velocity_setpoint(); + } + else if (current_control_mode == CONTROL_MODE_POSITION) + { + // RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); + send_position_setpoint(); + } + new_setpoint = false; + } + } + if (start_trajectory) + { + discrete_time_index++; } - - send_attitude_setpoint(); } void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) @@ -273,17 +420,57 @@ private: 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 { + } + 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]); + // RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]); } } + void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) + { + // set start values to current position + if (!user_in_control) + { + // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf + rho_0 = pow(msg->x,2) + pow(msg->y,2); + theta_0 = atan2(msg->y, msg->x); + last_angle = msg->heading; + + local_x = msg->x; + local_y = msg->y; + } + X = msg->x; + Y = msg->y; + float Z = msg->z; + if (!start_trajectory && (p0_x + 1.0 > X && p0_x - 1.0 < X) && (p0_y + 1.0 > Y && p0_y - 1.0 < Y) && (p0_z + 1.0 > Z && p0_z - 1.0 < Z)) + { + start_trajectory = true; + RCLCPP_INFO(this->get_logger(), "start trajectory"); + } + } + + void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg) + { + if (msg->control_mode >= CONTROL_MODE_MIN && msg->control_mode <= CONTROL_MODE_MAX) + { + current_control_mode = msg->control_mode; + RCLCPP_INFO(this->get_logger(), "Got valid control mode"); + user_in_control = true; // user has taken over control + } + else + { + RCLCPP_ERROR(this->get_logger(), "Received invalid control mode %d", msg->control_mode); + } + RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode); + } + /** * @brief Publish vehicle commands * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) diff --git a/src/relais_control/package.xml b/src/relais_control/package.xml new file mode 100644 index 00000000..806af7fe --- /dev/null +++ b/src/relais_control/package.xml @@ -0,0 +1,21 @@ + + + + relais_control + 0.0.0 + package to control the relais that enables Pixhawk RX and TX communication + 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/relais_control/relais_control/__init__.py b/src/relais_control/relais_control/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py new file mode 100644 index 00000000..cd7c427e --- /dev/null +++ b/src/relais_control/relais_control/relais_controller.py @@ -0,0 +1,66 @@ +import rclpy +from rclpy.node import Node + +try: + import RPi.GPIO as GPIO +except RuntimeError: + print("Error importing RPi.GPIO! This is probably because you need superuser privileges. You can achieve this by using 'sudo' to run your script") + +from drone_services.srv import ControlRelais +class RelaisController(Node): + def __init__(self): + super().__init__('relais_controller') + self.srv = self.create_service(ControlRelais, '/drone/control_relais', self.control_relais_callback) + + self.relais1_pin = 17 + self.relais2_pin = 27 + self.init_gpio() + self.turn_relais_on() + + def init_gpio(self): + GPIO.setwarnings(False) + + self.get_logger().info(str(GPIO.RPI_INFO)) + + GPIO.setmode(GPIO.BCM) + + GPIO.setup(self.relais1_pin, GPIO.OUT) + GPIO.setup(self.relais2_pin, GPIO.OUT) + self.get_logger().info("GPIO initialized") + + def turn_relais_on(self): + GPIO.output(self.relais1_pin, GPIO.HIGH) + GPIO.output(self.relais2_pin, GPIO.HIGH) + self.get_logger().info("Relais turned on") + + def control_relais_callback(self, request, response): + if request.relais1_on: + GPIO.output(self.relais1_pin, GPIO.HIGH) + response.bits = response.bits | 1 + else: + GPIO.output(self.relais1_pin, GPIO.LOW) + response.bits = response.bits & ~(1 << 0) + if request.relais2_on: + GPIO.output(self.relais2_pin, GPIO.HIGH) + response.bits = response.bits | (1 << 1) + else: + GPIO.output(self.relais2_pin, GPIO.LOW) + response.bits = response.bits & ~(1 << 1) + return response + +def main(args=None): + rclpy.init(args=args) + + relais_controller = RelaisController() + + rclpy.spin(relais_controller) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + relais_controller.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/relais_control/resource/relais_control b/src/relais_control/resource/relais_control new file mode 100644 index 00000000..e69de29b diff --git a/src/relais_control/setup.cfg b/src/relais_control/setup.cfg new file mode 100644 index 00000000..4d7436a8 --- /dev/null +++ b/src/relais_control/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/relais_control +[install] +install-scripts=$base/lib/relais_control diff --git a/src/relais_control/setup.py b/src/relais_control/setup.py new file mode 100644 index 00000000..40560f60 --- /dev/null +++ b/src/relais_control/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'relais_control' + +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='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'relais_controller = relais_control.relais_controller:main' + ], + }, +) diff --git a/src/relais_control/test/test_copyright.py b/src/relais_control/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/src/relais_control/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/relais_control/test/test_flake8.py b/src/relais_control/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/relais_control/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/relais_control/test/test_pep257.py b/src/relais_control/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/relais_control/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/test_controller.py b/src/test_controls/test_controls/test_controller.py index 312780bb..47f814aa 100644 --- a/src/test_controls/test_controls/test_controller.py +++ b/src/test_controls/test_controls/test_controller.py @@ -5,7 +5,13 @@ from sshkeyboard import listen_keyboard_manual import asyncio from drone_services.srv import SetAttitude +from drone_services.srv import SetTrajectory +from drone_services.srv import SetVehicleControl +from drone_services.srv import ArmDrone +CONTROL_MODE_ATTITUDE = 4 +CONTROL_MODE_VELOCITY = 16 +CONTROL_MODE_POSITION = 32 class TestController(Node): @@ -13,96 +19,236 @@ class TestController(Node): 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('set attitude service not available, waiting again...') + self.get_logger().info('successfully connected to set attitude service') + self.vehicle_control_cli = self.create_client( + SetVehicleControl, '/drone/set_vehicle_control') + while not self.vehicle_control_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('set vehicle control service not available, waiting again...') + self.get_logger().info('successfully connected to set vehicle control service') + self.traj_cli = self.create_client(SetTrajectory, '/drone/set_trajectory') + while not self.traj_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('set trajectory service not available, waiting again...') + self.get_logger().info('successfully connected to set trajectory service') + self.arm_cli = self.create_client(ArmDrone, '/drone/arm') + while not self.arm_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('arm service not available, waiting again...') + self.get_logger().info('successfully connected to arm service') - 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") + self.get_logger().info('all services available') + self.control_mode = 1 + + self.attitude_req = SetAttitude.Request() + self.vehicle_control_req = SetVehicleControl.Request() + self.traj_req = SetTrajectory.Request() + self.arm_req = ArmDrone.Request() + + self.get_logger().info("\nControls:\n1 - Attitude control\n2 - Velocity control\n3 - Position control\n/ - Arm drone\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\nCTRL-C - 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) + def send_arm(self): + self.future = self.arm_cli.call_async(self.arm_req) rclpy.spin_until_future_complete(self, self.future) - self.get_logger().info('publishing message on service') + self.get_logger().info('publishing message arm msg on service') + return self.future.result() + + def send_control_mode(self): + self.vehicle_control_req.control = self.control_mode + self.future = self.vehicle_control_cli.call_async(self.vehicle_control_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing message vehicle control msg on service') + return self.future.result() + + def send_attitude_request(self, yaw, pitch, roll, thrust): + self.attitude_req.yaw = yaw + self.attitude_req.pitch = pitch + self.attitude_req.roll = roll + self.attitude_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.attitude_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing attitude message on service') + return self.future.result() + + def send_velocity_request(self, x, y, z, angle): + self.traj_req.control_mode = 2 + self.traj_req.yaw = angle + self.traj_req.values = [x, y, z] + self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle)) + self.future = self.traj_cli.call_async(self.traj_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing velocity message on service') + return self.future.result() + + def send_position_request(self, x, y, z, angle): + self.traj_req.control_mode = 3 + self.traj_req.yaw = angle + self.traj_req.values = [x, y, z] + self.get_logger().info('set request to %f %f %f %f' % (x, y, z, angle)) + self.future = self.traj_cli.call_async(self.traj_req) + rclpy.spin_until_future_complete(self, self.future) + self.get_logger().info('publishing position message on service') return self.future.result() def on_release(self, key): # self.get_logger().info('released ' + str(key)) pass + def move_up(self): + pass + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.05) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=0.0, z=0.1, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.5, angle=0.0) + + def move_right(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=1.0, thrust=0.0) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=1.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=1.0, y=0.0, z=0.0, angle=0.0) + + def move_down(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.05) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=0.0, z=-0.1, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=-0.5, angle=0.0) + + def move_left(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=-1.0, thrust=0.0) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=-1.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=-1.0, y=0.0, z=0.0, angle=0.0) + + def rotate_right(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=1.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=1.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=1.0) + + def rotate_left(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=-1.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=-1.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=-1.0) + + def move_forward(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=-1.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=1.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=1.0, z=0.0, angle=0.0) + + def move_backward(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=1.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=-1.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=-1.0, z=0.0, angle=0.0) + + def stop(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.0) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=0.0, z=0.0, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=0.0, angle=0.0) + + def move_up_little(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=0.01) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=0.0, z=0.05, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=1.0, angle=0.0) + + def move_down_little(self): + if (self.control_mode == CONTROL_MODE_ATTITUDE): + self.send_attitude_request(pitch=0.0, yaw=0.0, + roll=0.0, thrust=-0.01) + elif (self.control_mode == CONTROL_MODE_VELOCITY): + self.send_velocity_request(x=0.0, y=0.0, z=-0.05, angle=0.0) + else: + self.send_position_request(x=0.0, y=0.0, z=-1.0, angle=0.0) + def on_press(self, key): try: # self.get_logger().info('pressed ' + char) if key == 'w': self.get_logger().info('forward') - self.send_request(pitch=-1.0, yaw=0.0, - roll=0.0, thrust=0.0) + self.move_forward() if key == 's': self.get_logger().info('backward') - self.send_request(pitch=1.0, yaw=0.0, - roll=0.0, thrust=0.0) + self.move_backward() if key == 'a': self.get_logger().info('left') - self.send_request(pitch=0.0, yaw=0.0, - roll=-1.0, thrust=0.0) + self.move_left() if key == 'd': self.get_logger().info('right') - self.send_request(pitch=0.0, yaw=0.0, - roll=1.0, thrust=0.0) + self.move_right() if key == 'q': self.get_logger().info('rotate left') - self.send_request(pitch=0.0, yaw=-1.0, - roll=0.0, thrust=0.0) + self.rotate_left() if key == 'e': self.get_logger().info('rotate right') - self.send_request(pitch=0.0, yaw=1.0, - roll=0.0, thrust=0.0) + self.rotate_right() if key == 'z': self.get_logger().info('down') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=-0.05) + self.move_down() if key == 'space': self.get_logger().info('up') - self.send_request(pitch=0.0, yaw=0.0, - roll=0.0, thrust=0.05) + self.move_up() 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) + self.move_down_little() 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) + self.move_up_little() 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) + self.stop() + if key == '1': + self.get_logger().info('attitude control') + self.control_mode = CONTROL_MODE_ATTITUDE + self.send_control_mode() + if key == '2': + self.get_logger().info('velocity control') + self.control_mode = CONTROL_MODE_VELOCITY + self.send_control_mode() + if key == '3': + self.get_logger().info('position control') + self.control_mode = CONTROL_MODE_POSITION + self.send_control_mode() + if key == '/': + self.get_logger().info('arming') + self.send_arm() except Exception as e: self.get_logger().error(str(e))