From 65c3c4c9ec79f33bcff369d929ec48d5425e1773 Mon Sep 17 00:00:00 2001
From: Paul Gesel <paulgesel@gmail.com>
Date: Mon, 26 Feb 2024 08:48:27 -0700
Subject: [PATCH] remove extra comments, move gazebo tags to top level, and
 remove logic from ur macro

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
---
 .../description/environment.xacro             |  2 -
 .../description/picknik_ur_macro.xacro        |  7 --
 .../description/environment.xacro             | 80 -------------------
 .../description/picknik_ur_gazebo.xacro       | 78 ++++++++++++++++++
 .../description/environment.xacro             |  2 -
 5 files changed, 78 insertions(+), 91 deletions(-)

diff --git a/src/picknik_ur_base_config/description/environment.xacro b/src/picknik_ur_base_config/description/environment.xacro
index 62271be6..5b570f97 100644
--- a/src/picknik_ur_base_config/description/environment.xacro
+++ b/src/picknik_ur_base_config/description/environment.xacro
@@ -1,7 +1,5 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- wrist_3_link/forearm_link have a radius 0f 0.0375m. Actual offset from surface is (radius - attached_link's radius). -->
-  <!-- UR Drivers check for a 2.5cm  distance between tool_flange/wrist_3 and forearm links. -->
   <xacro:macro name="environment" params="parent">
     <!-- Import environment macros -->
     <xacro:include filename="$(find picknik_accessories)/descriptions/geometry/collision_and_visual/cube_collision_and_visual.urdf.xacro" />
diff --git a/src/picknik_ur_base_config/description/picknik_ur_macro.xacro b/src/picknik_ur_base_config/description/picknik_ur_macro.xacro
index d5e46e7a..67960277 100644
--- a/src/picknik_ur_base_config/description/picknik_ur_macro.xacro
+++ b/src/picknik_ur_base_config/description/picknik_ur_macro.xacro
@@ -18,13 +18,6 @@
         <xacro:arg name="tool_device_name" default="/dev/ttyUSB0"/>
         <xacro:arg name="simulation" default=""/>
 
-        <xacro:if value="$(arg has_tool_changer)">
-            <xacro:property name="camera_adapter_parent" value="tool_changer_tool0"/>
-        </xacro:if>
-        <xacro:unless value="$(arg has_tool_changer)">
-            <xacro:property name="camera_adapter_parent" value="tool0"/>
-        </xacro:unless>
-
         <!-- Import macros for main hardware components -->
         <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
         <xacro:include filename="$(find robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro"/>
diff --git a/src/picknik_ur_gazebo_config/description/environment.xacro b/src/picknik_ur_gazebo_config/description/environment.xacro
index ec80bc4f..c094952b 100644
--- a/src/picknik_ur_gazebo_config/description/environment.xacro
+++ b/src/picknik_ur_gazebo_config/description/environment.xacro
@@ -1,7 +1,5 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- wrist_3_link/forearm_link have a radius 0f 0.0375m. Actual offset from surface is (radius - attached_link's radius). -->
-  <!-- UR Drivers check for a 2.5cm  distance between tool_flange/wrist_3 and forearm links. -->
   <xacro:macro name="environment" params="parent">
     <!-- parameters -->
     <xacro:arg name="gazebo_renderer" default="ogre"/>
@@ -52,83 +50,5 @@
       </xacro:realsense_d435>
     </xacro:if>
 
-    <!-- Force/Torque Sensor -->
-    <link name="robot/wrist_3_joint/force_torque_sensor" />
-    <joint name="wrist_to_fts" type="fixed">
-      <parent link="wrist_3_link" />
-      <child link="robot/wrist_3_joint/force_torque_sensor" />
-      <origin rpy="0 0 0" xyz="0 0 0" />
-    </joint>
-    <gazebo reference="wrist_3_joint">
-      <sensor name="tcp_fts_sensor" type="force_torque">
-        <always_on>1</always_on>
-        <update_rate>50</update_rate>
-        <visualize>0</visualize>
-        <topic>tcp_fts_sensor/ft_data</topic>
-        <pose>0 0 0 0 0 0</pose>
-        <force_torque>
-          <frame>child</frame>
-          <measure_direction>parent_to_child</measure_direction>
-          <force>
-            <x>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0</stddev>
-              </noise>
-            </x>
-            <y>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0</stddev>
-              </noise>
-            </y>
-            <z>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0</stddev>
-              </noise>
-            </z>
-          </force>
-          <torque>
-            <x>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0</stddev>
-              </noise>
-            </x>
-            <y>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0</stddev>
-              </noise>
-            </y>
-            <z>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0</stddev>
-              </noise>
-            </z>
-          </torque>
-        </force_torque>
-      </sensor>
-    </gazebo>
-
-    <!-- Gazebo Plugins -->
-    <gazebo>
-      <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
-        <parameters>$(find picknik_ur_gazebo_config)/config/control/picknik_ur5e.ros2_control.yaml</parameters>
-        <ros>
-          <remapping>/servo_controller/commands:=/robot_controllers/commands</remapping>
-          <remapping>/servo_controller/joint_trajectory:=/robot_controllers/joint_trajectory</remapping>
-        </ros>
-      </plugin>
-    </gazebo>
-
-    <gazebo>
-      <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
-        <render_engine>${gazebo_renderer}</render_engine>
-      </plugin>
-    </gazebo>
-
   </xacro:macro>
 </robot>
diff --git a/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro b/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro
index fa81919a..ba4206d7 100644
--- a/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro
+++ b/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro
@@ -35,4 +35,82 @@
   <!-- Environment: contains scene geometry and external sensors, e.g. cameras -->
   <xacro:environment parent="world"/>
 
+  <!-- Gazebo Force/Torque Sensor -->
+  <link name="robot/wrist_3_joint/force_torque_sensor" />
+  <joint name="wrist_to_fts" type="fixed">
+    <parent link="wrist_3_link" />
+    <child link="robot/wrist_3_joint/force_torque_sensor" />
+    <origin rpy="0 0 0" xyz="0 0 0" />
+  </joint>
+  <gazebo reference="wrist_3_joint">
+    <sensor name="tcp_fts_sensor" type="force_torque">
+      <always_on>1</always_on>
+      <update_rate>50</update_rate>
+      <visualize>0</visualize>
+      <topic>tcp_fts_sensor/ft_data</topic>
+      <pose>0 0 0 0 0 0</pose>
+      <force_torque>
+        <frame>child</frame>
+        <measure_direction>parent_to_child</measure_direction>
+        <force>
+          <x>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0</stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0</stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0</stddev>
+            </noise>
+          </z>
+        </force>
+        <torque>
+          <x>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0</stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0</stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0</mean>
+              <stddev>0</stddev>
+            </noise>
+          </z>
+        </torque>
+      </force_torque>
+    </sensor>
+  </gazebo>
+
+  <!-- Gazebo Plugins -->
+  <gazebo>
+    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+      <parameters>$(find picknik_ur_gazebo_config)/config/control/picknik_ur5e.ros2_control.yaml</parameters>
+      <ros>
+        <remapping>/servo_controller/commands:=/robot_controllers/commands</remapping>
+        <remapping>/servo_controller/joint_trajectory:=/robot_controllers/joint_trajectory</remapping>
+      </ros>
+    </plugin>
+  </gazebo>
+
+  <gazebo>
+    <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
+      <render_engine>${gazebo_renderer}</render_engine>
+    </plugin>
+  </gazebo>
+
 </robot>
diff --git a/src/picknik_ur_mock_hw_config/description/environment.xacro b/src/picknik_ur_mock_hw_config/description/environment.xacro
index 6f50da7c..f10dc92b 100644
--- a/src/picknik_ur_mock_hw_config/description/environment.xacro
+++ b/src/picknik_ur_mock_hw_config/description/environment.xacro
@@ -1,7 +1,5 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- wrist_3_link/forearm_link have a radius 0f 0.0375m. Actual offset from surface is (radius - attached_link's radius). -->
-  <!-- UR Drivers check for a 2.5cm  distance between tool_flange/wrist_3 and forearm links. -->
   <xacro:macro name="environment" params="parent">
     <!-- Import environment macros -->
     <xacro:include filename="$(find picknik_accessories)/descriptions/geometry/collision_and_visual/cube_collision_and_visual.urdf.xacro" />