armada_controller/include/armada_controller/armada_controller_node.hpp

@@ -15,6 +15,13 @@
 #include <memory>
 #include <vector>
 
+// Define control mode enum
+enum class ControlMode {
+  INACTIVE,   // Controller is not active
+  TRAJECTORY, // Following trajectory inputs
+  JOYSTICK    // Using joystick inputs
+};
+
 /**
  * @brief ROS node for the ARMADA controller
  */
@@ -51,7 +58,8 @@ private:
    * @brief Callback for joystick messages
    * @param msg Joystick message
    */
-  void joystickCallback(const ls2n_drone_interfaces::msg::Joystick::SharedPtr msg);
+  void
+  joystickCallback(const ls2n_drone_interfaces::msg::Joystick::SharedPtr msg);
 
   /**
    * @brief Timer callback for updating position based on joystick input
@@ -59,12 +67,12 @@ private:
   void joystickUpdateTimer();
 
   /**
-   * @brief Service callback to activate the armada controller
+   * @brief Service callback to activate trajectory control mode
    * @param request Empty request
    * @param response Empty response
    * @return Empty response
    */
-  void activateArmadaControl(
+  void activateTrajectoryControl(
       const std::shared_ptr<std_srvs::srv::Empty::Request> request,
       std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
@@ -78,6 +86,13 @@ private:
       const std::shared_ptr<std_srvs::srv::Empty::Request> request,
       std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
+  /**
+   * @brief Prepare for control activation (common code for both control modes)
+   * @param control_name Name of the control mode for error messages
+   * @return True if preparation was successful, false otherwise
+   */
+  bool prepareControlActivation(const std::string& control_name);
+
   /**
    * @brief Run the controller
    */
@@ -158,20 +173,17 @@ private:
 
   // ROS services
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr
-      activate_armada_control_service_;
+      activate_trajectory_control_service_;
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr
       activate_joystick_control_service_;
   std::vector<rclcpp::Client<
       ls2n_drone_interfaces::srv::ControllerSelection>::SharedPtr>
       controller_selection_clients_;
 
-  // Take-off parameters
-  double initial_z_;
-  double initial_time_;
   double previous_loop_time_ = 0.0;
 
   // Controller state
-  bool controller_active_ = false;
+  ControlMode control_mode_ = ControlMode::INACTIVE;
   bool simulation_mode_ = false;
 };
 

armada_controller/src/armada_controller.cpp

@@ -55,7 +55,7 @@ ArmadaController::ArmadaController(int num_drones, double payload_mass,
   kd_ = Eigen::MatrixXd::Zero(state_size, state_size);
 
   // Default gains
-  setPIDGains(16.0, 0.0, 8.0);
+  setPIDGains(9.0, 0.0, 6.0);
 }
 
 void ArmadaController::setPIDGains(double kp, double ki, double kd) {

armada_controller/src/armada_controller_node.cpp

@@ -46,7 +46,7 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
   desired_state_velocity_ = Eigen::VectorXd::Zero(state_size);
   desired_state_acceleration_ = Eigen::VectorXd::Zero(state_size);
 
-  // Initialize joystick control 
+  // Initialize joystick control
   joystick_velocity_command_ = Eigen::Vector3d::Zero();
   joystick_reference_position_ = Eigen::Vector3d::Zero();
 
@@ -98,11 +98,12 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
           });
 
   // Joystick subscriber
-  joystick_subscriber_ = create_subscription<ls2n_drone_interfaces::msg::Joystick>(
-      "/joystick", qos, 
-      [this](const ls2n_drone_interfaces::msg::Joystick::SharedPtr msg) {
-        this->joystickCallback(msg);
-      });
+  joystick_subscriber_ =
+      create_subscription<ls2n_drone_interfaces::msg::Joystick>(
+          "/CommandCenter/joystick", qos,
+          [this](const ls2n_drone_interfaces::msg::Joystick::SharedPtr msg) {
+            this->joystickCallback(msg);
+          });
 
   // Publishers for visualization
   desired_orientation_publishers_.resize(num_drones_);
@@ -127,11 +128,11 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
       "/Armada/payload/desired_acceleration", qos);
 
   // Services
-  activate_armada_control_service_ = create_service<std_srvs::srv::Empty>(
-      "/Armada/ActivateArmadaControl",
+  activate_trajectory_control_service_ = create_service<std_srvs::srv::Empty>(
+      "/Armada/ActivateTrajectoryControl",
       [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
              std::shared_ptr<std_srvs::srv::Empty::Response> response) {
-        this->activateArmadaControl(request, response);
+        this->activateTrajectoryControl(request, response);
       });
 
   // Joystick control service
@@ -143,12 +144,11 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
       });
 
   // Create joystick update timer (100Hz)
-  joystick_timer_ = create_wall_timer(10ms, 
-      [this]() {
-        if (joystick_mode_active_) {
-          this->joystickUpdateTimer();
-        }
-      });
+  joystick_timer_ = create_wall_timer(10ms, [this]() {
+    if (control_mode_ == ControlMode::JOYSTICK) {
+      this->joystickUpdateTimer();
+    }
+  });
 }
 
 void ArmadaControllerNode::droneOdometryCallback(
@@ -184,15 +184,15 @@ void ArmadaControllerNode::payloadOdometryCallback(
   payload_pose_received_ = true;
 
   // Run controller if active
-  if (controller_active_) {
+  if (control_mode_ != ControlMode::INACTIVE) {
     runController();
   }
 }
 
 void ArmadaControllerNode::trajectoryCallback(
     const trajectory_msgs::msg::JointTrajectory::SharedPtr msg) {
-  // If joystick mode is active, ignore trajectory messages
-  if (joystick_mode_active_) {
+  // If not in trajectory mode, ignore trajectory messages
+  if (control_mode_ != ControlMode::TRAJECTORY) {
     return;
   }
 
@@ -270,16 +270,19 @@ void ArmadaControllerNode::trajectoryCallback(
 
 void ArmadaControllerNode::joystickCallback(
     const ls2n_drone_interfaces::msg::Joystick::SharedPtr msg) {
-  if (!joystick_mode_active_) {
+  if (control_mode_ != ControlMode::JOYSTICK) {
     return;
   }
 
   // Scale the joystick input to get velocity commands
-  joystick_velocity_command_.x() = -msg->axis_left_vertical * max_joystick_velocity_;
-  joystick_velocity_command_.y() = msg->axis_left_horizontal * max_joystick_velocity_;
-  
+  joystick_velocity_command_.x() =
+      -msg->axis_left_horizontal * max_joystick_velocity_;
+  joystick_velocity_command_.y() =
+      msg->axis_left_vertical * max_joystick_velocity_;
+
   // Use right stick for vertical movement
-  joystick_velocity_command_.z() = msg->axis_right_vertical * max_joystick_velocity_;
+  joystick_velocity_command_.z() =
+      msg->axis_right_vertical * max_joystick_velocity_;
 }
 
 void ArmadaControllerNode::joystickUpdateTimer() {
@@ -287,9 +290,8 @@ void ArmadaControllerNode::joystickUpdateTimer() {
   auto current_time = this->now();
   double dt = 0.01; // Default 10ms
 
-  if (last_joystick_update_time_ != rclcpp::Time(0, 0)) {
-    dt = (current_time - last_joystick_update_time_).seconds();
-  }
+  dt = (current_time - last_joystick_update_time_).seconds();
+
   last_joystick_update_time_ = current_time;
 
   // Update reference position based on current velocity command
@@ -311,19 +313,16 @@ void ArmadaControllerNode::joystickUpdateTimer() {
   desired_state_acceleration_(2) = 0.0;
 }
 
-void ArmadaControllerNode::activateArmadaControl(
-    const std::shared_ptr<std_srvs::srv::Empty::Request>,
-    std::shared_ptr<std_srvs::srv::Empty::Response>) {
-  RCLCPP_INFO(get_logger(), "Activate armada control request made");
-
+bool ArmadaControllerNode::prepareControlActivation(
+    const std::string &control_name) {
   // Check if we have received odometry for all drones
   for (size_t i = 0; i < odometry_received_.size(); ++i) {
     if (!odometry_received_[i]) {
       RCLCPP_ERROR(get_logger(),
-                   "Cannot activate armada control, odometry feedback for "
+                   "Cannot activate %s control, odometry feedback for "
                    "drone %zu is missing!",
-                   i + 1);
-      return;
+                   control_name.c_str(), i + 1);
+      return false;
     }
   }
 
@@ -331,8 +330,9 @@ void ArmadaControllerNode::activateArmadaControl(
   if (!payload_pose_received_) {
     RCLCPP_ERROR(
         get_logger(),
-        "Cannot activate armada control, payload pose feedback is missing!");
-    return;
+        "Cannot activate %s control, payload pose feedback is missing!",
+        control_name.c_str());
+    return false;
   }
 
   // Skip controller selection if in simulation mode
@@ -348,7 +348,7 @@ void ArmadaControllerNode::activateArmadaControl(
         RCLCPP_ERROR(get_logger(),
                      "Controller selection service for drone %zu not available",
                      i + 1);
-        return;
+        return false;
       }
 
       RCLCPP_INFO(get_logger(),
@@ -367,7 +367,6 @@ void ArmadaControllerNode::activateArmadaControl(
 
   // Calculate current cable orientations and set as desired
   std::vector<Eigen::Vector3d> drone_pos;
-
   for (const auto &state : drone_states_) {
     drone_pos.push_back(state.position);
   }
@@ -384,27 +383,28 @@ void ArmadaControllerNode::activateArmadaControl(
     desired_state_(4 + 2 * i) = current_state(4 + 2 * i); // theta
   }
 
-  controller_active_ = true;
-  joystick_mode_active_ = false;
-  RCLCPP_INFO(get_logger(), "Armada control activated!");
+  return true;
 }
 
-void ArmadaControllerNode::activateJoystickControl(
+void ArmadaControllerNode::activateTrajectoryControl(
     const std::shared_ptr<std_srvs::srv::Empty::Request>,
     std::shared_ptr<std_srvs::srv::Empty::Response>) {
-  RCLCPP_INFO(get_logger(), "Activate joystick control request made");
+  RCLCPP_INFO(get_logger(), "Activate trajectory control request made");
 
-  // Check if controller is active
-  if (!controller_active_) {
-    RCLCPP_ERROR(get_logger(),
-                 "Cannot activate joystick control, armada controller not active");
+  if (!prepareControlActivation("trajectory")) {
     return;
   }
 
-  // Check if we have received payload pose
-  if (!payload_pose_received_) {
-    RCLCPP_ERROR(get_logger(),
-                 "Cannot activate joystick control, payload pose feedback is missing!");
+  control_mode_ = ControlMode::TRAJECTORY;
+  RCLCPP_INFO(get_logger(), "Trajectory control mode activated!");
+}
+
+void ArmadaControllerNode::activateJoystickControl(
+    const std::shared_ptr<std_srvs::srv::Empty::Request>,
+    std::shared_ptr<std_srvs::srv::Empty::Response>) {
+  RCLCPP_INFO(get_logger(), "Activate joystick control request made");
+
+  if (!prepareControlActivation("joystick")) {
     return;
   }
 
@@ -413,15 +413,12 @@ void ArmadaControllerNode::activateJoystickControl(
   joystick_velocity_command_ = Eigen::Vector3d::Zero();
   last_joystick_update_time_ = this->now();
 
-  // Update desired state with current position
-  desired_state_.segment<3>(0) = joystick_reference_position_;
-  
   // Zero velocities and accelerations
   desired_state_velocity_.setZero();
   desired_state_acceleration_.setZero();
 
-  joystick_mode_active_ = true;
-  RCLCPP_INFO(get_logger(), "Joystick control activated!");
+  control_mode_ = ControlMode::JOYSTICK;
+  RCLCPP_INFO(get_logger(), "Joystick control mode activated!");
 }
 
 void ArmadaControllerNode::runController() {

launch/armada_control.launch.py

 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
-    OpaqueFunction,
     IncludeLaunchDescription,
 )
 from launch.substitutions import LaunchConfiguration
@@ -11,22 +10,21 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 def generate_launch_description():
-
     platform_mass_launch_arg = DeclareLaunchArgument(
         "platform_mass",
         default_value="0.993",
         description="Platform mass [kg]",
     )
-    
+
     drone_masses_launch_arg = DeclareLaunchArgument(
         "drone_masses",
         default_value="[1.0,1.0,1.0]",
         description="Comma-separated list of drone masses in kg",
     )
-    
+
     max_joystick_velocity_launch_arg = DeclareLaunchArgument(
         "max_joystick_velocity",
-        default_value="0.5",
+        default_value="0.1",
         description="Maximum velocity in m/s for joystick control",
     )
 
@@ -81,13 +79,17 @@ def generate_launch_description():
             Node(
                 package="ls2n_drone_armada",
                 executable="armada_control",
-                parameters=[{
-                    "drones_to_control": ["Crazy2fly8", "Crazy2fly6", "Crazy2fly7"],
-                    "cable_length": 1.6,
-                    "platform_mass": LaunchConfiguration("platform_mass"),
-                    "drone_masses": [1.0, 1.0, 1.0],
-                    "max_joystick_velocity": LaunchConfiguration("max_joystick_velocity")
-                }],
+                parameters=[
+                    {
+                        "drones_to_control": ["Crazy2fly8", "Crazy2fly6", "Crazy2fly7"],
+                        "cable_length": 1.6,
+                        "platform_mass": LaunchConfiguration("platform_mass"),
+                        "drone_masses": [1.0, 1.0, 1.0],
+                        "max_joystick_velocity": LaunchConfiguration(
+                            "max_joystick_velocity"
+                        ),
+                    }
+                ],
             ),
             Node(
                 package="ls2n_drone_armada",

launch/sitl_armada_control.launch.py

@@ -257,7 +257,7 @@ def generate_launch_description() -> LaunchDescription:
 
     platform_mass_launch_arg = DeclareLaunchArgument(
         "platform_mass",
-        default_value="0.5",
+        default_value="1.0",
         description="Platform mass [kg]",
     )