Added reading wibotic_info

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

panther/panther_hardware.repos

@@ -19,3 +19,7 @@ repositories:
     type: git
     url: https://github.com/husarion/ros2_controllers/
     version: 9da42a07a83bbf3faf5cad11793fff22f25068af
+  wibotic_ros:
+    type: git
+    url: https://github.com/husarion/wibotic_ros/
+    version: 0.1.0

panther_docking/CMakeLists.txt

@@ -16,7 +16,8 @@ set(PACKAGE_DEPENDENCIES
     sensor_msgs
     std_srvs
     tf2_geometry_msgs
-    tf2_ros)
+    tf2_ros
+    wibotic_msgs)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)

panther_docking/README.md

@@ -46,8 +46,10 @@ The package contains a `PantherChargingDock` plugin for the [opennav_docking](ht
 
 - `base_frame` [*string*, default: **base_link**]: A base frame id of a robot.
 - `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot.
-- `<dock_type>.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id.
+- `<dock_type>.external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for dock pose.
 - `<dock_type>.docking_distance_threshold` [*double*, default: **0.05**]: A threshold of a distance between a robot pose and a dock pose to declare if docking succeed.
 - `<dock_type>.docking_yaw_threshold` [*double*, default: **0.3**]: A threshold of a difference of yaw angles between a robot pose and a dock pose to declare if docking succeed.
 - `<dock_type>.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X.
 - `<dock_type>.filter_coef` [*double*, default: **0.1**]: A key parameter that influences the trade-off between the filter's responsiveness and its smoothness, balancing how quickly it reacts to new pose data pose how much it smooths out fluctuations.
+- `<dock_type>.use_wibotic_info` [*bool*, default: **True**]: Use readings from `wibotic_info` topics to ensure that a robot is charging.
+- `<dock_type>.wibotic_info_timeout` [*double*, default: **0.2**]: A timeout in seconds for wibotic_info.

panther_docking/config/panther_docking_server.yaml

@@ -21,6 +21,8 @@
       docking_yaw_threshold: 0.1
       staging_x_offset: -0.5
       filter_coef: 0.1
+      use_wibotic: <use_wibotic_param>
+      wibotic_info_timeout: 0.5
 
     docks: ["main"]
     main:

panther_docking/include/panther_docking/panther_charging_dock.hpp

@@ -32,9 +32,13 @@
 #include <sensor_msgs/msg/battery_state.hpp>
 #include <std_srvs/srv/set_bool.hpp>
 
+#include "wibotic_msgs/msg/wibotic_info.hpp"
+
 namespace panther_docking
 {
 
+constexpr double kWiboticChargingCurrentThreshold = 0.0;
+
 /**
  * @class PantherChargingDock
  * @brief A class to represent a Panther charging dock.
@@ -45,6 +49,7 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
   using SharedPtr = std::shared_ptr<PantherChargingDock>;
   using UniquePtr = std::unique_ptr<PantherChargingDock>;
   using PoseStampedMsg = geometry_msgs::msg::PoseStamped;
+  using WiboticInfoMsg = wibotic_msgs::msg::WiboticInfo;
 
   /**
    * @brief Configure the dock with the necessary information.
@@ -140,10 +145,31 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
    */
   void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
 
+  /**
+   * @brief Method to update and publish the staging pose.
+   *
+   * Uses staging_x_offset_ and staging_yaw_offset_ to calculate the staging pose.
+   */
   void updateAndPublishStagingPose();
 
+  /**
+   * @brief Set the dock pose.
+   *
+   * This method sets the dock pose.  It can be used as a callback for a subscription.
+   *
+   * @param pose The dock pose.
+   */
   void setDockPose(const PoseStampedMsg::SharedPtr pose);
 
+  /**
+   * @brief Set the Wibotic info.
+   *
+   * This method sets the Wibotic info. It can be used as a callback for a subscription.
+   *
+   * @param msg The Wibotic info message.
+   */
+  void setWiboticInfo(const WiboticInfoMsg::SharedPtr msg);
+
   std::string base_frame_name_;
   std::string fixed_frame_name_;
   std::string dock_frame_;
@@ -156,9 +182,11 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
 
   rclcpp::Publisher<PoseStampedMsg>::SharedPtr staging_pose_pub_;
   rclcpp::Subscription<PoseStampedMsg>::SharedPtr dock_pose_sub_;
+  rclcpp::Subscription<WiboticInfoMsg>::SharedPtr wibotic_info_sub_;
 
   PoseStampedMsg dock_pose_;
   PoseStampedMsg staging_pose_;
+  WiboticInfoMsg::SharedPtr wibotic_info_;
 
   double external_detection_timeout_;
 
@@ -171,6 +199,9 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
   double staging_yaw_offset_;
 
   double pose_filter_coef_;
+
+  bool use_wibotic_info_;
+  double wibotic_info_timeout_;
 };
 
 }  // namespace panther_docking

panther_docking/launch/docking.launch.py

@@ -19,6 +19,7 @@
     EnvironmentVariable,
     LaunchConfiguration,
     PathJoinSubstitution,
+    PythonExpression,
 )
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -58,9 +59,23 @@ def generate_launch_description():
         choices=["debug", "info", "warning", "error"],
     )
 
+    use_wibotic_info = LaunchConfiguration("use_wibotic_info")
+    declare_use_wibotic_info_arg = DeclareLaunchArgument(
+        "use_wibotic_info",
+        default_value="False",
+        description="Whether Wibotic information is used",
+        choices=[True, False, "True", "False", "true", "false", "1", "0"],
+    )
+
     namespaced_docking_server_config = ReplaceString(
         source_file=docking_server_config_path,
-        replacements={"<robot_namespace>": namespace, "//": "/"},
+        replacements={
+            "<robot_namespace>": namespace,
+            "//": "/",
+            "<use_wibotic_info_param>": PythonExpression(
+                ["'false' if '", use_sim, "' else '", use_wibotic_info, "'"]
+            ),
+        },
     )
 
     docking_server_node = Node(
@@ -123,6 +138,7 @@ def generate_launch_description():
             declare_namespace_arg,
             declare_docking_server_config_path_arg,
             declare_log_level,
+            declare_use_wibotic_info_arg,
             station_launch,
             docking_server_node,
             docking_server_activate_node,

panther_docking/package.xml

@@ -25,6 +25,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>
+  <depend>wibotic_msgs</depend>
 
   <exec_depend>nav2_lifecycle_manager</exec_depend>
   <exec_depend>python3-imageio</exec_depend>

panther_docking/src/panther_charging_dock.cpp

@@ -62,6 +62,12 @@ void PantherChargingDock::activate()
     "docking/dock_pose", 1,
     std::bind(&PantherChargingDock::setDockPose, this, std::placeholders::_1));
   staging_pose_pub_ = node->create_publisher<PoseStampedMsg>("docking/staging_pose", 1);
+
+  if (use_wibotic_info_) {
+    wibotic_info_sub_ = node->create_subscription<WiboticInfoMsg>(
+      "wibotic_info", 1,
+      std::bind(&PantherChargingDock::setWiboticInfo, this, std::placeholders::_1));
+  }
 }
 
 void PantherChargingDock::deactivate()
@@ -90,6 +96,12 @@ void PantherChargingDock::declareParameters(const rclcpp_lifecycle::LifecycleNod
 
   nav2_util::declare_parameter_if_not_declared(
     node, name_ + ".filter_coef", rclcpp::ParameterValue(0.1));
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + ".use_wibotic_info", rclcpp::ParameterValue(true));
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name_ + ".wibotic_info_timeout", rclcpp::ParameterValue(1.5));
 }
 
 void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
@@ -103,6 +115,9 @@ void PantherChargingDock::getParameters(const rclcpp_lifecycle::LifecycleNode::S
   node->get_parameter(name_ + ".staging_x_offset", staging_x_offset_);
 
   node->get_parameter(name_ + ".filter_coef", pose_filter_coef_);
+
+  node->get_parameter(name_ + ".use_wibotic_info", use_wibotic_info_);
+  node->get_parameter(name_ + ".wibotic_info_timeout", wibotic_info_timeout_);
 }
 
 // When there is no pose actual position of robot is a staging pose
@@ -167,10 +182,37 @@ bool PantherChargingDock::isCharging()
 {
   RCLCPP_DEBUG(logger_, "Checking if charging");
   try {
-    return isDocked();
+    if (!use_wibotic_info_) {
+      return isDocked();
+    }
+
+    if (!wibotic_info_) {
+      throw opennav_docking_core::FailedToCharge("No Wibotic info received.");
+    }
+
+    rclcpp::Time requested_wibotic_info_time;
+    {
+      auto node = node_.lock();
+      requested_wibotic_info_time = node->now();
+    }
+
+    const auto duration = requested_wibotic_info_time - wibotic_info_->header.stamp;
+    if (duration > rclcpp::Duration::from_seconds(wibotic_info_timeout_)) {
+      RCLCPP_WARN_STREAM(
+        logger_, "Wibotic info is outdated. Time difference is: "
+                   << duration.seconds() << "s but timeout is " << wibotic_info_timeout_ << "s.");
+      return false;
+    }
+
+    if (wibotic_info_->i_charger > kWiboticChargingCurrentThreshold) {
+      return true;
+    }
   } catch (const opennav_docking_core::FailedToDetectDock & e) {
+    RCLCPP_ERROR_STREAM(logger_, "An occurred error while checking if charging: " << e.what());
     return false;
   }
+
+  return false;
 }
 
 bool PantherChargingDock::disableCharging() { return true; }
@@ -193,6 +235,11 @@ void PantherChargingDock::updateAndPublishStagingPose()
   staging_pose_pub_->publish(staging_pose_);
 }
 
+void PantherChargingDock::setWiboticInfo(const WiboticInfoMsg::SharedPtr msg)
+{
+  wibotic_info_ = std::make_shared<WiboticInfoMsg>(*msg);
+}
+
 }  // namespace panther_docking
 
 #include "pluginlib/class_list_macros.hpp"

panther_docking/test/unit/test_panther_charging_dock.cpp

@@ -30,9 +30,14 @@ static constexpr char kOdomFrame[] = "odom";
 class PantherChargingDockWrapper : public panther_docking::PantherChargingDock
 {
 public:
-  void setDockPose(geometry_msgs::msg::PoseStamped::SharedPtr pose)
+  void setDockPose(geometry_msgs::msg::PoseStamped::SharedPtr msg)
   {
-    panther_docking::PantherChargingDock::setDockPose(pose);
+    panther_docking::PantherChargingDock::setDockPose(msg);
+  }
+
+  void setWiboticInfo(wibotic_msgs::msg::WiboticInfo::SharedPtr msg)
+  {
+    panther_docking::PantherChargingDock::setWiboticInfo(msg);
   }
 };
 
@@ -44,6 +49,8 @@ class TestPantherChargingDock : public ::testing::Test
     const std::string & frame_id, const std::string & child_frame_id,
     const builtin_interfaces::msg::Time & stamp, const geometry_msgs::msg::Transform & transform);
 
+  void ActivateWiboticInfo();
+
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
   std::shared_ptr<PantherChargingDockWrapper> dock_;
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_pub;
@@ -78,6 +85,14 @@ void TestPantherChargingDock::SetTransform(
   tf_buffer_->setTransform(transform_stamped, "unittest", true);
 }
 
+void TestPantherChargingDock::ActivateWiboticInfo()
+{
+  node_->declare_parameter("dock.use_wibotic_info", true);
+  node_->declare_parameter("dock.wibotic_info_timeout", 1.0);
+  dock_->configure(node_, "dock", tf_buffer_);
+  dock_->activate();
+}
+
 TEST_F(TestPantherChargingDock, FailConfigureNoNode)
 {
   node_.reset();
@@ -190,6 +205,57 @@ TEST_F(TestPantherChargingDock, IsDocked)
   ASSERT_TRUE(dock_->isDocked());
 }
 
+TEST_F(TestPantherChargingDock, IsChargingNoWiboticInfo)
+{
+  ActivateWiboticInfo();
+  ASSERT_THROW({ dock_->isCharging(); }, opennav_docking_core::FailedToCharge);
+}
+
+TEST_F(TestPantherChargingDock, IsChargingTimeout)
+{
+  ActivateWiboticInfo();
+
+  wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info =
+    std::make_shared<wibotic_msgs::msg::WiboticInfo>();
+  dock_->setWiboticInfo(wibotic_info);
+  ASSERT_FALSE(dock_->isCharging());
+}
+
+TEST_F(TestPantherChargingDock, IsChargingCurrentZero)
+{
+  ActivateWiboticInfo();
+  wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info =
+    std::make_shared<wibotic_msgs::msg::WiboticInfo>();
+  wibotic_info->header.stamp = node_->now();
+  wibotic_info->i_charger = 0.0;
+
+  dock_->setWiboticInfo(wibotic_info);
+  ASSERT_FALSE(dock_->isCharging());
+}
+
+TEST_F(TestPantherChargingDock, IsChargingTimeoutWithGoodCurrent)
+{
+  ActivateWiboticInfo();
+  wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info =
+    std::make_shared<wibotic_msgs::msg::WiboticInfo>();
+  wibotic_info->i_charger = 0.1;
+
+  dock_->setWiboticInfo(wibotic_info);
+  ASSERT_FALSE(dock_->isCharging());
+}
+
+TEST_F(TestPantherChargingDock, IsChargingGoodCurrentWithoutTimeout)
+{
+  ActivateWiboticInfo();
+  wibotic_msgs::msg::WiboticInfo::SharedPtr wibotic_info =
+    std::make_shared<wibotic_msgs::msg::WiboticInfo>();
+  wibotic_info->i_charger = 0.1;
+  wibotic_info->header.stamp = node_->now();
+
+  dock_->setWiboticInfo(wibotic_info);
+  ASSERT_TRUE(dock_->isCharging());
+}
+
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);