From eac195a63ade1e54392990677379092d9c62898d Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Thu, 4 Nov 2021 11:01:02 +0100
Subject: [PATCH] align with simplified ccf
---
launch/robot_main.launch | 1 +
launch/simulated_main.launch | 3 ++-
launch/simulated_main_dummy_client.launch | 2 ++
src/moveit_sorting_controller.cpp | 5 ++++-
4 files changed, 9 insertions(+), 2 deletions(-)
diff --git a/launch/robot_main.launch b/launch/robot_main.launch
index b47dfe0..a1fa771 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 06c1572..afb6dc2 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 f2dc5a2..f319995 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 0d72c9b..8c30c3b 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;
--
GitLab