Merge pull request #2 from aerostack2/psdk_ros2_v1.3.0

[as2_platform_dji_psdk] Update changes in launcher and params to psdk_ros2 v1.3.0

config/platform_config_file.yaml

@@ -1,8 +1,7 @@
 /**:
   ros__parameters:
-    cmd_freq: 30.0  # Hz (default: 100.0)
-    info_freq: 10.0  # Hz (default: 10.0)
-    enable_camera: false  # (default: false)
-    enable_gimbal: false  # (default: false)
-    gimbal_base_frame_id: "gimbal_base"  # (default: "gimbal_base")
-    tf_timeout_threshold: 0.05 # Default tf timeout (50ms)
+    cmd_freq: 30.0  # Command send frequency to the platform (default: 30.0 Hz)
+    info_freq: 10.0  # Info send frequency to ROS 2 (default: 10.0 Hz)
+    enable_camera: false  # Enable camera image publishing (default: false)
+    enable_gimbal: false  # Enable gimbal control (default: false)
+    tf_timeout_threshold: 0.05 # tf timeout (default: 0.05 s)

config/psdk_authentication.yaml

@@ -0,0 +1,8 @@
+/**:
+    ros__parameters:
+      app_name: "YourPSDK"
+      app_id: "YourID"
+      app_key: "YourAppKey"
+      app_license: "YourAppLicense"
+      developer_account: "YourDeveloperAccount"
+      baudrate: "921600"

config/psdk_params.yaml

@@ -0,0 +1,45 @@
+/**:
+    ros__parameters:
+      num_of_initialization_retries: 3
+
+      imu_frame: "imu_link"
+      body_frame: "base_link"
+      map_frame: "psdk_map_enu"
+      gimbal_base_frame: "gimbal_base"
+      gimbal_frame: "gimbal"
+      camera_frame: "camera_link"
+      publish_transforms: true
+
+      file_path: "/tmp/"
+
+      # Mandatory modules to be initialized. Mark with a true those which you 
+      # consider mandatory for your application, false otherwise. Be aware that 
+      # some modules might have inter-dependencies. Non mandatory modules will
+      # still be initialized but if case of failure, the node will continue to run.
+      mandatory_modules:
+        telemetry: true
+        flight_control: true
+        camera: false
+        gimbal: false
+        liveview: false
+        hms: false
+
+      data_frequency: # Options are: 1, 5, 10, 50, 100, 200, 400 Hz 
+        imu: 1
+        attitude: 10
+        acceleration: 10
+        velocity: 50
+        angular_velocity: 10
+        position: 50
+        altitude: 50
+        gps_fused_position: 50
+        gps_data: 1
+        rtk_data: 50
+        magnetometer: 1
+        rc_channels_data: 1
+        gimbal_data: 10
+        flight_status: 1
+        battery_level: 1
+        control_information: 1
+        esc_data_frequency: 1
+

include/as2_platform_dji_psdk/as2_platform_dji_psdk.hpp

@@ -92,7 +92,6 @@ class DJIMatricePSDKPlatform : public as2::AerialPlatform
   as2::tf::TfHandler tf_handler_;
   std::chrono::nanoseconds tf_timeout_;
   bool enable_gimbal_;
-  std::string gimbal_base_frame_id_;
   psdk_interfaces::msg::GimbalRotation gimbal_command_msg_;
   rclcpp::Time last_gimbal_command_time_;
 

launch/as2_platform_dji_psdk.launch.py

@@ -39,9 +39,8 @@
 from as2_core.launch_configuration_from_config_file import LaunchConfigurationFromConfigFile
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description() -> LaunchDescription:
@@ -53,8 +52,10 @@ def generate_launch_description() -> LaunchDescription:
     """
     # Get default platform configuration file
     package_folder = get_package_share_directory('as2_platform_dji_psdk')
+
     platform_config_file = os.path.join(package_folder,
                                         'config/platform_config_file.yaml')
+
     control_modes = PathJoinSubstitution([
         FindPackageShare('as2_platform_dji_psdk'),
         'config', 'control_modes.yaml'

launch/wrapper.launch.py → launch/psdk_wrapper.launch.py

@@ -35,13 +35,16 @@
 __license__ = 'BSD-3-Clause'
 __version__ = '0.1.0'
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
+import as2_core.launch_param_utils as as2_utils
 import launch
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, EmitEvent
-from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration
 from launch_ros.actions import LifecycleNode
 from launch_ros.events.lifecycle import ChangeState
-from launch_ros.substitutions import FindPackageShare
 import lifecycle_msgs.msg
 
 
@@ -52,21 +55,20 @@ def generate_launch_description() -> LaunchDescription:
     :return: Launch description
     :rtype: LaunchDescription
     """
-    # Declare the namespace launch argument
-    psdk_params_file = PathJoinSubstitution([
-        FindPackageShare('as2_platform_dji_psdk'),
-        'config', 'psdk_params.yaml'
-    ])
+    package_folder = get_package_share_directory(
+        'as2_platform_dji_psdk')
 
-    link_config_file = PathJoinSubstitution([
-        FindPackageShare('as2_platform_dji_psdk'),
-        'config', 'link_config.json'
-    ])
+    psdk_params_file = os.path.join(package_folder,
+                                    'config/psdk_params.yaml')
 
-    hms_return_codes_file = PathJoinSubstitution([
-        FindPackageShare('as2_platform_dji_psdk'),
-        'config', 'hms_2023_08_22.json'
-    ])
+    psdk_authentication_params_file = os.path.join(package_folder,
+                                                   'config/psdk_authentication_params.yaml')
+
+    link_config_file = os.path.join(package_folder,
+                                    'config/link_config.json')
+
+    hms_return_codes_file = os.path.join(package_folder,
+                                         'config/hms_2023_08_22.json')
 
     # Prepare the wrapper node
     wrapper_node = LifecycleNode(
@@ -77,12 +79,13 @@ def generate_launch_description() -> LaunchDescription:
         output='screen',
         emulate_tty=True,
         parameters=[
+            *as2_utils.launch_configuration('psdk_params_file',
+                                            default_value=psdk_params_file),
             {
                 'link_config_file_path': LaunchConfiguration('link_config_file_path'),
                 'hms_return_codes_path': LaunchConfiguration('hms_return_codes_path'),
-                'tf_frame_prefix': LaunchConfiguration('tf_frame_prefix'),
             },
-            LaunchConfiguration('psdk_params_file_path'),
+            LaunchConfiguration('psdk_authentication_params_file'),
         ],
         remappings=[
             ('psdk_ros2/gps_position_fused', 'sensor_measurements/gps'),
@@ -114,18 +117,19 @@ def generate_launch_description() -> LaunchDescription:
                               default_value=EnvironmentVariable(
                                   'AEROSTACK2_SIMULATION_DRONE_ID'),
                               description='Drone namespace'),
-        DeclareLaunchArgument('psdk_params_file_path',
-                              default_value=psdk_params_file,
-                              description='DJI PSDK configuration file'),
+        DeclareLaunchArgument('psdk_authentication_params_file',
+                              default_value=psdk_authentication_params_file,
+                              description='DJI PSDK authentication file'),
         DeclareLaunchArgument('link_config_file_path',
                               default_value=link_config_file,
                               description='DJI PSDK link configuration file'),
         DeclareLaunchArgument('hms_return_codes_path',
                               default_value=hms_return_codes_file,
                               description='Path to JSON file with known DJI return codes'),
-        DeclareLaunchArgument('tf_frame_prefix',
-                              default_value='',
-                              description='TF frame prefix'),
+        *as2_utils.declare_launch_arguments(
+            'psdk_params_file',
+            default_value=psdk_params_file,
+            description='Paremeters for DJI PSDK authentication'),
     ])
 
     # Declare Launch options

src/as2_platform_dji_psdk.cpp

@@ -156,10 +156,6 @@ void DJIMatricePSDKPlatform::configureSensors()
     if (!success) {
       RCLCPP_INFO(this->get_logger(), "Could not reset gimbal");
       enable_gimbal_ = false;
-    } else {
-      this->declare_parameter<std::string>("gimbal_base_frame_id", "gimbal_base");
-      this->get_parameter("gimbal_base_frame_id", gimbal_base_frame_id_);
-      gimbal_base_frame_id_ = as2::tf::generateTfName(this->get_namespace(), gimbal_base_frame_id_);
     }
   }
   last_gimbal_command_time_ = this->now();