fix reachbility

src/DummyConnector.cpp

@@ -34,8 +34,8 @@ bool DummyConnector::reachableObject(const Object &robot, const Object &object)
     double dy = object.pos().y() - robot.pos().y();
     double dz = object.pos().z() - robot.pos().z();
 
-    // if the location is within a "cone" with a radius of 150mm around the robot base, it is too close to place
-    if (std::sqrt(dx*dx) < 0.15) {
+    // if the location is within a "cone" with a radius of 50mm around the robot base, it is too close to place
+    if (std::sqrt(dx*dx + dy*dy) < 0.05) {
         return false;
     }
 

src/dummy_rbg_connector.cpp

@@ -41,6 +41,9 @@ int main(int argc, char **argv) {
 
     DummyConnector connector{n, CELL_NAME};
 
+    // only for debugging (uncomment when running without the RAG part)
+    // connector.loadScene("/home/jm/work/git/ceti/models2021/src/cgv_connector/config/config_pick_place_scene.json");
+
     // add an MQTT connection
     std::string mqtt_address = getParameter(n, "mqtt/mqtt_address", "tcp://localhost:1883");
     std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqtt_address, NODE_NAME);