Added wibotic station and wibotic reveiver (#54)

* Added wibotic station and wibotic reveiver Signed-off-by: Jakub Delicat <[email protected]> * Added wibotic receiver test Signed-off-by: Jakub Delicat <[email protected]> * Changed order Signed-off-by: Jakub Delicat <[email protected]> * Changed description of namespace Signed-off-by: Jakub Delicat <[email protected]> * Applied pre-comiit Signed-off-by: Jakub Delicat <[email protected]> --------- Signed-off-by: Jakub Delicat <[email protected]>
husarion · Sep 13, 2024 · 99e0189 · 99e0189
1 parent b29f563
commit 99e0189
Show file tree

Hide file tree

Showing 11 changed files with 378 additions and 34 deletions.
diff --git a/config/gz_stereolabs_zed_remappings.yaml b/config/gz_stereolabs_zed_remappings.yaml
@@ -1,31 +1,31 @@
 ---
 # https://github.com/stereolabs/zed-ros2-wrapper
-- ros_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/camera_info
-  gz_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/camera_info
-  ros_type_name: sensor_msgs/msg/CameraInfo
-  gz_type_name: ignition.msgs.CameraInfo
-  lazy: true
+  - ros_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/camera_info
+    gz_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/camera_info
+    ros_type_name: sensor_msgs/msg/CameraInfo
+    gz_type_name: ignition.msgs.CameraInfo
+    lazy: true
 
-- ros_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/image_rect_color
-  gz_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/image_rect_color
-  ros_type_name: sensor_msgs/msg/Image
-  gz_type_name: ignition.msgs.Image
-  lazy: true
+  - ros_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/image_rect_color
+    gz_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/image_rect_color
+    ros_type_name: sensor_msgs/msg/Image
+    gz_type_name: ignition.msgs.Image
+    lazy: true
 
-- ros_topic_name: <robot_namespace><device_namespace>/zed_node/camera_info
-  gz_topic_name: <robot_namespace><device_namespace>/zed_node/camera_info
-  ros_type_name: sensor_msgs/msg/CameraInfo
-  gz_type_name: ignition.msgs.CameraInfo
-  lazy: true
+  - ros_topic_name: <robot_namespace><device_namespace>/zed_node/camera_info
+    gz_topic_name: <robot_namespace><device_namespace>/zed_node/camera_info
+    ros_type_name: sensor_msgs/msg/CameraInfo
+    gz_type_name: ignition.msgs.CameraInfo
+    lazy: true
 
-- ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth
-  gz_topic_name: <robot_namespace><device_namespace>/zed_node/depth
-  ros_type_name: sensor_msgs/msg/Image
-  gz_type_name: ignition.msgs.Image
-  lazy: true
+  - ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth
+    gz_topic_name: <robot_namespace><device_namespace>/zed_node/depth
+    ros_type_name: sensor_msgs/msg/Image
+    gz_type_name: ignition.msgs.Image
+    lazy: true
 
-- ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth/points
-  gz_ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth/points
-  ros_type_name: sensor_msgs/msg/PointCloud2
-  gz_type_name: ignition.msgs.PointCloudPacked
-  lazy: true
+  - ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth/points
+    gz_ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth/points
+    ros_type_name: sensor_msgs/msg/PointCloud2
+    gz_type_name: ignition.msgs.PointCloudPacked
+    lazy: true
diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py
@@ -88,6 +88,7 @@ def get_launch_descriptions_from_yaml_node(
         "GRP02": "robotiq",
         # "GRP03": "robotiq", Waiting for release
         # https://github.com/PickNikRobotics/ros2_robotiq_gripper/blob/main/robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro
+        "WCH02": "wibotic_station",
     }
 
     for component in node["components"]:

diff --git a/launch/gz_orbbec_astra.launch.py b/launch/gz_orbbec_astra.launch.py
@@ -29,10 +29,10 @@ def fix_depth_image_tf(context, *args, **kwargs):
     robot_namespace = LaunchConfiguration("robot_namespace").perform(context)
     device_namespace = LaunchConfiguration("device_namespace").perform(context)
 
-    if robot_namespace.startswith('/'):
+    if robot_namespace.startswith("/"):
         robot_namespace = robot_namespace[1:] + "/"
 
-    if device_namespace.startswith('/'):
+    if device_namespace.startswith("/"):
         device_namespace = device_namespace[1:]
 
     parent_frame = robot_namespace + device_namespace + "_depth_optical_frame"

diff --git a/launch/gz_stereolabs_zed.launch.py b/launch/gz_stereolabs_zed.launch.py
@@ -13,12 +13,14 @@
 # limitations under the License.
 
 import os
+
+from ament_index_python import get_package_share_directory
+from launch_ros.actions import Node
+from nav2_common.launch import ReplaceString
+
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch_ros.actions import Node
 from launch.substitutions import EnvironmentVariable, LaunchConfiguration
-from nav2_common.launch import ReplaceString
-from ament_index_python import get_package_share_directory
 
 
 # The frame of the point cloud from ignition gazebo 6 isn't provided by <frame_id>.
@@ -27,14 +29,16 @@ def fix_depth_image_tf(context, *args, **kwargs):
     robot_namespace = LaunchConfiguration("robot_namespace").perform(context)
     device_namespace = LaunchConfiguration("device_namespace").perform(context)
 
-    if robot_namespace.startswith('/'):
+    if robot_namespace.startswith("/"):
         robot_namespace = robot_namespace[1:] + "/"
 
-    if device_namespace.startswith('/'):
+    if device_namespace.startswith("/"):
         device_namespace = device_namespace[1:]
 
     parent_frame = robot_namespace + device_namespace + "_center_optical_frame"
-    child_frame = "panther/base_link/" + robot_namespace + device_namespace + "_stereolabs_zed_depth"
+    child_frame = (
+        "panther/base_link/" + robot_namespace + device_namespace + "_stereolabs_zed_depth"
+    )
 
     static_transform_publisher = Node(
         package="tf2_ros",

diff --git a/launch/gz_wibotic_station.launch.py b/launch/gz_wibotic_station.launch.py
@@ -0,0 +1,109 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Husarion sp. z o.o.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+import yaml
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration
+
+
+def launch_setup(context, *args, **kwargs):
+    components_config_path = LaunchConfiguration("components_config_path").perform(context)
+    robot_namespace = LaunchConfiguration("robot_namespace")
+    device_namespace = LaunchConfiguration("device_namespace")
+
+    components_config = None
+    if components_config_path == "None":
+        return []
+
+    with open(os.path.join(components_config_path)) as file:
+        components_config = yaml.safe_load(file)
+
+    actions = []
+
+    for component in components_config["components"]:
+        component_type = component["type"]
+        if component_type == "WCH02":
+
+            component_xyz = [x for x in component["xyz"].split()]
+            component_rpy = [x for x in component["rpy"].split()]
+
+            spawn_station = Node(
+                package="ros_gz_sim",
+                executable="create",
+                arguments=[
+                    "-name",
+                    [robot_namespace, "_", device_namespace, "_station"],
+                    "-topic",
+                    "station_description",
+                    "-x",
+                    component_xyz[0],
+                    "-y",
+                    component_xyz[1],
+                    "-z",
+                    component_xyz[2],
+                    "-R",
+                    component_rpy[0],
+                    "-P",
+                    component_rpy[1],
+                    "-Y",
+                    component_rpy[2],
+                ],
+                namespace=robot_namespace,
+                emulate_tty=True,
+            )
+
+            actions.append(spawn_station)
+
+    return actions
+
+
+def generate_launch_description():
+    declare_components_config_path_arg = DeclareLaunchArgument(
+        "components_config_path",
+        default_value="None",
+        description=(
+            "Additional components configuration file. Components described in this file "
+            "are dynamically included in Panther's urdf."
+            "Panther options are described here "
+            "https://husarion.com/manuals/panther/panther-options/"
+        ),
+    )
+
+    declare_device_namespace = DeclareLaunchArgument(
+        "device_namespace",
+        default_value="",
+        description="Device namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.",
+    )
+
+    declare_robot_namespace = DeclareLaunchArgument(
+        "robot_namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Namespace which will appear in front of all topics (including /tf and /tf_static).",
+    )
+
+    return LaunchDescription(
+        [
+            declare_components_config_path_arg,
+            declare_device_namespace,
+            declare_robot_namespace,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )
diff --git a/meshes/wibotic_receiver.stl b/meshes/wibotic_receiver.stl
diff --git a/meshes/wibotic_station.stl b/meshes/wibotic_station.stl
diff --git a/test/test_components_xacro.py b/test/test_components_xacro.py
@@ -49,7 +49,8 @@
     "MAN07": ["kinova_gen3_7dof", "base_link", "camera_color_frame", "camera_sensor"],
     # "GRP01": [], not implemented in robotiq_description
     "GRP02": ["robotiq", "robotiq_85_base_link", "", ""],
-    # "GRP03": ["robotiq", "robotiq_140_base_link", "", ""], not implemented in robotiq_description
+    # "GRP03": ["robotiq", "robotiq_140_base_link", "", ""], not implemented in robotiq_description,
+    "WCH01": ["wibotic_receiver", "wibotic_receiver_link", "", ""],
 }
 
 

diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro
@@ -268,6 +268,20 @@
         />
       </xacro:if>
 
+      <xacro:if value="${type == 'WCH01'}">
+        <xacro:include
+          filename="$(find ros_components_description)/urdf/wibotic_receiver.urdf.xacro"
+          ns="wireless_charger" />
+
+        <xacro:wireless_charger.wibotic_receiver
+          parent_link="${parent_link}"
+          xyz="${xyz}"
+          rpy="${rpy}"
+          namespace="${ns}"
+          device_namespace="${device_namespace}"
+        />
+      </xacro:if>
+
       <xacro:if value="${index}">
         <xacro:load_componenet
           counter="${index}"

diff --git a/urdf/wibotic_receiver.urdf.xacro b/urdf/wibotic_receiver.urdf.xacro
@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+
+<!--
+# Copyright 2024 Husarion. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+-->
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="wibotic_receiver"
+    params="parent_link xyz rpy
+                         namespace:=None
+                          device_namespace:=None">
+
+
+    <xacro:if value="${namespace == 'None'}">
+      <xacro:property name="ns" value="" />
+    </xacro:if>
+    <xacro:unless value="${namespace == 'None'}">
+      <xacro:property name="ns" value="${namespace}/" />
+    </xacro:unless>
+
+    <xacro:if value="${device_namespace == 'None'}">
+      <xacro:property name="device_ns" value="" />
+    </xacro:if>
+    <xacro:unless value="${device_namespace == 'None'}">
+      <xacro:property name="device_ns" value="${device_namespace}/" />
+    </xacro:unless>
+
+    <xacro:if value="${device_namespace == 'None'}">
+      <xacro:property name="prefix" value="" />
+    </xacro:if>
+    <xacro:unless value="${device_namespace == 'None'}">
+      <xacro:property name="prefix" value="${device_namespace}_" />
+    </xacro:unless>
+
+    <joint name="${parent_link.rstrip('_link')}_to_${prefix}wibotic_receiver_joint" type="fixed">
+      <origin xyz="${xyz}" rpy="${rpy}" />
+      <parent link="${parent_link}" />
+      <child link="${prefix}wibotic_receiver_link" />
+
+    </joint>
+
+    <link name="${prefix}wibotic_receiver_link">
+      <visual>
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 ${pi/2.0}" />
+        <geometry>
+          <mesh filename="package://ros_components_description/meshes/wibotic_receiver.stl" />
+        </geometry>
+        <material name="Grey">
+          <color rgba="0.15 0.15 0.15 1.0" />
+        </material>
+      </visual>
+
+      <collision>
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 ${pi/2.0}" />
+        <geometry>
+          <mesh filename="package://ros_components_description/meshes/wibotic_receiver.stl" />
+        </geometry>
+      </collision>
+    </link>
+
+  </xacro:macro>
+</robot>