Added wibotic station and wibotic reveiver

Signed-off-by: Jakub Delicat <[email protected]>
husarion · Sep 5, 2024 · a4b7fc6 · a4b7fc6
1 parent b29f563
commit a4b7fc6
Show file tree

Hide file tree

Showing 7 changed files with 307 additions and 0 deletions.
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_wibotic_station.launch.py b/launch/gz_wibotic_station.launch.py
@@ -0,0 +1,111 @@
+#!/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 import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import (
+    EnvironmentVariable,
+    LaunchConfiguration,
+)
+from launch_ros.actions import Node
+
+
+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="Sensor 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/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,57 @@
+<?xml version="1.0"?>
+<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>
diff --git a/urdf/wibotic_station.urdf.xacro b/urdf/wibotic_station.urdf.xacro
@@ -0,0 +1,124 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="wibotic_station">
+  <xacro:arg name="namespace" default="" />
+  <xacro:arg name="apriltag_image_path" default="" />
+  <xacro:arg name="apriltag_size" default="0.15" />
+
+  <xacro:macro name="wibotic_station"
+    params="namespace:=None parent_link xyz rpy">
+
+    <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>
+
+    <joint name="${parent_link.rstrip('_link')}_to_wibotic_station_link_joint" type="fixed">
+      <origin xyz="${xyz}" rpy="${rpy}" />
+      <parent link="${parent_link}" />
+      <child link="wibotic_station_link" />
+    </joint>
+
+    <link name="wibotic_station_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_station.stl" />
+        </geometry>
+        <material name="LightGrey">
+          <color rgba="0.6 0.6 0.6 1.0" />
+        </material>
+      </visual>
+
+      <!-- Fake collision fit to STL -->
+      <collision>
+        <origin xyz="-0.2 0.0  0.4" rpy="0.0 0.0 0.0" />
+        <geometry>
+          <box size="0.4 0.5 0.8" />
+        </geometry>
+      </collision>
+
+      <!-- Fake inertia -->
+      <inertial>
+        <origin xyz="-0.2 0.0 0.0" rpy="0.0 0.0 0.0" />
+        <mass value="15.0" />
+        <inertia ixx="0.001111" ixy="0.0" ixz="0.0"
+          iyy="0.001111" iyz="0.0"
+          izz="0.001111" />
+      </inertial>
+    </link>
+
+  </xacro:macro>
+
+  <xacro:macro name="create_apriltag"
+    params="name size:=0.2 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>
+
+
+    <joint name="${name}_to_${name}_image_link_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="0 -${pi/2.0} ${pi/2.0}" />
+      <parent link="${name}_link" />
+      <child link="${name}_image_link" />
+    </joint>
+
+    <link name="${name}_link"/>
+
+    <link name="${name}_image_link">
+      <collision>
+        <origin xyz="0 0 0" rpy="0 ${pi/2.0} 0" />
+        <geometry>
+          <box size="${size} ${size} 0.0001" />
+        </geometry>
+      </collision>
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 ${pi/2.0} 0" />
+        <geometry>
+          <box size="${size} ${size} 0.0001" />
+        </geometry>
+        <material name="Grey">
+          <color rgba="0.15 0.15 0.15 1.0" />
+        </material>
+      </visual>
+
+      <inertial>
+        <mass value="1e-5" />
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+      </inertial>
+    </link>
+
+    <gazebo reference="${name}_image_link">
+      <visual>
+        <material>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.6 0.6 0.6 1</specular>
+          <ambient>1.0 1.0 1.0</ambient>
+          <lighting>true</lighting>
+          <emissive>0 0 0 1</emissive>
+          <pbr>
+            <metal>
+              <albedo_map>$(arg apriltag_image_path)</albedo_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+    </gazebo>
+
+    <xacro:property name="apriltag_size_prop" value="$(arg apriltag_size)" />
+    <xacro:wibotic_station
+      namespace="$(arg namespace)" parent_link="${name}_image_link"  xyz="-0.001 0.0 ${-apriltag_size_prop/2.0 - 0.125}"
+      rpy="0 0 0 " />
+
+  </xacro:macro>
+
+  <xacro:create_apriltag name="apriltag" namespace="$(arg namespace)" size="$(arg apriltag_size)" />
+
+</robot>