From 7edb62ef34d2a4ea4e70edbd52081e6b9bbb5bd9 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Sat, 11 Apr 2026 11:12:23 +0200
Subject: [PATCH] Fixed camera pose

---
 toid_bot_description/rviz/default.rviz        | 79 +++++++++++++------
 .../src/toid_bot_description.urdf             | 13 +++
 2 files changed, 70 insertions(+), 22 deletions(-)

diff --git a/toid_bot_description/rviz/default.rviz b/toid_bot_description/rviz/default.rviz
index 5c953cf..34836b4 100644
--- a/toid_bot_description/rviz/default.rviz
+++ b/toid_bot_description/rviz/default.rviz
@@ -1,13 +1,14 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
         - /Status1
+        - /Pose1
       Splitter Ratio: 0.5
-    Tree Height: 542
+    Tree Height: 732
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -74,6 +75,14 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        camera:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         drivewhl_l_link:
           Alpha: 1
           Show Axes: false
@@ -118,6 +127,10 @@ Visualization Manager:
           Value: true
         base_link:
           Value: true
+        camera:
+          Value: true
+        camera_base:
+          Value: true
         drivewhl_l_link:
           Value: true
         drivewhl_r_link:
@@ -134,21 +147,43 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        base_link:
-          base_footprint:
-            {}
-          drivewhl_l_link:
-            {}
-          drivewhl_r_link:
-            {}
-          left_caster:
-            {}
-          lidar:
-            {}
-          right_caster:
-            {}
+        base_footprint:
+          base_link:
+            drivewhl_l_link:
+              {}
+            drivewhl_r_link:
+              {}
+            left_caster:
+              {}
+            lidar:
+              {}
+            right_caster:
+              {}
+          camera_base:
+            camera:
+              {}
       Update Interval: 0
       Value: true
+    - Alpha: 1
+      Axes Length: 0.10000000149011612
+      Axes Radius: 0.009999999776482582
+      Class: rviz_default_plugins/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.10000000149011612
+      Head Radius: 0.029999999329447746
+      Name: Pose
+      Shaft Length: 0.10000000149011612
+      Shaft Radius: 0.009999999776482582
+      Shape: Axes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /closest_acorn
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -195,7 +230,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/XYOrbit
-      Distance: 1.59040105342865
+      Distance: 1.2598285675048828
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -210,18 +245,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6053992509841919
+      Pitch: 0.015398791059851646
       Target Frame: <Fixed Frame>
       Value: XYOrbit (rviz_default_plugins)
-      Yaw: 1.088563323020935
+      Yaw: 0.09356015175580978
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 1250
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016c000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002de00fffffffb0000000800540069006d0065010000000000000450000000000000000000000229000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000027a000003d8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000003d80000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015b000003d8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000003d80000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007620000005efc0100000002fb0000000800540069006d00650100000000000007620000048700fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000003d800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -230,6 +265,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1200
+  Width: 1890
   X: 60
-  Y: 60
+  Y: 64
diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf
index 83de634..c9ce36a 100644
--- a/toid_bot_description/src/toid_bot_description.urdf
+++ b/toid_bot_description/src/toid_bot_description.urdf
@@ -24,6 +24,10 @@
 <xacro:property name="lidar_height" value="0.02"/>
 <xacro:property name="lidar_xoff" value="0.09"/>
 
+<xacro:property name="camera_xoff" value="0.07369"/>
+<xacro:property name="camera_zoff" value="0.16664"/>
+<xacro:property name="camera_pitch" value="${-105 * pi/180}"/>
+
 <xacro:property name="base_zoff" value="${-wheel_zoff + wheel_radius + base_height/2}"/>
 
 <link name="base_link">
@@ -39,6 +43,9 @@
 
 <link name="base_footprint"/>
 
+<link name="camera" />
+
+
 <link name="lidar">
     <visual>
         <geometry>
@@ -56,6 +63,12 @@
     <origin xyz="${-wheel_xoff} 0.0 ${base_zoff}" rpy="0 0 0"/>
 </joint>
 
+<joint name="camera_base_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child link="camera"/>
+    <origin xyz="${camera_xoff} 0 ${camera_zoff}" rpy="${camera_pitch} 0 ${-pi/2}"/>
+</joint>
+
 <joint name="lidar_joint" type="fixed">
     <parent link="base_link"/>
     <child link="lidar"/>