Merge pull request #273 from NannCai/joystick

Joystick
IMRCLab · Aug 11, 2023 · 9483029 · 9483029
2 parents 0b0c740 + f626731
commit 9483029
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 9 deletions.
diff --git a/crazyflie/config/teleop.yaml b/crazyflie/config/teleop.yaml
@@ -45,5 +45,12 @@
       y: 0.0
       z: 0.10
     auto_yaw_rate: 0.0 # rad/s, use 0.0 for manual yaw control
-
+    takeoff:
+      duration: 2.0
+      height: 0.5
+      button: 7  # 7 start
+    land:
+      button: 6 # 6 back
+    emergency:
+      button: 1 # 1 red
 
diff --git a/crazyflie/src/teleop.cpp b/crazyflie/src/teleop.cpp
@@ -81,6 +81,18 @@ class TeleopNode : public rclcpp::Node
         this->declare_parameter("cmd_vel_world.y_limit", rclcpp::PARAMETER_DOUBLE_ARRAY);
         this->declare_parameter("cmd_vel_world.z_limit", rclcpp::PARAMETER_DOUBLE_ARRAY);
 
+        this->declare_parameter<float>("takeoff.duration");
+        this->get_parameter<float>("takeoff.duration", takeoff_paras_.duration);
+        this->declare_parameter<float>("takeoff.height");
+        this->get_parameter<float>("takeoff.height", takeoff_paras_.height);
+        this->declare_parameter<int>("takeoff.button");
+        this->get_parameter<int>("takeoff.button", takeoff_paras_.button);
+
+        this->declare_parameter<int>("land.button");
+        this->get_parameter<int>("land.button", land_button);
+        this->declare_parameter<int>("emergency.button");
+        this->get_parameter<int>("emergency.button", emergency_button);
+
         on_mode_switched();
 
         dt_ = 1.0f/frequency_;
@@ -132,6 +144,16 @@ class TeleopNode : public rclcpp::Node
         return a;
     }
 
+    struct 
+    { 
+        float duration;
+        float height;
+        int button;
+    } takeoff_paras_;
+
+    int land_button;
+    int emergency_button;
+
     void on_parameter_event(const rcl_interfaces::msg::ParameterEvent &event)
     {
         if (event.node == "/teleop") {
@@ -195,15 +217,17 @@ class TeleopNode : public rclcpp::Node
     {
 
         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[Xbox360Buttons::Red] == 1 && lastButtonState[Xbox360Buttons::Red] == 0) {
+            if (msg->buttons[emergency_button] == 1 && lastButtonState[emergency_button] == 0) {
                 emergency();
             }
-            if (msg->buttons[Xbox360Buttons::Start] == 1 && lastButtonState[Xbox360Buttons::Start] == 0) {
+
+            if (msg->buttons[takeoff_paras_.button] == 1 && lastButtonState[takeoff_paras_.button] == 0) {
                 takeoff();
             }
-            if (msg->buttons[Xbox360Buttons::Back] == 1 && lastButtonState[Xbox360Buttons::Back] == 0) {
+            if (msg->buttons[land_button] == 1 && lastButtonState[land_button] == 0) {
                 land();
             }
         }
@@ -262,12 +286,12 @@ class TeleopNode : public rclcpp::Node
         }
         auto request = std::make_shared<Takeoff::Request>();
         request->group_mask = 0;
-        request->height = 0.5;
-        request->duration = rclcpp::Duration::from_seconds(2);
+        request->height = takeoff_paras_.height;
+        request->duration = rclcpp::Duration::from_seconds(takeoff_paras_.duration);
         client_takeoff_->async_send_request(request);
 
-        timer_takeoff_ = this->create_wall_timer(2s, [this]() {
-            state_.z = 0.5;
+        timer_takeoff_ = this->create_wall_timer(std::chrono::duration<float>(takeoff_paras_.duration), [this]() {
+            state_.z = takeoff_paras_.height;  
             is_low_level_flight_active_ = true;
             this->timer_takeoff_->cancel();
         });