armada_controller/CMakeLists.txt

@@ -10,12 +10,6 @@ find_package(rosidl_default_generators REQUIRED)
 
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
-# Generate services
-rosidl_generate_interfaces(ls2n_drone_armada
-  "srv/MoveUp.srv"
-  DEPENDENCIES std_msgs
-)
-
 # Create a library for the controller core
 add_library(armada_controller_core src/armada_controller.cpp)
 
@@ -39,10 +33,6 @@ target_include_directories(armada_controller PRIVATE include
 # Link the executable to the core library
 target_link_libraries(armada_controller armada_controller_core Eigen3::Eigen)
 
-# Link with the generated services
-rosidl_get_typesupport_target(cpp_typesupport_target ls2n_drone_armada "rosidl_typesupport_cpp")
-target_link_libraries(armada_controller "${cpp_typesupport_target}")
-
 # ROS2 dependencies for the executable
 ament_target_dependencies(
   armada_controller

armada_controller/include/armada_controller/armada_controller_node.hpp

@@ -3,9 +3,9 @@
 
 #include "armada_controller/armada_controller.hpp"
 
-#include "ls2n_drone_armada/srv/move_up.hpp"
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <ls2n_drone_interfaces/msg/joystick.hpp>
 #include <ls2n_drone_interfaces/srv/controller_selection.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -47,6 +47,17 @@ private:
   void trajectoryCallback(
       const trajectory_msgs::msg::JointTrajectory::SharedPtr msg);
 
+  /**
+   * @brief Callback for joystick messages
+   * @param msg Joystick message
+   */
+  void joystickCallback(const ls2n_drone_interfaces::msg::Joystick::SharedPtr msg);
+
+  /**
+   * @brief Timer callback for updating position based on joystick input
+   */
+  void joystickUpdateTimer();
+
   /**
    * @brief Service callback to activate the armada controller
    * @param request Empty request
@@ -57,6 +68,16 @@ private:
       const std::shared_ptr<std_srvs::srv::Empty::Request> request,
       std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
+  /**
+   * @brief Service callback to activate joystick control mode
+   * @param request Empty request
+   * @param response Empty response
+   * @return Empty response
+   */
+  void activateJoystickControl(
+      const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+      std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
   /**
    * @brief Run the controller
    */
@@ -81,6 +102,7 @@ private:
   double payload_mass_;
   std::vector<double> drone_masses_;
   double cable_length_;
+  double max_joystick_velocity_;
 
   // Drone and payload states
   struct DroneState {
@@ -103,6 +125,13 @@ private:
   Eigen::VectorXd desired_state_velocity_;
   Eigen::VectorXd desired_state_acceleration_;
 
+  // Joystick control state
+  bool joystick_mode_active_ = false;
+  Eigen::Vector3d joystick_velocity_command_;
+  Eigen::Vector3d joystick_reference_position_;
+  rclcpp::Time last_joystick_update_time_;
+  rclcpp::TimerBase::SharedPtr joystick_timer_;
+
   // ROS publishers
   std::vector<rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr>
       force_publishers_;
@@ -124,10 +153,14 @@ private:
       payload_odometry_subscriber_;
   rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr
       trajectory_subscriber_;
+  rclcpp::Subscription<ls2n_drone_interfaces::msg::Joystick>::SharedPtr
+      joystick_subscriber_;
 
   // ROS services
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr
       activate_armada_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_;
@@ -137,37 +170,9 @@ private:
   double initial_time_;
   double previous_loop_time_ = 0.0;
 
-  /**
-   * @brief Service callback to move the payload up
-   * @param request MoveUp request with distance and speed
-   * @param response MoveUp response with success status
-   */
-  void moveUpCallback(
-      const std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Request> request,
-      std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Response> response);
-
-  /**
-   * @brief Timer callback for updating z position during move up operation
-   */
-  void moveUpTimerCallback();
-
   // Controller state
   bool controller_active_ = false;
   bool simulation_mode_ = false;
-
-  // Move up operation state
-  bool move_up_active_ = false;
-  Eigen::Vector3d move_up_start_position_;
-  double move_up_target_distance_ = 0.0;
-  double move_up_speed_ = 0.0;
-  double move_up_start_time_ = 0.0;
-  double move_up_current_distance_ = 0.0;
-
-  // Timer for move up operation
-  rclcpp::TimerBase::SharedPtr move_up_timer_;
-
-  // Service for move up operation
-  rclcpp::Service<ls2n_drone_armada::srv::MoveUp>::SharedPtr move_up_service_;
 };
 
 #endif // ARMADA_CONTROLLER_NODE_HPP_

armada_controller/src/armada_controller_node.cpp

@@ -16,12 +16,14 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
   declare_parameter<double>("platform_mass", 1.0);
   declare_parameter<std::vector<double>>("drone_masses", {0.0});
   declare_parameter<bool>("simulation", false);
+  declare_parameter<double>("max_joystick_velocity", 0.5);
 
   // Get parameters
   auto drones_names_list = get_parameter("drones_to_control").as_string_array();
   cable_length_ = get_parameter("cable_length").as_double();
   payload_mass_ = get_parameter("platform_mass").as_double();
   simulation_mode_ = get_parameter("simulation").as_bool();
+  max_joystick_velocity_ = get_parameter("max_joystick_velocity").as_double();
 
   num_drones_ = static_cast<int>(drones_names_list.size());
   RCLCPP_INFO(get_logger(), "Controlling %d drones", num_drones_);
@@ -44,6 +46,10 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
   desired_state_velocity_ = Eigen::VectorXd::Zero(state_size);
   desired_state_acceleration_ = Eigen::VectorXd::Zero(state_size);
 
+  // Initialize joystick control 
+  joystick_velocity_command_ = Eigen::Vector3d::Zero();
+  joystick_reference_position_ = Eigen::Vector3d::Zero();
+
   // Create controller
   controller_ = std::make_unique<ArmadaController>(
       num_drones_, payload_mass_, drone_masses_, cable_length_);
@@ -91,6 +97,13 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
             this->trajectoryCallback(msg);
           });
 
+  // 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);
+      });
+
   // Publishers for visualization
   desired_orientation_publishers_.resize(num_drones_);
   current_orientation_publishers_.resize(num_drones_);
@@ -121,14 +134,20 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
         this->activateArmadaControl(request, response);
       });
 
-  // Add move up service
-  move_up_service_ = create_service<ls2n_drone_armada::srv::MoveUp>(
-      "/Armada/MoveUp",
-      [this](
-          const std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Request>
-              request,
-          std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Response> response) {
-        this->moveUpCallback(request, response);
+  // Joystick control service
+  activate_joystick_control_service_ = create_service<std_srvs::srv::Empty>(
+      "/Armada/ActivateJoystickControl",
+      [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+             std::shared_ptr<std_srvs::srv::Empty::Response> response) {
+        this->activateJoystickControl(request, response);
+      });
+
+  // Create joystick update timer (100Hz)
+  joystick_timer_ = create_wall_timer(10ms, 
+      [this]() {
+        if (joystick_mode_active_) {
+          this->joystickUpdateTimer();
+        }
       });
 }
 
@@ -172,6 +191,11 @@ void ArmadaControllerNode::payloadOdometryCallback(
 
 void ArmadaControllerNode::trajectoryCallback(
     const trajectory_msgs::msg::JointTrajectory::SharedPtr msg) {
+  // If joystick mode is active, ignore trajectory messages
+  if (joystick_mode_active_) {
+    return;
+  }
+
   // Check if there are points in the trajectory
   if (msg->points.empty()) {
     RCLCPP_WARN(get_logger(), "Received empty trajectory message");
@@ -244,6 +268,49 @@ void ArmadaControllerNode::trajectoryCallback(
   }
 }
 
+void ArmadaControllerNode::joystickCallback(
+    const ls2n_drone_interfaces::msg::Joystick::SharedPtr msg) {
+  if (!joystick_mode_active_) {
+    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_;
+  
+  // Use right stick for vertical movement
+  joystick_velocity_command_.z() = msg->axis_right_vertical * max_joystick_velocity_;
+}
+
+void ArmadaControllerNode::joystickUpdateTimer() {
+  // Calculate time since last update
+  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();
+  }
+  last_joystick_update_time_ = current_time;
+
+  // Update reference position based on current velocity command
+  joystick_reference_position_ += joystick_velocity_command_ * dt;
+
+  // Update desired state with new reference position
+  desired_state_(0) = joystick_reference_position_.x();
+  desired_state_(1) = joystick_reference_position_.y();
+  desired_state_(2) = joystick_reference_position_.z();
+
+  // Update velocity
+  desired_state_velocity_(0) = joystick_velocity_command_.x();
+  desired_state_velocity_(1) = joystick_velocity_command_.y();
+  desired_state_velocity_(2) = joystick_velocity_command_.z();
+
+  // Zero acceleration
+  desired_state_acceleration_(0) = 0.0;
+  desired_state_acceleration_(1) = 0.0;
+  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>) {
@@ -318,9 +385,45 @@ void ArmadaControllerNode::activateArmadaControl(
   }
 
   controller_active_ = true;
+  joystick_mode_active_ = false;
   RCLCPP_INFO(get_logger(), "Armada control 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");
+
+  // Check if controller is active
+  if (!controller_active_) {
+    RCLCPP_ERROR(get_logger(),
+                 "Cannot activate joystick control, armada controller not active");
+    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!");
+    return;
+  }
+
+  // Initialize joystick reference to current payload position
+  joystick_reference_position_ = payload_state_.position;
+  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!");
+}
+
 void ArmadaControllerNode::runController() {
   // Gather drone positions and velocities
   std::vector<Eigen::Vector3d> drone_positions;
@@ -370,108 +473,6 @@ void ArmadaControllerNode::sendDesiredForces(
   }
 }
 
-void ArmadaControllerNode::moveUpCallback(
-    const std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Request> request,
-    std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Response> response) {
-
-  RCLCPP_INFO(get_logger(), "Move up request: distance=%.2f, speed=%.2f",
-              request->distance, request->speed);
-
-  // Check if controller is active
-  if (!controller_active_) {
-    response->success = false;
-    response->message = "Armada controller is not active";
-    RCLCPP_ERROR(get_logger(), "Cannot move up: Armada controller not active");
-    return;
-  }
-
-  // Check if we already have a move up operation in progress
-  if (move_up_active_) {
-    response->success = false;
-    response->message = "Move up operation already in progress";
-    RCLCPP_ERROR(get_logger(),
-                 "Cannot start move up: operation already in progress");
-    return;
-  }
-
-  // Check parameters validity
-  if (request->distance <= 0.0) {
-    response->success = false;
-    response->message = "Distance must be positive";
-    RCLCPP_ERROR(get_logger(), "Invalid move up distance: %.2f",
-                 request->distance);
-    return;
-  }
-
-  if (request->speed <= 0.0) {
-    response->success = false;
-    response->message = "Speed must be positive";
-    RCLCPP_ERROR(get_logger(), "Invalid move up speed: %.2f", request->speed);
-    return;
-  }
-
-  // Store current position
-  move_up_start_position_ = payload_state_.position;
-  move_up_target_distance_ = request->distance;
-  move_up_speed_ = request->speed;
-  move_up_start_time_ = this->now().seconds();
-  move_up_current_distance_ = 0.0;
-  move_up_active_ = true;
-
-  // Create timer for 100Hz updates (10ms period)
-  move_up_timer_ = this->create_wall_timer(
-      std::chrono::milliseconds(10),
-      std::bind(&ArmadaControllerNode::moveUpTimerCallback, this));
-
-  response->success = true;
-  response->message = "Move up operation started";
-  RCLCPP_INFO(get_logger(),
-              "Move up operation started: target=%.2f, speed=%.2f",
-              request->distance, request->speed);
-}
-
-void ArmadaControllerNode::moveUpTimerCallback() {
-  // Check if we're still active
-  if (!move_up_active_ || !controller_active_) {
-    move_up_timer_->cancel();
-    move_up_active_ = false;
-    RCLCPP_INFO(get_logger(), "Move up operation stopped");
-    return;
-  }
-
-  // Calculate elapsed time
-  double current_time = this->now().seconds();
-  double elapsed_time = current_time - move_up_start_time_;
-
-  // Calculate new distance based on speed and time
-  move_up_current_distance_ = move_up_speed_ * elapsed_time;
-
-  // Check if we've reached the target
-  if (move_up_current_distance_ >= move_up_target_distance_) {
-    move_up_current_distance_ = move_up_target_distance_;
-    move_up_active_ = false;
-    move_up_timer_->cancel();
-    RCLCPP_INFO(get_logger(), "Move up operation completed");
-  }
-
-  // Update desired position
-  Eigen::Vector3d new_desired_position = move_up_start_position_;
-  new_desired_position.z() += move_up_current_distance_;
-
-  // Update desired velocity (only in z direction)
-  Eigen::Vector3d new_desired_velocity = Eigen::Vector3d::Zero();
-  if (move_up_active_) {
-    new_desired_velocity.z() = move_up_speed_;
-  }
-
-  // Update desired state
-  desired_state_.segment<3>(0) = new_desired_position;
-  desired_state_velocity_.segment<3>(0) = new_desired_velocity;
-
-  RCLCPP_DEBUG(get_logger(), "Move up progress: %.2f/%.2f meters",
-               move_up_current_distance_, move_up_target_distance_);
-}
-
 void ArmadaControllerNode::sendCurrentAndDesiredConfig() {
   // Get current state
   const auto &current_state = controller_->getCurrentState();

armada_controller/srv/MoveUp.srv

-# Request - parameters for moving up
-float64 distance  # The distance to move up in meters
-float64 speed     # The speed at which to move in meters/second
-
----
-# Response
-bool success      # Whether the command was accepted
-string message    # Additional information

launch/armada_control.launch.py

@@ -23,6 +23,12 @@ def generate_launch_description():
         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",
+        description="Maximum velocity in m/s for joystick control",
+    )
 
     return LaunchDescription(
         [
@@ -44,6 +50,7 @@ def generate_launch_description():
             ),
             platform_mass_launch_arg,
             drone_masses_launch_arg,
+            max_joystick_velocity_launch_arg,
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
                     [
@@ -78,7 +85,8 @@ def generate_launch_description():
                     "drones_to_control": ["Crazy2fly8", "Crazy2fly6", "Crazy2fly7"],
                     "cable_length": 1.6,
                     "platform_mass": LaunchConfiguration("platform_mass"),
-                    "drone_masses": [1.0, 1.0, 1.0]
+                    "drone_masses": [1.0, 1.0, 1.0],
+                    "max_joystick_velocity": LaunchConfiguration("max_joystick_velocity")
                 }],
             ),
             Node(

launch/sitl_armada_control.launch.py

@@ -72,7 +72,7 @@ def launch_setup(
     gazebo_world_path = os.path.join(
         get_package_share_directory("ls2n_drone_armada"),
         "worlds",
-        "armada_world_net.sdf",
+        "armada_world.sdf",
     )
 
     init_drone_poses = (
@@ -251,7 +251,7 @@ def generate_launch_description() -> LaunchDescription:
 
     cable_length_launch_arg = DeclareLaunchArgument(
         "cable_length",
-        default_value="3.0",
+        default_value="1.25",
         description="Length of a cable [m]",
     )