From cea7bdf29e0289f878443f624827f9e25c8885e3 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Thu, 13 May 2021 12:10:24 +0200
Subject: [PATCH] fix reachbility
---
src/DummyConnector.cpp | 4 ++--
src/dummy_rbg_connector.cpp | 3 +++
2 files changed, 5 insertions(+), 2 deletions(-)
diff --git a/src/DummyConnector.cpp b/src/DummyConnector.cpp
index ae51c2d..3bbe84a 100644
--- a/src/DummyConnector.cpp
+++ b/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;
}
diff --git a/src/dummy_rbg_connector.cpp b/src/dummy_rbg_connector.cpp
index dc906cd..b28cf87 100644
--- a/src/dummy_rbg_connector.cpp
+++ b/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);
--
GitLab