From 41907824018d9c860c10ce7c454cd2ab9d121108 Mon Sep 17 00:00:00 2001
From: Jakub Delicat <jakub.delicat@husarion.com>
Date: Tue, 29 Oct 2024 13:24:13 +0000
Subject: [PATCH] Added requested receiver tf

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
---
 urdf/wibotic_station.urdf.xacro | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/urdf/wibotic_station.urdf.xacro b/urdf/wibotic_station.urdf.xacro
index 481f617..8c68952 100644
--- a/urdf/wibotic_station.urdf.xacro
+++ b/urdf/wibotic_station.urdf.xacro
@@ -20,7 +20,7 @@
 
   <xacro:arg name="device_namespace" default="" />
   <xacro:arg name="apriltag_image_path" default="" />
-  <xacro:arg name="apriltag_size" default="0.15" />
+  <xacro:arg name="apriltag_size" default="0.07" />
 
   <xacro:macro name="wibotic_station"
     params="device_namespace:=None parent_link xyz rpy">
@@ -32,7 +32,7 @@
       <xacro:property name="prefix" value="${device_namespace}_" />
     </xacro:unless>
 
-    <joint name="${parent_link.rstrip('_link')}_to_${prefix}wibotic_station_link_joint" type="fixed">
+    <joint name="${parent_link.rstrip('_link')}_to_${prefix}wibotic_station_joint" type="fixed">
       <origin xyz="${xyz}" rpy="${rpy}" />
       <parent link="${parent_link}" />
       <child link="${prefix}wibotic_station_link" />
@@ -67,6 +67,16 @@
       </inertial>
     </link>
 
+    <joint name="${prefix}wibotic_station_to_${prefix}wibotic_receiver_requested_pose_joint" type="fixed">
+      <!-- This shouldbe when docking is released for panther-docker
+      <origin xyz="0.05 0.0 0.295" rpy="0.0 0.0 ${pi}" /> -->
+      <origin xyz="${0.05 + 0.30} 0.0 0.0" rpy="0.0 0.0 ${pi}" />
+      <parent link="${prefix}wibotic_station_link" />
+      <child link="${prefix}wibotic_receiver_requested_pose_link" />
+    </joint>
+
+    <link name="${prefix}wibotic_receiver_requested_pose_link" />
+
   </xacro:macro>
 
   <xacro:macro name="create_apriltag"
@@ -131,8 +141,8 @@
 
     <xacro:property name="apriltag_size_prop" value="$(arg apriltag_size)" />
     <xacro:wibotic_station
-      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 " />
+      device_namespace="$(arg device_namespace)" parent_link="${prefix}${name}_image_link"  xyz="0.21 0.0 0.115"
+      rpy="${pi} 0 0 " />
 
   </xacro:macro>