Ros2 docking charging dock wibotic msgs (#436)

* Fixed namespaces and updated ros_component_description Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Changed GetDockPose approach Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Cleanup charger Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added unit tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docs: Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fix upside down Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added updateAndPublishStagingPose method Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added suggestins | added reading docking_server parameter is dock pose publisher Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Remove redundant tests Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added reading wibotic_info Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added launching wibotic_connector_can Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added right condition to wibotic_connector_can Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * wibotic_ros is needed to build Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update repose, update parameters Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Added condition for use_docking Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * update repose Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Fixed use_wibotic_info param Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Rafal's suggestions applied Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * README update and comment delete Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update docs Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> * Update .repose Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com> --------- Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
husarion · Nov 19, 2024 · 0327ac4 · 0327ac4
1 parent 4ee2f68
commit 0327ac4
Show file tree

Hide file tree

Showing 12 changed files with 246 additions and 37 deletions.
diff --git a/README.md b/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`                                                                                                                                                                                                                                                                                          |

diff --git a/panther/panther_hardware.repos b/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
diff --git a/panther/panther_simulation.repos b/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
diff --git a/panther_docking/CMakeLists.txt b/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)

diff --git a/panther_docking/README.md b/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.
diff --git a/panther_docking/config/panther_docking_server.yaml b/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
@@ -17,10 +17,12 @@
     panther_charging_dock:
       plugin: panther_docking::PantherChargingDock
       external_detection_timeout: 0.1
-      docking_distance_threshold: 0.08
-      docking_yaw_threshold: 0.1
+      docking_distance_threshold: 0.12
+      docking_yaw_threshold: 0.15
       staging_x_offset: -0.5
       filter_coef: 0.1
+      use_wibotic_info: <use_wibotic_info_param>
+      wibotic_info_timeout: 1.0
 
     docks: ["main"]
     main:
@@ -32,5 +34,5 @@
       k_phi: 1.0
       k_delta: 2.0
       v_linear_min: 0.0
-      v_linear_max: 0.3
+      v_linear_max: 0.1
       v_angular_max: 0.15
diff --git a/panther_docking/include/panther_docking/panther_charging_dock.hpp b/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);
 
+  /**
+   * @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

diff --git a/panther_docking/launch/docking.launch.py b/panther_docking/launch/docking.launch.py
@@ -13,11 +13,14 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument  # , IncludeLaunchDescription
-from launch.conditions import IfCondition  # , UnlessCondition
-
-# from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import ReplaceString
@@ -52,9 +55,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(
@@ -102,29 +119,59 @@ def generate_launch_description():
         condition=IfCondition(use_docking),
     )
 
-    # FIXME: This launch does not work with the simulation. It can be caused by different versions of opencv
-    # station_launch = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource(
-    #         PathJoinSubstitution(
-    #             [
-    #                 FindPackageShare("panther_docking"),
-    #                 "launch",
-    #                 "station.launch.py",
-    #             ]
-    #         ),
-    #     ),
-    #     launch_arguments={"namespace": namespace}.items(),
-    #     condition=UnlessCondition(use_sim),
-    # )
+    station_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("panther_docking"),
+                    "launch",
+                    "station.launch.py",
+                ]
+            ),
+        ),
+        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])
+        ),
+    )
+
+    # FIXME: Remove before release
+    panther_manager_dir = FindPackageShare("panther_manager")
+    docking_manager_node = Node(
+        package="panther_manager",
+        executable="docking_manager_node",
+        name="docking_manager",
+        parameters=[
+            PathJoinSubstitution([panther_manager_dir, "config", "docking_manager.yaml"]),
+            {
+                "bt_project_path": PathJoinSubstitution(
+                    [panther_manager_dir, "behavior_trees", "DockingBT.btproj"]
+                )
+            },
+        ],
+        namespace=namespace,
+        emulate_tty=True,
+    )
 
     return LaunchDescription(
         [
             declare_use_docking_arg,
             declare_docking_server_config_path_arg,
             declare_log_level,
-            # station_launch,
+            declare_use_wibotic_info_arg,
+            station_launch,
             docking_server_node,
             docking_server_activate_node,
             dock_pose_publisher,
+            wibotic_connector_can,
+            docking_manager_node,
         ]
     )
diff --git a/panther_docking/launch/station.launch.py b/panther_docking/launch/station.launch.py
@@ -29,10 +29,11 @@
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
 from launch_ros.substitutions import FindPackageShare
-from moms_apriltag import TagGenerator2
 
 
 def generate_apriltag_and_get_path(tag_id):
+    from moms_apriltag import TagGenerator2
+
     tag_generator = TagGenerator2("tag36h11")
     tag_image = tag_generator.generate(tag_id, scale=1000)
 

diff --git a/panther_docking/package.xml b/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,9 +26,11 @@
   <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>
+  <exec_depend>wibotic_connector_can</exec_depend>
   <exec_depend>xacro</exec_depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>