From 429469b80bba19942d13fb9b6d35612a1640af1c Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Wed, 5 May 2021 11:58:57 +0200
Subject: [PATCH] update interfaces

---
 CMakeLists.txt                                |  2 +-
 include/Connector.h                           | 19 ++++++++++---
 include/DummyConnector.h                      | 19 ++++++++++---
 include/MoveItConnector.h                     |  6 ++---
 include/scene_constructor_util.h              |  2 +-
 proto/Ping.proto                              |  5 ----
 .../{cgv_connector.proto => connector.proto}  | 26 +++++++++++++++---
 src/Connector.cpp                             |  9 ++++---
 src/DummyConnector.cpp                        | 27 ++++++++++++++++---
 src/MoveItConnector.cpp                       | 12 ++++++---
 src/dummy_cgv.cpp                             |  2 +-
 src/dummy_cgv_connector.cpp                   |  2 +-
 src/moveit_cgv_connector.cpp                  |  2 +-
 src/scene_collision_object.cpp                |  2 +-
 src/scene_constructor_util.cpp                |  2 +-
 src/scene_controller_node.cpp                 |  3 ++-
 16 files changed, 103 insertions(+), 37 deletions(-)
 delete mode 100644 proto/Ping.proto
 rename proto/{cgv_connector.proto => connector.proto} (55%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2ace2ca..dd3ee26 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -139,7 +139,7 @@ include_directories(
         ${CMAKE_CURRENT_SOURCE_DIR}
 )
 
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/Ping.proto proto/cgv_connector.proto)
+protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/connector.proto)
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
diff --git a/include/Connector.h b/include/Connector.h
index adc1d96..b5e8250 100644
--- a/include/Connector.h
+++ b/include/Connector.h
@@ -6,20 +6,20 @@
 #define CGV_CONNECTOR_WORKSPACE_CONNECTOR_H
 
 #include <functional>
-// #include <algorithm>
 #include <optional>
 
 #include <ros/ros.h>
 
 #include "Connection.h"
 
-#include <cgv_connector.pb.h>
+#include <connector.pb.h>
 
 class Connector {
 
 private:
 
     std::function<void(Selection)> selectionAction;
+    std::function<void(Selection)> pickPlaceAction;
 
     void receive(const std::string &channel, const std::string &data);
 
@@ -45,10 +45,23 @@ public:
     // application "skills"
     virtual bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly);
     virtual bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly);
-    virtual bool reachable(Object &robot, Object &objectTheLocationOfWhichWeIgnore, Object &location);
+
+    /// Compute if a location is reachable by a robot, i.e., if an object can be placed or picked at this location
+    /// \param robot
+    /// \param location
+    /// \param object to be picked and placed. The current location of this object is ignored.
+    /// \return true if the location is reachable
+    virtual bool reachableLocation(Object &robot, Object &location, Object &object) = 0;
+
+    /// Compute if an object is reachable by a robot, i.e., if an object can grasp it
+    /// \param robot
+    /// \param object to be picked
+    /// \return true if the location is reachable
+    virtual bool reachableObject(Object &robot, Object &object) = 0;
 
     // reactive components: application logic provided by callbacks
     void reactToSelectionMessage(std::function<void(Selection)> lambda);
+    void reactToPickAndPlaceMessage(std::function<void(Selection)> lambda);
 
     // helper methods
     std::optional<Object> resolveObject(const std::string &id);
diff --git a/include/DummyConnector.h b/include/DummyConnector.h
index 2c7dab7..59f620d 100644
--- a/include/DummyConnector.h
+++ b/include/DummyConnector.h
@@ -5,8 +5,6 @@
 #ifndef CGV_CONNECTOR_WORKSPACE_DUMMYCONNECTOR_H
 #define CGV_CONNECTOR_WORKSPACE_DUMMYCONNECTOR_H
 
-#include <cgv_connector.pb.h>
-
 #include "Connector.h"
 
 class DummyConnector : public Connector {
@@ -16,7 +14,22 @@ public:
 
     bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) override;
     bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) override;
-    bool reachable(Object &robot, Object &objectTheLocationOfWhichWeIgnore, Object &location) override;
+
+    /// Compute if a location is reachable by a robot, i.e., if an object can be placed or picked at this location
+    /// If the location is within a "cone" with a radius of 150mm around the robot base, it is too close to reach.
+    //  If the location is more than 750mm away from the robot base it is too far away.
+    //  Otherwise, we assume that we can reach it.
+    /// \param robot
+    /// \param location
+    /// \param object to be picked and placed. The current location of this object is ignored.
+    /// \return true if the location is reachable
+    virtual bool reachableLocation(Object &robot, Object &location, Object &object) override;
+
+    /// Compute if an object is reachable by a robot, i.e., if an object can grasp it
+    /// \param robot
+    /// \param object to be picked
+    /// \return true if the location is reachable
+    virtual bool reachableObject(Object &robot, Object &object) override;
 };
 
 #endif //CGV_CONNECTOR_WORKSPACE_DUMMYCONNECTOR_H
diff --git a/include/MoveItConnector.h b/include/MoveItConnector.h
index 690fd43..22d130f 100644
--- a/include/MoveItConnector.h
+++ b/include/MoveItConnector.h
@@ -5,8 +5,7 @@
 #ifndef CGV_CONNECTOR_WORKSPACE_MOVEITCONNECTOR_H
 #define CGV_CONNECTOR_WORKSPACE_MOVEITCONNECTOR_H
 
-#include "cgv_connector/SceneUpdate.h"
-#include <cgv_connector/PickPlace.h>
+#include <cgv_connector/SceneUpdate.h>
 
 #include "Connector.h"
 
@@ -24,7 +23,8 @@ public:
 
     bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) override;
     bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) override;
-    bool reachable(Object &robot, Object &objectTheLocationOfWhichWeIgnore, Object &location) override;
+    bool reachableLocation(Object &robot, Object &location, Object &object) override;
+    bool reachableObject(Object &robot, Object &object) override;
 };
 
 
diff --git a/include/scene_constructor_util.h b/include/scene_constructor_util.h
index 78522a9..ee83e4c 100644
--- a/include/scene_constructor_util.h
+++ b/include/scene_constructor_util.h
@@ -9,7 +9,7 @@
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit/move_group_interface/move_group_interface.h>
 
-#include "cgv_connector.pb.h"
+#include "connector.pb.h"
 #include "scene_collision_object.h"
 
 static const std::string TABLE_PILLAR_1 = "tablePillar1";
diff --git a/proto/Ping.proto b/proto/Ping.proto
deleted file mode 100644
index 7bd1268..0000000
--- a/proto/Ping.proto
+++ /dev/null
@@ -1,5 +0,0 @@
-syntax = "proto3";
-
-message Ping {
-    string name = 1;
-}
\ No newline at end of file
diff --git a/proto/cgv_connector.proto b/proto/connector.proto
similarity index 55%
rename from proto/cgv_connector.proto
rename to proto/connector.proto
index 33fe002..fe23e0b 100644
--- a/proto/cgv_connector.proto
+++ b/proto/connector.proto
@@ -1,8 +1,11 @@
-// cgv_connector.proto
-// this file contains the messages that are exchanged between the cgv framework and the st ROS interface
+// connector.proto
+// this file contains the messages that are exchanged between the ROS connector and other systems
 
 syntax = "proto3";
 
+option java_package = "de.tudresden.inf.st.ceti";
+option java_multiple_files = true;
+
 message Object {
 
     // Position is object-center related
@@ -34,7 +37,7 @@ message Object {
         BOX = 1;
         BIN = 2;
         ARM = 3;
-        LOCATION = 4;
+        DROP_OFF_LOCATION = 4;
     }
 
     string id = 1;
@@ -53,4 +56,21 @@ message Scene {
 // the selection is done by the CGV framework and sent to ROS
 message Selection {
     string id = 1; // the id corresponds to an id of an Object in a Scene
+}
+
+// vvv from rs vvv.
+// Merged message to contain both pick and place in one
+message PickPlace {
+    string idRobot = 1; // the id corresponds to and id of the robot Object that should execute this operation
+    string idPick = 2; // the id corresponds to an id of the Object in a Scene to be picked
+    string idPlace = 3; // the id corresponds to an id of the Object in a Scene where the picked object shall be placed
+}
+// Reachability of objects, as reported by MoveIt
+message Reachability {
+    message ObjectReachability {
+        string idObject = 1; // the id of the object to reach
+        bool reachable = 2; // whether the object can be reached
+    }
+    string idRobot = 1; // the id of the robot arm
+    repeated ObjectReachability objects = 2; // all objects reachable
 }
\ No newline at end of file
diff --git a/src/Connector.cpp b/src/Connector.cpp
index e596abf..4d9feae 100644
--- a/src/Connector.cpp
+++ b/src/Connector.cpp
@@ -6,9 +6,8 @@
 #include <memory>
 #include <google/protobuf/text_format.h>
 #include <ros/package.h>
-#include "Connector.h"
 
-#include <ros/ros.h>
+#include "Connector.h"
 
 void Connector::addConnection(std::unique_ptr<Connection> &&connection) {
     connection->initializeConnection([this](auto &&channel, auto &&data) {
@@ -106,11 +105,13 @@ bool Connector::pickAndPlace(Object &robot, Object &object, Object &location, bo
         object.mutable_pos()->set_x(location.pos().x());
         object.mutable_pos()->set_y(location.pos().y());
         object.mutable_pos()->set_z(location.pos().z() - location.size().height()/2 + object.size().height()/2);
+        return true;
     } else {
         return false;
     }
 }
 
-bool Connector::reachable(Object &robot, Object &objectTheLocationOfWhichWeIgnore, Object &location) {
-    return false;
+
+void Connector::reactToPickAndPlaceMessage(std::function<void(Selection)> lambda) {
+    pickPlaceAction = std::move(lambda);
 }
diff --git a/src/DummyConnector.cpp b/src/DummyConnector.cpp
index d2f6872..62c10fc 100644
--- a/src/DummyConnector.cpp
+++ b/src/DummyConnector.cpp
@@ -3,7 +3,7 @@
 //
 
 #include <ros/ros.h>
-
+#include <cmath>
 
 #include "DummyConnector.h"
 #include "Connector.h"
@@ -25,9 +25,28 @@ bool DummyConnector::pickAndPlace(Object &robot, Object &object, Object &locatio
     return result;
 }
 
-bool DummyConnector::reachable(Object &robot, Object &objectTheLocationOfWhichWeIgnore, Object &location) {
-    // TODO currently, nothing is reachable
-    return false;
+bool DummyConnector::reachableLocation(Object &robot, Object &location, Object &object) {
+    // we pretend the location is an object and try to reach it
+    return reachableObject(robot, location);
+}
+
+bool DummyConnector::reachableObject(Object &robot, Object &object) {
+    double dx = object.pos().x() - robot.pos().x();
+    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) {
+        return false;
+    }
+
+    // if the location is more than 750mm away from the robot base it is too far away
+    if (std::sqrt(dx*dx + dy*dy + dz+dz) > 0.75) {
+        return false;
+    }
+
+    // otherwise we can reach it
+    return true;
 }
 
 
diff --git a/src/MoveItConnector.cpp b/src/MoveItConnector.cpp
index 96ed89c..8b7dcb2 100644
--- a/src/MoveItConnector.cpp
+++ b/src/MoveItConnector.cpp
@@ -126,13 +126,17 @@ bool MoveItConnector::pickAndPlace(Object &robot, Object &object, Object &locati
     return true;
 }
 
-bool MoveItConnector::reachable(Object &robot, Object &objectTheLocationOfWhichWeIgnore, Object &location) {
-    // TODO currently, nothing is reachable
-    return false;
-}
 
 MoveItConnector::MoveItConnector(ros::NodeHandle &nodeHandle)
         : Connector(nodeHandle),
           pick_drop_client(nodeHandle.serviceClient<cgv_connector::PickDropService>("/pick_drop_service")),
           pick_place_client(nodeHandle.serviceClient<cgv_connector::PickPlaceService>("/pick_place_service")) {}
 
+bool MoveItConnector::reachableObject(Object &robot, Object &object) {
+    return false;
+}
+
+bool MoveItConnector::reachableLocation(Object &robot, Object &location, Object &object) {
+    return false;
+}
+
diff --git a/src/dummy_cgv.cpp b/src/dummy_cgv.cpp
index 22d682a..36bd801 100644
--- a/src/dummy_cgv.cpp
+++ b/src/dummy_cgv.cpp
@@ -15,7 +15,7 @@
 #include <google/protobuf/text_format.h>
 #include <random>
 
-#include "cgv_connector.pb.h"
+#include "connector.pb.h"
 
 #include <google/protobuf/util/json_util.h>
 #include <google/protobuf/message.h>
diff --git a/src/dummy_cgv_connector.cpp b/src/dummy_cgv_connector.cpp
index 7a2bb1a..edc0ed7 100644
--- a/src/dummy_cgv_connector.cpp
+++ b/src/dummy_cgv_connector.cpp
@@ -15,7 +15,7 @@
 #include <ros/package.h>
 #include <std_msgs/Empty.h>
 
-#include "cgv_connector.pb.h"
+#include "connector.pb.h"
 
 #include "DummyConnector.h"
 #include "NngConnection.h"
diff --git a/src/moveit_cgv_connector.cpp b/src/moveit_cgv_connector.cpp
index a62e278..bee1b61 100644
--- a/src/moveit_cgv_connector.cpp
+++ b/src/moveit_cgv_connector.cpp
@@ -12,7 +12,7 @@
 #include <ros/ros.h>
 #include <std_msgs/Empty.h>
 
-#include "cgv_connector.pb.h"
+#include "connector.pb.h"
 
 
 #include <cgv_connector/BinCheck.h>
diff --git a/src/scene_collision_object.cpp b/src/scene_collision_object.cpp
index 52993b0..0eb193c 100644
--- a/src/scene_collision_object.cpp
+++ b/src/scene_collision_object.cpp
@@ -20,4 +20,4 @@ void SceneCollisionObject::setType(const std::string &type) {
     SceneCollisionObject::type = type;
 }
 
-SceneCollisionObject::SceneCollisionObject() = default;;
\ No newline at end of file
+SceneCollisionObject::SceneCollisionObject() = default;
\ No newline at end of file
diff --git a/src/scene_constructor_util.cpp b/src/scene_constructor_util.cpp
index 0ccdced..2576b32 100644
--- a/src/scene_constructor_util.cpp
+++ b/src/scene_constructor_util.cpp
@@ -2,7 +2,7 @@
 // Created by Sebastian Ebert on 03.08.20.
 //
 #include <ros/ros.h>
-#include "../include/scene_constructor_util.h"
+#include "scene_constructor_util.h"
 
 /**
  * Build a collision object from a part (originated by configuration message of table)
diff --git a/src/scene_controller_node.cpp b/src/scene_controller_node.cpp
index 19949cb..d85769e 100644
--- a/src/scene_controller_node.cpp
+++ b/src/scene_controller_node.cpp
@@ -8,7 +8,7 @@
 #include <google/protobuf/util/json_util.h>
 #include <google/protobuf/message.h>
 
-#include "cgv_connector.pb.h"
+#include "connector.pb.h"
 #include "cgv_connector/SceneUpdate.h"
 #include "cgv_connector/BinCheck.h"
 #include "cgv_connector/PickDropService.h"
@@ -398,6 +398,7 @@ bool pick(const std::string &objectId,
     }
 
     ROS_WARN("[SCENECONTROLLER] Pick planning failed: An object is currently grasped.");
+    return false;
 }
 
 geometry_msgs::Pose getBaseLinkPose() {
-- 
GitLab