Added wibotic receiver test

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

Hide file tree

Showing 3 changed files with 60 additions and 25 deletions.
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/wibotic_receiver.urdf.xacro b/urdf/wibotic_receiver.urdf.xacro
@@ -1,4 +1,21 @@
 <?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

diff --git a/urdf/wibotic_station.urdf.xacro b/urdf/wibotic_station.urdf.xacro
@@ -1,26 +1,44 @@
 <?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" name="wibotic_station">
-  <xacro:arg name="namespace" default="" />
+
+  <xacro:arg name="device_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">
+    params="device_namespace:=None parent_link xyz rpy">
 
-    <xacro:if value="${namespace == 'None'}">
-      <xacro:property name="ns" value="" />
+    <xacro:if value="${device_namespace == 'None'}">
+      <xacro:property name="prefix" value="" />
     </xacro:if>
-    <xacro:unless value="${namespace == 'None'}">
-      <xacro:property name="ns" value="${namespace}/" />
+    <xacro:unless value="${device_namespace == 'None'}">
+      <xacro:property name="prefix" value="${device_namespace}_" />
     </xacro:unless>
 
-    <joint name="${parent_link.rstrip('_link')}_to_wibotic_station_link_joint" type="fixed">
+    <joint name="${parent_link.rstrip('_link')}_to_${prefix}wibotic_station_link_joint" type="fixed">
       <origin xyz="${xyz}" rpy="${rpy}" />
       <parent link="${parent_link}" />
-      <child link="wibotic_station_link" />
+      <child link="${prefix}wibotic_station_link" />
     </joint>
 
-    <link name="wibotic_station_link">
+    <link name="${prefix}wibotic_station_link">
       <visual>
         <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 ${pi/2.0}" />
         <geometry>
@@ -52,25 +70,24 @@
   </xacro:macro>
 
   <xacro:macro name="create_apriltag"
-    params="name size:=0.2 namespace:=None">
+    params="name size:=0.2 device_namespace:=None">
 
-    <xacro:if value="${namespace == 'None'}">
-      <xacro:property name="ns" value="" />
+    <xacro:if value="${device_namespace == 'None'}">
+      <xacro:property name="prefix" value="" />
     </xacro:if>
-    <xacro:unless value="${namespace == 'None'}">
-      <xacro:property name="ns" value="${namespace}/" />
+    <xacro:unless value="${device_namespace == 'None'}">
+      <xacro:property name="prefix" value="${device_namespace}_" />
     </xacro:unless>
 
-
-    <joint name="${name}_to_${name}_image_link_joint" type="fixed">
+    <joint name="${name}_to_${name}_${prefix}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" />
+      <parent link="${prefix}${name}_link" />
+      <child link="${prefix}${name}_image_link" />
     </joint>
 
-    <link name="${name}_link"/>
+    <link name="${prefix}${name}_link"/>
 
-    <link name="${name}_image_link">
+    <link name="${prefix}${name}_image_link">
       <collision>
         <origin xyz="0 0 0" rpy="0 ${pi/2.0} 0" />
         <geometry>
@@ -95,7 +112,7 @@
       </inertial>
     </link>
 
-    <gazebo reference="${name}_image_link">
+    <gazebo reference="${prefix}${name}_image_link">
       <visual>
         <material>
           <diffuse>1.0 1.0 1.0 1</diffuse>
@@ -111,14 +128,14 @@
         </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}"
+      device_namespace="$(arg device_namespace)" parent_link="${prefix}${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)" />
+  <xacro:create_apriltag name="apriltag" device_namespace="$(arg device_namespace)" size="$(arg apriltag_size)" />
 
 </robot>