diff --git a/launch/robot_main.launch b/launch/robot_main.launch index b47dfe09fe419be082afbf4e12994ab341a75c66..a1fa771ad23771d4a1a12d3ee1a754ec3e3c8c5c 100644 --- a/launch/robot_main.launch +++ b/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> diff --git a/launch/simulated_main.launch b/launch/simulated_main.launch index 06c157280830d69e13b2655b4a18ef40948384da..afb6dc2ea73be5d433d28bcc7ee8a251c77d15fb 100644 --- a/launch/simulated_main.launch +++ b/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> \ No newline at end of file +</launch> diff --git a/launch/simulated_main_dummy_client.launch b/launch/simulated_main_dummy_client.launch index f2dc5a207b36b2495ad520fd42ae1304ce2474d8..f319995c1198783f1076baa5f7d4cccbfb16767a 100644 --- a/launch/simulated_main_dummy_client.launch +++ b/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> diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp index 0d72c9bc2b8b3f9099f4346d0347493816044225..8c30c3b5aa21c2f71ee5d1966ae89eddaeabe8dc 100644 --- a/src/moveit_sorting_controller.cpp +++ b/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;