Cleanup charger

Signed-off-by: Jakub Delicat <[email protected]>

panther_docking/config/panther_docking_server.yaml

@@ -16,8 +16,6 @@
     dock_plugins: ["panther_charging_dock"]
     panther_charging_dock:
       plugin: panther_docking::PantherChargingDock
-      base_frame: "<robot_namespace>/base_link"
-      odom_frame: "<robot_namespace>/odom"
       docking_distance_threshold: 0.08
       docking_yaw_threshold: 0.1
       staging_x_offset: -0.5

panther_docking/include/panther_docking/panther_charging_dock.hpp

@@ -139,38 +139,8 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
    */
   void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
 
-  /**
-   * @brief Offset the staging pose.
-   *
-   * This method offsets the staging dock pose by values described in a configuration.
-   *
-   * @param dock_pose The dock pose to offset.
-   *
-   * @return The offset staging pose.
-   */
-  PoseStampedMsg offsetStagingPoseToDockPose(const PoseStampedMsg & dock_pose);
-
-  /**
-   * @brief Offset the detected dock pose.
-   *
-   * This method offsets the detected dock pose by values described in a configuration.
-   *
-   * @param detected_dock_pose The detected dock pose to offset.
-   *
-   * @return The offset detected dock pose.
-   */
-  PoseStampedMsg offsetDetectedDockPose(const PoseStampedMsg & detected_dock_pose);
-
-  /**
-   * @brief Update the staging pose and publish it.
-   *
-   * This method makes all necessary transformations to update the staging pose and publishes it.
-   *
-   * @param frame The frame to publish the staging pose in.
-   */
-  void updateStagingPoseAndPublish(const std::string & frame);
-
   std::string base_frame_name_;
+  std::string fixed_frame_name_;
   std::string dock_frame_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("PantherChargingDock")};
@@ -196,8 +166,6 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
   double staging_yaw_offset_;
 
   double pose_filter_coef_;
-
-  builtin_interfaces::msg::Time request_detection_time_;
 };
 
 }  // namespace panther_docking

panther_docking/src/panther_charging_dock.cpp

@@ -75,7 +75,9 @@ void PantherChargingDock::deactivate()
 void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
 {
   nav2_util::declare_parameter_if_not_declared(
-    node, name_ + ".base_frame", rclcpp::ParameterValue("base_link"));
+    node, "base_frame", rclcpp::ParameterValue("base_link"));
+
+  nav2_util::declare_parameter_if_not_declared(node, "fixed_frame", rclcpp::ParameterValue("odom"));
 
   nav2_util::declare_parameter_if_not_declared(
     node, name_ + ".docking_distance_threshold", rclcpp::ParameterValue(0.05));
@@ -96,7 +98,8 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod
 
 void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
 {
-  node->get_parameter(name_ + ".base_frame", base_frame_name_);
+  node->get_parameter("base_frame", base_frame_name_);
+  node->get_parameter("fixed_frame", fixed_frame_name_);
 
   node->get_parameter(name_ + ".external_detection_timeout", external_detection_timeout_);
   node->get_parameter(name_ + ".docking_distance_threshold", docking_distance_threshold_);
@@ -110,16 +113,10 @@ void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::S
 PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose(
   const geometry_msgs::msg::Pose & pose, const std::string & frame)
 {
-  RCLCPP_DEBUG(logger_, "Getting staging pose");
+  RCLCPP_DEBUG_STREAM(logger_, "Getting staging pose in frame: " << frame);
 
   // When there is no global pose to reach thanks to nav2
   if (pose == geometry_msgs::msg::Pose()) {
-    dock_frame_ = frame;
-
-    if (dock_frame_.empty()) {
-      throw opennav_docking_core::FailedToControl("Cannot undock before docking!");
-    }
-
     const double yaw = tf2::getYaw(dock_pose_.pose.orientation);
     staging_pose_ = dock_pose_;
     staging_pose_.pose.position.x += cos(yaw) * staging_x_offset_;
@@ -134,13 +131,15 @@ PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose(
 bool PantherChargingDock::getRefinedPose(PoseStampedMsg & pose)
 {
   RCLCPP_DEBUG(logger_, "Getting refined pose");
+  rclcpp::Time request_detection_time;
+
   {
     auto node = node_.lock();
-    request_detection_time_ = node->now();
+    request_detection_time = node->now();
   }
 
   auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_);
-  auto duration = rclcpp::Time(request_detection_time_) - rclcpp::Time(dock_pose_.header.stamp);
+  auto duration = rclcpp::Time(request_detection_time) - rclcpp::Time(dock_pose_.header.stamp);
   if (duration > timeout) {
     RCLCPP_WARN_STREAM(
       logger_, "Lost detection or did not detect: timeout exceeded: " << duration.seconds());