diff --git a/.gitignore b/.gitignore index 2c0b9fe6..6307ca45 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,5 @@ docs/_build/doctrees .pre-commit-config.yaml .vscode/ -build \ No newline at end of file +build +.gitignore \ No newline at end of file diff --git a/README.md b/README.md index 29e7560b..12b9007b 100644 --- a/README.md +++ b/README.md @@ -26,3 +26,62 @@ Please notice that this wrapper is still a work in progress. While we aim to mak `psdk_ros2` wrapper is under the [Mozilla Public License Version 2.0](https://github.com/umdlife/psdk_ros2/blob/main/LICENSE.md). \ Please note that the [DJI Payload-SDK](https://github.com/dji-sdk/Payload-SDK) libraries which are needed to run this wrapper use MIT License. + + +--- + +Here's the step-by-step instructions for running the PSDK wrapper as a non-root user without requiring a password. + + +## Running PSDK Wrapper as a Non-Root User + +To run the PSDK wrapper as a non-root user, switch to the `non_root_user` branch and configure it according to the instructions provided in the [PSDK ROS 2 Wiki](https://umdlife.github.io/psdk_ros2/index.html). However, running the wrapper in this way will still prompt for a password. + +To avoid entering the password each time, follow the instructions below to modify the sudoers file so that specific commands can be executed without a password. + +### Step-by-Step Instructions + +1. **Backup the sudoers File** + Before making any changes, back up the existing sudoers file as a precaution: + ```bash + sudo cp /etc/sudoers /etc/sudoers.bak + ``` + +2. **Edit the sudoers File** + You should never edit the sudoers file directly with a regular text editor. Instead, use `visudo`, which checks for syntax errors before saving: + ```bash + sudo visudo + ``` + +3. **Modify the File** + Inside the editor, find the section that looks like this: + ```bash + # Allow members of group sudo to execute any command + sudo ALL=(ALL:ALL) ALL + ``` + Replace it with the following line to allow members of the sudo group to run commands without a password: + ```bash + sudo ALL=(ALL:ALL) NOPASSWD: ALL + ``` + +4. **Save and Exit** + - If `visudo` opens with Vim (default editor), press `Esc`, type `:wq`, and press Enter to save and exit. + - If `visudo` opens with Nano, press `Ctrl + O` to save, then Enter, and `Ctrl + X` to exit. + + +5. **Testing the Configuration** + Open a new terminal and run a command that typically requires a password, such as: + ```bash + sudo whoami + ``` + If the command runs without prompting for a password, the configuration is correct. + +6. **Run the PSDK Wrapper** + Now, try launching the PSDK wrapper. It should initialize as a non-root user without requiring a password. + ```bash + ros2 launch psdk_wrapper wrapper.launch.py + ``` + +--- + +This configuration allows members of the sudo group to execute commands without needing to enter a password, streamlining the process of running the PSDK wrapper. If you encounter any issues, revert to the backed-up sudoers file (`/etc/sudoers.bak`) and review the changes. \ No newline at end of file diff --git a/psdk_wrapper/CMakeLists.txt b/psdk_wrapper/CMakeLists.txt index 5ab540d9..63ca6730 100644 --- a/psdk_wrapper/CMakeLists.txt +++ b/psdk_wrapper/CMakeLists.txt @@ -39,8 +39,8 @@ include(FetchContent) # Declare the content to fetch FetchContent_Declare( Payload_SDK - GIT_REPOSITORY https://github.com/dji-sdk/Payload-SDK.git - GIT_TAG 3.8.1 + GIT_REPOSITORY https://github.com/bitcurious/Payload-SDK.git + GIT_TAG non-root-user ) FetchContent_GetProperties(Payload_SDK) diff --git a/psdk_wrapper/include/psdk_wrapper/modules/flight_control.hpp b/psdk_wrapper/include/psdk_wrapper/modules/flight_control.hpp index 540e042e..64f0a553 100644 --- a/psdk_wrapper/include/psdk_wrapper/modules/flight_control.hpp +++ b/psdk_wrapper/include/psdk_wrapper/modules/flight_control.hpp @@ -149,9 +149,8 @@ class FlightControlModule : public rclcpp_lifecycle::LifecycleNode * @brief Callback function to exposing a generic control method of the * aircraft.The type of commands as well as the reference frame is specified * in a flag within the msg. - * @param msg sensor_msgs::msg::Joy. Axes represent the x, y, z and yaw + * @param msg sensor_msgs::msg::Joy. Axes represent the x, y, z, yaw and flag * command. - * @note This type of control is not implemented at this moment. */ void flight_control_generic_cb(const sensor_msgs::msg::Joy::SharedPtr msg); @@ -390,6 +389,26 @@ class FlightControlModule : public rclcpp_lifecycle::LifecycleNode const std::shared_ptr request, const std::shared_ptr response); + /** + *@brief Sets horizontal control mode as per flag, updates x, y, and z + * values and sets Joystick mode. + */ + void set_horizontal_mode(float x_cmd, float y_cmd, float z_cmd, float yaw_cmd, + uint8_t flag); + + /** + * @brief Sets vertical control mode as per, flag updates x, y, and z + * values and sets Joystick mode. + */ + void set_vertical_mode(float x_cmd, float y_cmd, float z_cmd, float yaw_cmd, + uint8_t flag); + /** + *@brief Sets yaw control mode as per flag, updates yaw + * value and sets Joystick mode. + */ + void set_yaw_mode(float x_cmd, float y_cmd, float z_cmd, float yaw_cmd, + uint8_t flag); + /* ROS 2 Subscribers */ rclcpp::Subscription::SharedPtr flight_control_generic_sub_; @@ -440,6 +459,26 @@ class FlightControlModule : public rclcpp_lifecycle::LifecycleNode get_horizontal_radar_obstacle_avoidance_srv_; bool is_module_initialized_{false}; + + T_DjiFlightControllerJoystickMode joystick_mode; + float x_cmd, y_cmd, z_cmd, yaw_cmd; + + // Decode values for control flag. + enum Control + { + FRAME_GROUND = 0x00, + FRAME_BODY = 0x02, + HORIZONTAL_VELOCITY = 0x40, + HORIZONTAL_POSITION = 0x80, + HORIZONTAL_ANGLE = 0x00, + VERTICAL_VELOCITY = 0x00, + VERTICAL_POSITION = 0x10, + VERTICAL_THRUST = 0x20, + YAW_ANGLE = 0x00, + YAW_RATE = 0x08, + }; + + uint8_t flag; }; } // namespace psdk_ros2 diff --git a/psdk_wrapper/src/modules/flight_control.cpp b/psdk_wrapper/src/modules/flight_control.cpp index 69eba145..8adbb712 100644 --- a/psdk_wrapper/src/modules/flight_control.cpp +++ b/psdk_wrapper/src/modules/flight_control.cpp @@ -22,7 +22,6 @@ namespace psdk_ros2 { - FlightControlModule::FlightControlModule(const std::string &name) : rclcpp_lifecycle::LifecycleNode( name, "", @@ -903,10 +902,241 @@ void FlightControlModule::flight_control_generic_cb( const sensor_msgs::msg::Joy::SharedPtr msg) { - /** @todo implemnent generic control functionality */ + /** @author Umesh Mane + * Contact: umeshmane280@gmail.com + */ + (void)msg; - RCLCPP_WARN(get_logger(), - "Generic control setpoint is not currently implemented!"); + float x_setpoint = msg->axes[0]; + float y_setpoint = msg->axes[1]; + float z_setpoint = msg->axes[2]; + float yaw_setpoint = msg->axes[3]; + flag = (uint8_t)(msg->axes[4]); + // uint8_t flag = msg->axes[4]; + + uint8_t FRAME = (flag & 0x06); + uint8_t HOLD = (flag & 0x01); + // Set coordinate mode + if (FRAME == Control::FRAME_GROUND) + { + RCLCPP_INFO(get_logger(), "GROUND_FRAME"); + joystick_mode.horizontalCoordinate = + DJI_FLIGHT_CONTROLLER_HORIZONTAL_GROUND_COORDINATE; + } + else if (FRAME == Control::FRAME_BODY) + { + RCLCPP_INFO(get_logger(), "BODY_FRAME"); + joystick_mode.horizontalCoordinate = + DJI_FLIGHT_CONTROLLER_HORIZONTAL_BODY_COORDINATE; + } + // set horizontal mode + set_horizontal_mode(x_setpoint, y_setpoint, z_setpoint, yaw_setpoint, flag); + // set vertical mode + set_vertical_mode(x_setpoint, y_setpoint, z_setpoint, yaw_setpoint, flag); + // set yaw mode + set_yaw_mode(x_setpoint, y_setpoint, z_setpoint, yaw_setpoint, flag); + + // Set stable control mode + joystick_mode.stableControlMode = + DJI_FLIGHT_CONTROLLER_STABLE_CONTROL_MODE_ENABLE; + + // Call the function to set the joystick mode + DjiFlightController_SetJoystickMode(joystick_mode); + + T_DjiFlightControllerJoystickCommand joystick_command = {x_cmd, y_cmd, z_cmd, + yaw_cmd}; + DjiFlightController_ExecuteJoystickAction(joystick_command); +} + +void +FlightControlModule::set_horizontal_mode(float x_setpoint, float y_setpoint, + float z_setpoint, float yaw_setpoint, + uint8_t flag) +{ + uint8_t FRAME = (flag & 0x06); + uint8_t HORI = (flag & 0xC0); + // float x_cmd, y_cmd, z_cmd, yaw_cmd; + if (FRAME == Control::FRAME_GROUND) + { + RCLCPP_INFO(get_logger(), "HORIZONTAL_GROUND"); + // 1.1 Horizontal channels + if ((HORI == Control::HORIZONTAL_VELOCITY) || + (HORI == Control::HORIZONTAL_POSITION)) + { + x_cmd = y_setpoint; + y_cmd = x_setpoint; + if (HORI == Control::HORIZONTAL_VELOCITY) + { + RCLCPP_INFO(get_logger(), "HORIZONTAL_VELOCITY"); + joystick_mode.horizontalControlMode = + DJI_FLIGHT_CONTROLLER_HORIZONTAL_VELOCITY_CONTROL_MODE; + } + if (HORI == Control::HORIZONTAL_POSITION) + { + RCLCPP_INFO(get_logger(), "HORIZONTAL_POSITION"); + joystick_mode.horizontalControlMode = + DJI_FLIGHT_CONTROLLER_HORIZONTAL_POSITION_CONTROL_MODE; + } + } + else + { + // GROUND frame is specified, but angle and rate command is generated in + // body frame + // Transform from FL to FR + x_cmd = psdk_utils::rad_to_deg(x_setpoint); + y_cmd = psdk_utils::rad_to_deg(-y_setpoint); + if (HORI == Control::HORIZONTAL_ANGLE) + { + RCLCPP_INFO(get_logger(), "HORIZONTAL_ANGLE"); + joystick_mode.horizontalControlMode = + DJI_FLIGHT_CONTROLLER_HORIZONTAL_ANGLE_CONTROL_MODE; + } + } + } + else if (FRAME == Control::FRAME_BODY) + { + RCLCPP_INFO(get_logger(), "HORIZONTAL_BODY"); + // 2.1 Horizontal channels + if ((HORI == Control::HORIZONTAL_VELOCITY) || + (HORI == Control::HORIZONTAL_POSITION)) + { + // The X and Y Vel and Pos should be only based on rotation after Yaw, + // whithout roll and pitch. Otherwise the behavior will be weird. + // Transform from F-R to F-L + x_cmd = x_setpoint; + y_cmd = -y_setpoint; + if (HORI == Control::HORIZONTAL_VELOCITY) + { + RCLCPP_INFO(get_logger(), "HORIZONTAL_VELOCITY"); + joystick_mode.horizontalControlMode = + DJI_FLIGHT_CONTROLLER_HORIZONTAL_VELOCITY_CONTROL_MODE; + } + if (HORI == Control::HORIZONTAL_POSITION) + { + RCLCPP_INFO(get_logger(), "HORIZONTAL_POSITION"); + joystick_mode.horizontalControlMode = + DJI_FLIGHT_CONTROLLER_HORIZONTAL_POSITION_CONTROL_MODE; + } + } + else + { + x_cmd = psdk_utils::rad_to_deg(x_setpoint); + y_cmd = psdk_utils::rad_to_deg(-y_setpoint); + if (HORI == Control::HORIZONTAL_ANGLE) + { + RCLCPP_INFO(get_logger(), "HORIZONTAL_ANGLE"); + joystick_mode.horizontalControlMode = + DJI_FLIGHT_CONTROLLER_HORIZONTAL_ANGLE_CONTROL_MODE; + } + } + } +} + +void +FlightControlModule::set_vertical_mode(float x_setpoint, float y_setpoint, + float z_setpoint, float yaw_setpoint, + uint8_t flag) +{ + uint8_t FRAME = (flag & 0x06); + uint8_t VERT = (flag & 0x30); + if (FRAME == Control::FRAME_GROUND) + { + RCLCPP_INFO(get_logger(), "VERTICAL_GROUND"); + // 1.2 Verticle Channel + if ((VERT == Control::VERTICAL_VELOCITY) || + (VERT == Control::VERTICAL_POSITION)) + { + z_cmd = z_setpoint; + if (VERT == Control::VERTICAL_VELOCITY) + { + RCLCPP_INFO(get_logger(), "VERTICAL_VELOCITY"); + joystick_mode.verticalControlMode = + DJI_FLIGHT_CONTROLLER_VERTICAL_VELOCITY_CONTROL_MODE; + } + if (VERT == Control::VERTICAL_POSITION) + { + RCLCPP_INFO(get_logger(), "VERTICAL_POSITION"); + joystick_mode.verticalControlMode = + DJI_FLIGHT_CONTROLLER_VERTICAL_POSITION_CONTROL_MODE; + } + } + else + { + // GROUND frame is specified, but thrust command is generated in body + // frame + z_cmd = z_setpoint; + if (VERT == Control::VERTICAL_THRUST) + { + RCLCPP_INFO(get_logger(), "VERTICAL_THRUST"); + joystick_mode.verticalControlMode = + DJI_FLIGHT_CONTROLLER_VERTICAL_THRUST_CONTROL_MODE; + } + } + } + else if (FRAME == Control::FRAME_BODY) + { + RCLCPP_INFO(get_logger(), "VERTICAL_BODY"); + // 2.2 Vertical channel + if ((VERT == Control::VERTICAL_VELOCITY) || + (VERT == Control::VERTICAL_POSITION)) + { + // BODY frame is specified, but hight and z-velocity is generated in + // ground frame + z_cmd = z_setpoint; + if (VERT == Control::VERTICAL_VELOCITY) + { + RCLCPP_INFO(get_logger(), "VERTICAL_VELOCITY"); + joystick_mode.verticalControlMode = + DJI_FLIGHT_CONTROLLER_VERTICAL_VELOCITY_CONTROL_MODE; + } + if (VERT == Control::VERTICAL_POSITION) + { + RCLCPP_INFO(get_logger(), "VERTICAL_POSITION"); + joystick_mode.verticalControlMode = + DJI_FLIGHT_CONTROLLER_VERTICAL_POSITION_CONTROL_MODE; + } + } + else + { + z_cmd = z_setpoint; + if (VERT == Control::VERTICAL_THRUST) + { + RCLCPP_INFO(get_logger(), "VERTICAL_THRUST"); + joystick_mode.verticalControlMode = + DJI_FLIGHT_CONTROLLER_VERTICAL_THRUST_CONTROL_MODE; + } + } + } +} + +void +FlightControlModule::set_yaw_mode(float x_setpoint, float y_setpoint, + float z_setpoint, float yaw_setpoint, + uint8_t flag) +{ + uint8_t FRAME = (flag & 0x06); + uint8_t YAW = (flag & 0x08); + if (YAW == Control::YAW_ANGLE) + { + tf2::Matrix3x3 rotation_FLU2ENU; + rotation_FLU2ENU.setRPY(0.0, 0.0, yaw_setpoint); + tf2::Matrix3x3 rotation_FRD2NED(psdk_utils::R_NED2ENU.transpose() * + rotation_FLU2ENU * + psdk_utils::R_FLU2FRD.transpose()); + double temp1, temp2, temp_yaw; + rotation_FRD2NED.getRPY(temp1, temp2, temp_yaw); + yaw_cmd = static_cast(temp_yaw); + yaw_cmd = psdk_utils::rad_to_deg(yaw_cmd); + RCLCPP_INFO(get_logger(), "YAW_ANGLE"); + joystick_mode.yawControlMode = DJI_FLIGHT_CONTROLLER_YAW_ANGLE_CONTROL_MODE; + } + else if (YAW == Control::YAW_RATE) + { + yaw_cmd = psdk_utils::rad_to_deg(-yaw_setpoint); + RCLCPP_INFO(get_logger(), "YAW_RATE"); + joystick_mode.yawControlMode = + DJI_FLIGHT_CONTROLLER_YAW_ANGLE_RATE_CONTROL_MODE; + } } void