From 314f089f080f9f764e15c3bacf8b2af899fc983c Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 27 May 2024 22:23:17 +0200 Subject: [PATCH] teleop: add support for arming --- crazyflie/config/teleop.yaml | 2 ++ crazyflie/launch/launch.py | 1 + crazyflie/scripts/gui.py | 2 ++ crazyflie/src/teleop.cpp | 68 +++++++++++++++++++++--------------- 4 files changed, 45 insertions(+), 28 deletions(-) diff --git a/crazyflie/config/teleop.yaml b/crazyflie/config/teleop.yaml index a9523e2a5..4b072aa36 100644 --- a/crazyflie/config/teleop.yaml +++ b/crazyflie/config/teleop.yaml @@ -53,4 +53,6 @@ button: 6 # 6 back emergency: button: 1 # 1 red + arm: + button: 3 # yellow diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index be2ed1339..9761c5535 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -100,6 +100,7 @@ def generate_launch_description(): name='teleop', remappings=[ ('emergency', 'all/emergency'), + ('arm', 'all/arm'), ('takeoff', 'all/takeoff'), ('land', 'all/land'), # uncomment to manually control (and update teleop.yaml) diff --git a/crazyflie/scripts/gui.py b/crazyflie/scripts/gui.py index a0f221b71..54304520c 100755 --- a/crazyflie/scripts/gui.py +++ b/crazyflie/scripts/gui.py @@ -194,6 +194,8 @@ def on_status(self, msg, name) -> None: supervisor_text += "auto-arm; " if msg.supervisor_info & Status.SUPERVISOR_INFO_CAN_FLY: supervisor_text += "can fly; " + else: + status_ok = False if msg.supervisor_info & Status.SUPERVISOR_INFO_IS_FLYING: supervisor_text += "is flying; " is_flying = True diff --git a/crazyflie/src/teleop.cpp b/crazyflie/src/teleop.cpp index 59ce47538..e07d01e99 100755 --- a/crazyflie/src/teleop.cpp +++ b/crazyflie/src/teleop.cpp @@ -7,6 +7,7 @@ #include "std_srvs/srv/empty.hpp" #include "crazyflie_interfaces/srv/takeoff.hpp" #include "crazyflie_interfaces/srv/land.hpp" +#include "crazyflie_interfaces/srv/arm.hpp" #include #include "geometry_msgs/msg/twist.hpp" #include "crazyflie_interfaces/msg/full_state.hpp" @@ -19,26 +20,13 @@ using std::placeholders::_1; using std_srvs::srv::Empty; using crazyflie_interfaces::srv::Takeoff; using crazyflie_interfaces::srv::Land; +using crazyflie_interfaces::srv::Arm; using crazyflie_interfaces::srv::NotifySetpointsStop; using crazyflie_interfaces::msg::FullState; using namespace std::chrono_literals; using namespace Eigen; -namespace Xbox360Buttons { - - enum { Green = 0, - Red = 1, - Blue = 2, - Yellow = 3, - LB = 4, - RB = 5, - Back = 6, - Start = 7, - COUNT = 8, - }; -} - class TeleopNode : public rclcpp::Node { public: @@ -93,6 +81,10 @@ class TeleopNode : public rclcpp::Node this->declare_parameter("emergency.button"); this->get_parameter("emergency.button", emergency_button); + this->declare_parameter("arm.button"); + this->get_parameter("arm.button", arm_button); + is_armed_ = false; + on_mode_switched(); dt_ = 1.0f/frequency_; @@ -107,6 +99,7 @@ class TeleopNode : public rclcpp::Node } client_emergency_ = this->create_client("emergency"); + client_arm_ = this->create_client("arm"); client_takeoff_ = this->create_client("takeoff"); client_land_ = this->create_client("land"); client_notify_setpoints_stop_ = this->create_client("notify_setpoints_stop"); @@ -153,6 +146,7 @@ class TeleopNode : public rclcpp::Node int land_button; int emergency_button; + int arm_button; void on_parameter_event(const rcl_interfaces::msg::ParameterEvent &event) { @@ -215,21 +209,26 @@ class TeleopNode : public rclcpp::Node void joyChanged(const sensor_msgs::msg::Joy::SharedPtr msg) { + static std::vector lastButtonState; - static std::vector lastButtonState(Xbox360Buttons::COUNT); - - std::cout << "takeoff_paras_.button: " << takeoff_paras_.button << std::endl; - if (msg->buttons.size() >= Xbox360Buttons::COUNT && lastButtonState.size() >= Xbox360Buttons::COUNT) { - if (msg->buttons[emergency_button] == 1 && lastButtonState[emergency_button] == 0) { - emergency(); - } + auto checkButton = [&](int button) { + return msg->buttons.size() > button && + msg->buttons[button] == 1 && + lastButtonState.size() > button && + lastButtonState[button] == 0; + }; - if (msg->buttons[takeoff_paras_.button] == 1 && lastButtonState[takeoff_paras_.button] == 0) { - takeoff(); - } - if (msg->buttons[land_button] == 1 && lastButtonState[land_button] == 0) { - land(); - } + if (checkButton(emergency_button)) { + emergency(); + } + if (checkButton(arm_button)) { + arm(); + } + if (checkButton(takeoff_paras_.button)) { + takeoff(); + } + if (checkButton(land_button)) { + land(); } lastButtonState = msg->buttons; @@ -243,7 +242,6 @@ class TeleopNode : public rclcpp::Node else { twist_.angular.z = auto_yaw_rate_; } - } sensor_msgs::msg::Joy::_axes_type::value_type getAxis(const sensor_msgs::msg::Joy::SharedPtr &msg, Axis a) @@ -278,6 +276,18 @@ class TeleopNode : public rclcpp::Node client_emergency_->async_send_request(request); } + void arm() + { + if (!client_arm_->service_is_ready()) { + RCLCPP_ERROR(get_logger(), "Arm service not ready!"); + return; + } + auto request = std::make_shared(); + request->arm = !is_armed_; + client_arm_->async_send_request(request); + is_armed_ = !is_armed_; + } + void takeoff() { if (!client_takeoff_->service_is_ready()) { @@ -373,6 +383,7 @@ class TeleopNode : public rclcpp::Node rclcpp::Subscription::SharedPtr subscription_; rclcpp::Client::SharedPtr client_emergency_; + rclcpp::Client::SharedPtr client_arm_; rclcpp::Client::SharedPtr client_takeoff_; rclcpp::Client::SharedPtr client_land_; rclcpp::Client::SharedPtr client_notify_setpoints_stop_; @@ -393,6 +404,7 @@ class TeleopNode : public rclcpp::Node float dt_; bool is_low_level_flight_active_; double auto_yaw_rate_; + bool is_armed_; }; int main(int argc, char *argv[])