Merge remote-tracking branch 'origin/ros2-docking' into ros2-docking-…

…nav2

README.md

@@ -109,6 +109,7 @@ Launch arguments are largely common to both simulation and physical robot. Howev
 | 🤖🖥️  | `use_ekf`                    | Enable or disable EKF. <br/> ***bool:*** `True`                                                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `use_sim`                    | Whether simulation is used. <br/> ***bool:*** `False`                                                                                                                                                                                                                                                                             |
 | 🤖🖥️  | `user_led_animations_file`   | Path to a YAML file with a description of the user-defined animations. <br/> ***string:*** `''`                                                                                                                                                                                                                                                             |
+| 🤖🖥️  | `use_wibotic_info` |Whether Wibotic information is used. <br/>  **bool**: `True` |
 | 🤖🖥️  | `wheel_config_path`          | Path to wheel configuration file. <br/> ***string:*** [`{wheel_type}.yaml`](./panther_description/config)                                                                                                                                                                                                                                                   |
 | 🤖🖥️  | `wheel_type`                 | Type of wheel. If you choose a value from the preset options ('WH01', 'WH02', 'WH04'), you can ignore the 'wheel_config_path' and 'controller_config_path' parameters. For custom wheels, please define these parameters to point to files that accurately describe the custom wheels. <br/> ***string:*** `WH01` (choices: `WH01`, `WH02`, `WH04`, `custom`) |
 | 🖥️   | `x`                          | Initial robot position in the global 'x' axis. <br/> ***float:*** `0.0`                                                                                                                                                                                                                                                                                          |

panther/panther_hardware.repos

@@ -14,8 +14,12 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: b29f41ac00ab1a6fbac3e1d03602575094de277a
+    version: 0813c3eebec410c2635b1db3ab87b094c38658c6
   ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity
     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/panther_simulation.repos

@@ -14,7 +14,7 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: b29f41ac00ab1a6fbac3e1d03602575094de277a
+    version: 0813c3eebec410c2635b1db3ab87b094c38658c6
   ros2_controllers: # Caused by two error: 1. https://github.com/ros-controls/ros2_controllers/pull/1104 2. There is no nice way to change `sensor_name` imu_bradcaster param when spawning multiple robots -> ros2_control refer only to single imu entity
     type: git
     url: https://github.com/husarion/ros2_controllers/
@@ -23,3 +23,7 @@ repositories:
     type: git
     url: https://github.com/husarion/husarion_gz_worlds.git
     version: 9d514a09c74bca2afbfba76cf2c87134918bbf97
+  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>.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.
+- `panther_charging_dock.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.
+- `panther_charging_dock.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.
+- `panther_charging_dock.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.
+- `panther_charging_dock.staging_x_offset` [*double*, default: **-0.7**]: A staging pose is defined by offsetting a dock pose in axis X.
+- `panther_charging_dock.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.
+- `panther_charging_dock.use_wibotic_info` [*bool*, default: **True**]: Whether Wibotic information is used.
+- `panther_charging_dock.wibotic_info_timeout` [*double*, default: **1.5**]: A timeout in seconds to receive wibotic_info.

panther_docking/config/panther_docking_server.yaml

@@ -2,7 +2,7 @@
   ros__parameters:
     controller_frequency: 50.0
     initial_perception_timeout: 5.0
-    wait_charge_timeout: 5.0
+    wait_charge_timeout: 10.0
     dock_approach_timeout: 20.0
     undock_linear_tolerance: 0.08
     undock_angular_tolerance: 0.08
@@ -16,7 +16,7 @@
     dock_plugins: ["panther_charging_dock"]
     panther_charging_dock:
       plugin: panther_docking::PantherChargingDock
-      external_detection_timeout: 0.2
+      external_detection_timeout: 0.1
       docking_distance_threshold: 0.12
       docking_yaw_threshold: 0.2
       staging_x_offset: -0.8

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,27 @@ class PantherChargingDock : public opennav_docking_core::ChargingDock
    */
   void getParameters(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
 
-  void updateAndPublishStagingPose(const std::string & frame);
+  /**
+   * @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 Dock pose callback, used for external detection.
+   *
+   * @param pose The dock pose.
+   */
   void setDockPose(const PoseStampedMsg::SharedPtr pose);
 
+  /**
+   * @brief Wibotic info callback, used when `use_wibotic_info` is enabled.
+   *
+   * @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 +178,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 +195,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

@@ -64,9 +64,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="True",
+        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(
@@ -141,16 +155,29 @@ def generate_launch_description():
         launch_arguments={"namespace": namespace}.items(),
     )
 
+    wibotic_connector_can = Node(
+        package="wibotic_connector_can",
+        executable="wibotic_connector_can",
+        namespace=namespace,
+        emulate_tty=True,
+        arguments=["--ros-args", "--log-level", log_level, "--log-level", "rcl:=INFO"],
+        condition=IfCondition(
+            PythonExpression(["not ", use_sim, " and ", use_wibotic_info, " and ", use_docking])
+        ),
+    )
+
     return LaunchDescription(
         [
             declare_apriltag_config_path_arg,
-            declare_use_docking_arg,
             declare_docking_server_config_path_arg,
+            declare_use_docking_arg,
+            declare_use_wibotic_info_arg,
             declare_log_level,
             station_launch,
             docking_server_node,
             docking_server_activate_node,
             dock_pose_publisher,
             apriltag_node,
+            wibotic_connector_can,
         ]
     )

panther_docking/package.xml

@@ -17,6 +17,7 @@
   <depend>geometry_msgs</depend>
   <depend>nav2_util</depend>
   <depend>opennav_docking</depend>
+  <depend>panther_manager</depend>
   <depend>panther_utils</depend>
   <depend>pluginlib</depend>
   <depend>python3-pip</depend>
@@ -25,10 +26,12 @@
   <depend>sensor_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>
+  <depend>wibotic_msgs</depend>
 
   <exec_depend condition="$HUSARION_ROS_BUILD_TYPE == simulation">apriltag_ros</exec_depend>
   <exec_depend>nav2_lifecycle_manager</exec_depend>
   <exec_depend>python3-imageio</exec_depend>
+  <exec_depend>wibotic_connector_can</exec_depend>
   <exec_depend>xacro</exec_depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

panther_docking/src/panther_charging_dock.cpp

@@ -45,6 +45,10 @@ void PantherChargingDock::configure(
   declareParameters(node);
   getParameters(node);
 
+  if (!use_wibotic_info_) {
+    RCLCPP_INFO(logger_, "Wibotic info is disabled.");
+  }
+
   pose_filter_ = std::make_unique<opennav_docking::PoseFilter>(
     pose_filter_coef_, external_detection_timeout_);
 }
@@ -62,6 +66,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 +100,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,8 +119,12 @@ 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
 PantherChargingDock::PoseStampedMsg PantherChargingDock::getStagingPose(
   const geometry_msgs::msg::Pose & pose, const std::string & frame)
 {
@@ -166,10 +186,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; }
@@ -201,6 +248,11 @@ void PantherChargingDock::updateAndPublishStagingPose(const std::string & frame)
   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"