teleop: add support for arming

crazyflie/config/teleop.yaml

@@ -53,4 +53,6 @@
  button: 6 # 6 back
  emergency:
  button: 1 # 1 red
+ arm:
+ button: 3 # yellow
 

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)

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

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 <crazyflie_interfaces/srv/notify_setpoints_stop.hpp>
 #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<int>("emergency.button");
  this->get_parameter<int>("emergency.button", emergency_button);
 
+ this->declare_parameter<int>("arm.button");
+ this->get_parameter<int>("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<Empty>("emergency");
+ client_arm_ = this->create_client<Arm>("arm");
  client_takeoff_ = this->create_client<Takeoff>("takeoff");
  client_land_ = this->create_client<Land>("land");
  client_notify_setpoints_stop_ = this->create_client<NotifySetpointsStop>("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<int> lastButtonState;
 
- static std::vector<int> 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<Arm::Request>();
+ 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<sensor_msgs::msg::Joy>::SharedPtr subscription_;
  rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client_emergency_;
+ rclcpp::Client<Arm>::SharedPtr client_arm_;
  rclcpp::Client<Takeoff>::SharedPtr client_takeoff_;
  rclcpp::Client<Land>::SharedPtr client_land_;
  rclcpp::Client<NotifySetpointsStop>::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[])