From 2f1efe4cf4eff214d0f9d96d5522ce336e0b2a5f Mon Sep 17 00:00:00 2001
From: Ian McMahon <imcmahon@rethinkrobotics.com>
Date: Fri, 28 Oct 2016 13:51:10 -0400
Subject: [PATCH] Adds optional SRDF controller_box collision shapes

This change adds an optional controller_box collision
shape to be in sync with the URDF from sawyer_description.

This can be disabled, by invoking with controller_box:=false
when invoking $roslaunch sawyer_moveit_config sawyer_moveit.launch

It is set to true by default in order to prevent accidental
trajectory executions through the controller box.
---
 .../launch/planning_context.launch                 |  6 +++++-
 sawyer_moveit_config/launch/sawyer_moveit.launch   |  3 +++
 .../srdf/controller_box.srdf.xacro                 | 11 +++++++++++
 sawyer_moveit_config/srdf/sawyer.srdf.xacro        | 14 +++++++++++++-
 4 files changed, 32 insertions(+), 2 deletions(-)
 create mode 100644 sawyer_moveit_config/srdf/controller_box.srdf.xacro

diff --git a/sawyer_moveit_config/launch/planning_context.launch b/sawyer_moveit_config/launch/planning_context.launch
index 35c23e5..4cdeb07 100644
--- a/sawyer_moveit_config/launch/planning_context.launch
+++ b/sawyer_moveit_config/launch/planning_context.launch
@@ -4,6 +4,9 @@
   <arg name="electric_gripper" default="false"/>
   <!-- Set the kinematic tips for the left_arm and right_arm move_groups -->
   <arg name="tip_name" default="right_hand"/>
+  <!-- Add controller box collision shape to check for link collisions if set to true-->
+  <arg name="controller_box" default="true"/>
+
 
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
   <arg name="load_robot_description" default="false"/>
@@ -18,7 +21,8 @@
   <param name="robot_description_semantic"
       command="$(find xacro)/xacro.py --inorder $(find sawyer_moveit_config)/srdf/sawyer.srdf.xacro
           electric_gripper:=$(arg electric_gripper)
-          tip_name:=$(arg tip_name)"/>
+          tip_name:=$(arg tip_name)
+          controller_box:=$(arg controller_box)"/>
   
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
diff --git a/sawyer_moveit_config/launch/sawyer_moveit.launch b/sawyer_moveit_config/launch/sawyer_moveit.launch
index 80b9c90..39637b4 100644
--- a/sawyer_moveit_config/launch/sawyer_moveit.launch
+++ b/sawyer_moveit_config/launch/sawyer_moveit.launch
@@ -10,6 +10,8 @@
 
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
   <arg name="load_robot_description" default="false"/>
+  <!-- Add controller box collision shape to check for link collisions if set to true-->
+  <arg name="controller_box" default="true"/>
   <!-- Left and electric gripper arg. Set to true to check for collisions for their links -->
   <arg name="electric_gripper" default="false"/>
   <!-- Set the kinematic tip for the right_arm move_group -->
@@ -20,6 +22,7 @@
     <arg name="load_robot_description" value="$(arg load_robot_description)"/>
     <arg name="electric_gripper" value="$(arg electric_gripper)"/>
     <arg name="tip_name" value="$(arg tip_name)"/>
+    <arg name="controller_box" value="$(arg controller_box)"/>
   </include>
 
   <arg name="kinect" default="false" />
diff --git a/sawyer_moveit_config/srdf/controller_box.srdf.xacro b/sawyer_moveit_config/srdf/controller_box.srdf.xacro
new file mode 100644
index 0000000..0c4f078
--- /dev/null
+++ b/sawyer_moveit_config/srdf/controller_box.srdf.xacro
@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="controller_box">
+    <xacro:macro name="controller_box">
+    <disable_collisions link1="pedestal" link2="pedestal_feet" reason="Adjacent" />
+    <disable_collisions link1="controller_box" link2="pedestal_feet" reason="Adjacent" />
+    </xacro:macro>
+</robot>
diff --git a/sawyer_moveit_config/srdf/sawyer.srdf.xacro b/sawyer_moveit_config/srdf/sawyer.srdf.xacro
index 647f845..c6699ad 100644
--- a/sawyer_moveit_config/srdf/sawyer.srdf.xacro
+++ b/sawyer_moveit_config/srdf/sawyer.srdf.xacro
@@ -1,10 +1,22 @@
 <?xml version="1.0" ?>
 <robot name="sawyer" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!--SRDF Xacro Args-->
   <xacro:arg name="tip_name" default="right_gripper"/>
+  <xacro:arg name="electric_gripper" default="false"/>
+  <xacro:arg name="controller_box" default="true"/>
+
+  <!--Base Sawyer Collisions-->
   <xacro:include filename="$(find sawyer_moveit_config)/srdf/sawyer_base.srdf.xacro" />
   <xacro:sawyer_base tip_name="$(arg tip_name)"/>
+
+  <!--Controller Box Collisions-->
+  <xacro:if value="$(arg controller_box)">
+    <xacro:include filename="$(find sawyer_moveit_config)/srdf/controller_box.srdf.xacro" />
+    <xacro:controller_box/>
+  </xacro:if>
+
   <!--Right End Effector Collisions-->
-  <xacro:arg name="electric_gripper" default="false"/>
   <xacro:if value="$(arg electric_gripper)">
     <xacro:include filename="$(find sawyer_moveit_config)/srdf/rethink_electric_gripper.srdf.xacro" />
     <xacro:rethink_electric_gripper side="right"/>