Merge remote-tracking branch 'public/noetic/main' into feature/multi-table-setup

config/config_scene_ads.yaml

+{ 'objects': [
+  { 'id': 'tablePillar1','pos': { 'x': 0.625,'y': 0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar2','pos': { 'x': -0.215,'y': -0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar3','pos': { 'x': -0.215,'y': 0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar4','pos': { 'x': 0.625,'y': -0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'table','pos': { 'x' : 0.205 ,'z': 0.7 },'size': { 'length': 0.9,'width': 1.7,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'binBlue','type': 'BIN','pos': { 'x': 0.34,'y': -0.65,'z': 0.8325 },'size': { 'length': 0.2,'width': 0.3,'height': 0.15 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+  { 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': -0.65,'z': 0.8325 },'size': { 'length': 0.2,'width': 0.3,'height': 0.15 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+ 
+  { 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+  { 'id': 'objectRed2','type': 'BOX','pos': { 'x': 0.0, 'y' : 0.5,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+  
+  { 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.1,'y': 0.6,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+  { 'id': 'objectGreen2','type': 'BOX','pos': { 'x': 0.45,'y': 0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
+  
+  { 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+  { 'id': 'objectBlue2','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+  { 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }

config/config_scene_st.yaml

+{ 'objects': [
+  { 'id': 'tablePillar1','pos': { 'x': 0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar2','pos': { 'x': -0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar3','pos': { 'x': -0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'tablePillar4','pos': { 'x': 0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'table','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+  { 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+  { 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+ 
+ { 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+  { 'id': 'objectRed2','type': 'BOX','pos': { 'x': -0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+ 
+ { 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.45,'y': -0.3,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+  { 'id': 'objectGreen2','type': 'BOX','pos': { 'x': -0.45,'y': -0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
+ 
+ { 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+  { 'id': 'objectBlue2','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+  { 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }

launch/dummy_main_external_client.launch

+<launch>
+    <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="main_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6576" />
+        <param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+    </node>
+</launch>

launch/robot_main.launch

 <launch>
 
     <arg name="robot_ip" default="172.31.1.13" />
+    <!-- <arg name="robot_ip" default="172.16.0.2" /> -->
     <arg name="load_gripper" default="true" />
     <arg name="connection_address" default="tcp://*:6576" />
     <arg name="client_controllers" default="[]" />
@@ -10,7 +11,7 @@
         <arg name="load_gripper" value="$(arg load_gripper)"/>
     </include>
 
-    <node pkg="ccf" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
+    <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
         <param name="connection_address" type="string" value="$(arg connection_address)" />
         <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
     </node>

launch/robot_main_dummy_selector.launch

 <launch>
     <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
-    <include file="robot_main.launch" />
+    <include file="$(find ccf_immersive_sorting)/launch/robot_main.launch" />
 </launch>

launch/simulated_main.launch

@@ -5,7 +5,7 @@
 
     <include file="$(find ccf)/launch/simulation_setup.launch"/>
 
-    <node pkg="ccf" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
+    <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
         <param name="connection_address" type="string" value="$(arg connection_address)" />
         <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
     </node>

launch/simulated_main_dummy_selector.launch

 <launch>
     <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
-    <include file="simulated_main.launch" />
+    <include file="$(find ccf_immersive_sorting)/launch/simulated_main.launch" />
 </launch>

launch/simulated_main_external_client.launch

+<launch>
+    <include file="$(find ccf)/launch/simulation_setup.launch"/>
+
+    <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="main_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6576" />
+        <param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+    </node>
+</launch>