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