update interfaces

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}

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);

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

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;
 };
 
 

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";

proto/Ping.proto

-syntax = "proto3";
-
-message Ping {
-    string name = 1;
-}
\ No newline at end of file

proto/cgv_connector.proto → proto/connector.proto

-// 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;
@@ -54,3 +57,20 @@ message Scene {
 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

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);
 }

src/DummyConnector.cpp

@@ -3,7 +3,7 @@
 //
 
 #include <ros/ros.h>
-
+#include <cmath>
 
 #include "DummyConnector.h"
 #include "Connector.h"
@@ -25,11 +25,30 @@ 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
+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;
+}
+
 
 DummyConnector::DummyConnector(const ros::NodeHandle &nodeHandle) : Connector(nodeHandle) {}
 

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;
+}
+

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>

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"

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>

src/scene_collision_object.cpp

@@ -20,4 +20,4 @@ void SceneCollisionObject::setType(const std::string &type) {
     SceneCollisionObject::type = type;
 }
 
-SceneCollisionObject::SceneCollisionObject() = default;;
+SceneCollisionObject::SceneCollisionObject() = default;
\ No newline at end of file

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)

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() {