Add frames as params + improve comments

psdk_wrapper/CMakeLists.txt

@@ -22,7 +22,6 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
-find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(sensor_msgs REQUIRED)

psdk_wrapper/cfg/psdk_params.yaml

@@ -10,6 +10,11 @@
       hardware_connection: "DJI_USE_UART_AND_USB_BULK_DEVICE"
       uart_dev_1: "/dev/dji_serial"
       uart_dev_2: "/dev/dji_advanced_sensing"
+
+      imu_frame: "psdk_imu_link"
+      body_frame: "psdk_base_link"
+      map_frame: "psdk_map_enu"
+      gimbal_frame: "psdk_gimbal_link"
 
       data_frequency: # Options are: 1, 5, 10, 50, 100, 200, 400 Hz 
         imu: 100

psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp

@@ -215,6 +215,10 @@ class PSDKWrapper : public nav2_util::LifecycleNode
     std::string hardware_connection;
     std::string uart_dev_1;
     std::string uart_dev_2;
+    std::string imu_frame;
+    std::string body_frame;
+    std::string map_frame;
+    std::string gimbal_frame;
     int imu_frequency;
     int attitude_frequency;
     int acceleration_frequency;
@@ -865,8 +869,6 @@ class PSDKWrapper : public nav2_util::LifecycleNode
 
   /* Global variables*/
   PSDKParams params_;
-  std::string body_frame_{"base_link"};
-  std::string ground_frame_{"map"};
   rclcpp::Node::SharedPtr node_;
 
   int gps_signal_level_{0};

psdk_wrapper/src/modules/flight_control.cpp

@@ -65,11 +65,11 @@ PSDKWrapper::set_home_from_current_location_cb(
   {
     RCLCPP_ERROR(
         get_logger(),
-        "Could not set the home location using current aicraft location");
+        "Could not set the home location using current aicraft position");
     response->success = false;
     return;
   }
-  RCLCPP_INFO(get_logger(), "Home position has been set to current location!");
+  RCLCPP_INFO(get_logger(), "Home location has been set to current position!");
   response->success = true;
 }
 
@@ -84,7 +84,7 @@ PSDKWrapper::set_home_altitude_cb(
   {
     RCLCPP_ERROR(
         get_logger(),
-        "Could not set the home location using current aicraft location");
+        "Could not set the home altitude at the current aicraft location");
     response->success = false;
     return;
   }
@@ -104,7 +104,7 @@ PSDKWrapper::get_home_altitude_cb(
   {
     RCLCPP_ERROR(
         get_logger(),
-        "Could not set the home location using current aicraft location");
+        "Could not get the home location using current aicraft location");
     response->success = false;
     return;
   }

psdk_wrapper/src/modules/telemetry.cpp

@@ -246,7 +246,7 @@ PSDKWrapper::attitude_callback(const uint8_t *data, uint16_t dataSize,
 
   geometry_msgs::msg::QuaternionStamped quaternion_msg;
   quaternion_msg.header.stamp = this->get_clock()->now();
-  quaternion_msg.header.frame_id = body_frame_;
+  quaternion_msg.header.frame_id = params_.body_frame;
   quaternion_msg.quaternion.w = current_quat_FLU2ENU.getW();
   quaternion_msg.quaternion.x = current_quat_FLU2ENU.getX();
   quaternion_msg.quaternion.y = current_quat_FLU2ENU.getY();
@@ -266,7 +266,7 @@ PSDKWrapper::velocity_callback(const uint8_t *data, uint16_t dataSize,
           *reinterpret_cast<const T_DjiFcSubscriptionVelocity *>(data));
   geometry_msgs::msg::TwistStamped twist_msg;
   twist_msg.header.stamp = this->get_clock()->now();
-  twist_msg.header.frame_id = ground_frame_;
+  twist_msg.header.frame_id = params_.map_frame;
   /* Note: The y and x data is swapped to follow the REP103 convention and use
    * ENU representation. Original DJI twist msg is given as NEU.
    */
@@ -288,7 +288,7 @@ PSDKWrapper::imu_callback(const uint8_t *data, uint16_t dataSize,
           *reinterpret_cast<const T_DjiFcSubscriptionHardSync *>(data));
   sensor_msgs::msg::Imu imu_msg;
   imu_msg.header.stamp = this->get_clock()->now();
-  imu_msg.header.frame_id = body_frame_;
+  imu_msg.header.frame_id = params_.imu_frame;
   /* Note: The quaternion provided by DJI is in FRD body coordinate frame wrt.
    * to a NED ground coordinate frame. Following REP 103, this quaternion is
    * transformed in FLU in body frame wrt. to a ENU ground coordinate frame
@@ -340,7 +340,7 @@ PSDKWrapper::position_vo_callback(const uint8_t *data, uint16_t dataSize,
   tf2::Vector3 position_ENU = psdk_utils::R_NED2ENU * position_NED;
   psdk_interfaces::msg::PositionFused position_msg;
   position_msg.header.stamp = this->get_clock()->now();
-  position_msg.header.frame_id = ground_frame_;
+  position_msg.header.frame_id = params_.map_frame;
   position_msg.position.x = position_ENU.getX();
   position_msg.position.y = position_ENU.getY();
   position_msg.position.z = position_ENU.getZ();
@@ -411,7 +411,7 @@ PSDKWrapper::gps_velocity_callback(const uint8_t *data, uint16_t dataSize,
           *reinterpret_cast<const T_DjiFcSubscriptionGpsVelocity *>(data));
   geometry_msgs::msg::TwistStamped gps_velocity_msg;
   gps_velocity_msg.header.stamp = this->get_clock()->now();
-  gps_velocity_msg.header.frame_id = ground_frame_;
+  gps_velocity_msg.header.frame_id = params_.map_frame;
   // Convert cm/s given by dji topic to m/s
   gps_velocity_msg.twist.linear.x = gps_velocity->x / 100;
   gps_velocity_msg.twist.linear.y = gps_velocity->y / 100;

psdk_wrapper/src/psdk_wrapper.cpp

@@ -35,6 +35,11 @@ PSDKWrapper::PSDKWrapper(const std::string &node_name)
   declare_parameter("uart_dev_1", rclcpp::ParameterValue(""));
   declare_parameter("uart_dev_2", rclcpp::ParameterValue(""));
 
+  declare_parameter("imu_frame", rclcpp::ParameterValue("psdk_imu_link"));
+  declare_parameter("body_frame", rclcpp::ParameterValue("psdk_base_link"));
+  declare_parameter("map_frame", rclcpp::ParameterValue("psdk_map_enu"));
+  declare_parameter("gimbal_frame", rclcpp::ParameterValue("psdk_gimbal_link"));
+
   declare_parameter("data_frequency.imu", 1);
   declare_parameter("data_frequency.timestamp", 1);
   declare_parameter("data_frequency.attitude", 1);
@@ -336,6 +341,31 @@ PSDKWrapper::load_parameters()
   setenv(name, params_.uart_dev_2.c_str(), 1);
   RCLCPP_INFO(get_logger(), "Uart dev 2: %s", params_.uart_dev_2.c_str());
 
+  if (!get_parameter("imu_frame", params_.imu_frame))
+  {
+    RCLCPP_WARN(get_logger(),
+                "imu_frame param not defined, using default one: %s",
+                params_.imu_frame.c_str());
+  }
+  if (!get_parameter("body_frame", params_.body_frame))
+  {
+    RCLCPP_WARN(get_logger(),
+                "body_frame param not defined, using default one: %s",
+                params_.body_frame.c_str());
+  }
+  if (!get_parameter("map_frame", params_.map_frame))
+  {
+    RCLCPP_WARN(get_logger(),
+                "map_frame param not defined, using default one: %s",
+                params_.map_frame.c_str());
+  }
+  if (!get_parameter("gimbal_frame", params_.gimbal_frame))
+  {
+    RCLCPP_WARN(get_logger(),
+                "gimbal_frame param not defined, using default one: %s",
+                params_.gimbal_frame.c_str());
+  }
+
   // Get data frequency
   if (!get_parameter("data_frequency.imu", params_.imu_frequency))
   {