align with simplified ccf

launch/robot_main.launch

@@ -14,6 +14,7 @@
     <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)" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
     </node>
 
 </launch>

launch/simulated_main.launch

@@ -8,6 +8,7 @@
     <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)" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
     </node>
 
-</launch>
+</launch>
\ No newline at end of file

launch/simulated_main_dummy_client.launch

@@ -4,9 +4,11 @@
     <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>
     <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="client_controller" output="screen">
         <param name="connection_address" type="string" value="tcp://*:6586" />
         <param name="client_controllers" type="yaml" value="[]" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
     </node>
 </launch>

src/moveit_sorting_controller.cpp

@@ -10,6 +10,7 @@
 #define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
 
 #include <ros/ros.h>
+#include <ros/package.h>
 #include <std_msgs/Empty.h>
 
 #include "connector.pb.h"
@@ -53,7 +54,9 @@ int main(int argc, char **argv) {
         connector.addConnection(std::move(client_connection));
     }
 
-    // scene loading is not required, the scene is updated by moveit
+    ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
+
+    connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene.yaml"));
 
     Object *robot = nullptr;
     Object *selectedBox = nullptr;