diff --git a/include/ccf/controller/Controller.h b/include/ccf/controller/Controller.h index 2e5ad4b56fd8fddee27ffe8ff256d990004aa265..113cbbec2a208304f1527eb66d6a3ccf22520cec 100644 --- a/include/ccf/controller/Controller.h +++ b/include/ccf/controller/Controller.h @@ -22,7 +22,9 @@ private: protected: ros::NodeHandle nodeHandle; std::vector<std::unique_ptr<Connection>> connections; + virtual void receive(const std::string &channel, const std::string &data) = 0; + public: explicit Controller(const ros::NodeHandle &nodeHandle); diff --git a/include/ccf/controller/DummyRobotArmController.h b/include/ccf/controller/DummyRobotArmController.h index 7481d60c3d4c974a4ce6a09c269ad3f470927dc1..7cac02482245c15cf3ad18ac5dbdf01fd181596d 100644 --- a/include/ccf/controller/DummyRobotArmController.h +++ b/include/ccf/controller/DummyRobotArmController.h @@ -13,6 +13,7 @@ public: explicit DummyRobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName); bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) override; + bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) override; /// Compute if a location is reachable by a robot, i.e., if an object can be placed or picked at this location diff --git a/include/ccf/controller/MoveItRobotArmController.h b/include/ccf/controller/MoveItRobotArmController.h index a748bc555339c27ac60e100e8ddac1055d1f50d2..0b0066a10b10cfee78ce556fa8181ea48ea00cdb 100644 --- a/include/ccf/controller/MoveItRobotArmController.h +++ b/include/ccf/controller/MoveItRobotArmController.h @@ -26,8 +26,11 @@ public: void updateScene(ccf::SceneUpdateRequest &req); bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) override; + bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) override; + bool reachableLocation(const Object &robot, const Object &location, const Object &object) override; + bool reachableObject(const Object &robot, const Object &object) override; }; diff --git a/include/ccf/controller/RobotArmController.h b/include/ccf/controller/RobotArmController.h index 1e2774b8df802f98bc121c7e0fa1a899d34b0593..606955e71ae3379b60f066a59788dc66057e8dbd 100644 --- a/include/ccf/controller/RobotArmController.h +++ b/include/ccf/controller/RobotArmController.h @@ -42,7 +42,8 @@ public: RobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName); - void loadScene(const std::string& sceneFile); + void loadScene(const std::string &sceneFile); + std::shared_ptr<Scene> getScene(); void sendScene(); @@ -51,6 +52,7 @@ 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); /// Compute if a location is reachable by a robot, i.e., if an object can be placed or picked at this location @@ -68,11 +70,13 @@ public: // reactive components: application logic provided by callbacks void reactToSelectionMessage(std::function<void(Selection)> lambda); + void reactToPickAndPlaceMessage(std::function<void(PickPlace)> lambda); + void reactToSceneUpdateMessage(std::function<void()> lambda); // helper methods - Object* resolveObject(const std::string &id); + Object *resolveObject(const std::string &id); }; #endif //CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H diff --git a/include/ccf/util/scene_constructor_util.h b/include/ccf/util/scene_constructor_util.h index ee83e4cbf936a3658301ae362909332c9aeb8fd1..a680d152c9734c055aa2e236aa6ecaad27e8806c 100644 --- a/include/ccf/util/scene_constructor_util.h +++ b/include/ccf/util/scene_constructor_util.h @@ -43,7 +43,7 @@ public: * @param scene a scene object containing the bin objects */ static bool constructBins(std::vector<SceneCollisionObject> &collision_objects, - std::vector<std_msgs::ColorRGBA> &collision_objects_colors, const Scene& scene); + std::vector<std_msgs::ColorRGBA> &collision_objects_colors, const Scene &scene); /** * Adds a list of objects to be picked/placed to the planning scene. @@ -52,7 +52,7 @@ public: * @param scene a scene object containing the objects */ static bool constructObjects(std::vector<SceneCollisionObject> &collision_objects, - std::vector<std_msgs::ColorRGBA> &collision_objects_colors, const Scene& scene); + std::vector<std_msgs::ColorRGBA> &collision_objects_colors, const Scene &scene); }; #endif //CGV_CONNECTOR_SCENE_CONSTRUCTOR_H \ No newline at end of file diff --git a/src/ccf/connection/MqttConnection.cpp b/src/ccf/connection/MqttConnection.cpp index 8d082aafd1750510df3ee6e03b68a6115ca3da79..b12faf6b0b09b0e1e5f0af8964d3365dba3a02da 100644 --- a/src/ccf/connection/MqttConnection.cpp +++ b/src/ccf/connection/MqttConnection.cpp @@ -20,7 +20,8 @@ bool MqttConnection::ensureConnection() { } return true; } catch (const mqtt::exception &exception) { - ROS_ERROR_STREAM("[MqttConnection] Problem during reconnection. Retrying in 1s. Exception: " << exception.to_string()); + ROS_ERROR_STREAM("[MqttConnection] Problem during reconnection. Retrying in 1s. Exception: " + << exception.to_string()); std::this_thread::sleep_for(std::chrono::seconds(1)); } } diff --git a/src/ccf/controller/DummyRobotArmController.cpp b/src/ccf/controller/DummyRobotArmController.cpp index ea1fbfe48f1a002d599b5c3a6d901dddfdb7a499..55a1825a8dc5169369eca3d58ce54906d5d718e3 100644 --- a/src/ccf/controller/DummyRobotArmController.cpp +++ b/src/ccf/controller/DummyRobotArmController.cpp @@ -35,12 +35,12 @@ bool DummyRobotArmController::reachableObject(const Object &robot, const Object double dz = object.pos().z() - robot.pos().z(); // 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) { + if (std::sqrt(dx * dx + dy * dy) < 0.05) { 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) { + if (std::sqrt(dx * dx + dy * dy + dz * dz) > 0.75) { return false; } @@ -48,7 +48,8 @@ bool DummyRobotArmController::reachableObject(const Object &robot, const Object return true; } -DummyRobotArmController::DummyRobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName) : RobotArmController(nodeHandle, cellName) {} +DummyRobotArmController::DummyRobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName) + : RobotArmController(nodeHandle, cellName) {} diff --git a/src/ccf/controller/RobotArmController.cpp b/src/ccf/controller/RobotArmController.cpp index 2f4008bb685cf32e0046d12d53cb6e430fb480ab..bf1f0f9a1ba29cb9e78ebdcd018b953eacd0fdaf 100644 --- a/src/ccf/controller/RobotArmController.cpp +++ b/src/ccf/controller/RobotArmController.cpp @@ -6,7 +6,6 @@ #include <stdexcept> #include <memory> #include <google/protobuf/text_format.h> -#include <ros/package.h> #include <google/protobuf/util/json_util.h> #include "ccf/controller/RobotArmController.h" @@ -30,8 +29,8 @@ void RobotArmController::receive(const std::string &channel, const std::string & } } -Object* RobotArmController::resolveObject(const std::string &id) { - for (int i = 0; i<scene->objects_size(); i++) { +Object *RobotArmController::resolveObject(const std::string &id) { + for (int i = 0; i < scene->objects_size(); i++) { if (scene->objects(i).id() == id) { return scene->mutable_objects(i); } @@ -87,8 +86,8 @@ void RobotArmController::reactToSelectionMessage(std::function<void(Selection)> RobotArmController::RobotArmController(const ros::NodeHandle &nodeHandle, const std::string &cellName) : Controller(nodeHandle), scene(nullptr), sceneUpdateAction([]() {}), - pickPlaceAction([](const PickPlace& p) {}), - selectionAction([](const Selection& s) {}), + pickPlaceAction([](const PickPlace &p) {}), + selectionAction([](const Selection &s) {}), cellName(cellName) { selectionTopic = "selection"; @@ -109,7 +108,8 @@ RobotArmController::RobotArmController(const ros::NodeHandle &nodeHandle, const } bool RobotArmController::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { - if (object.orientation().x() == 0 && object.orientation().y() == 0 && location.orientation().x() == 0 && location.orientation().y() == 0) { + if (object.orientation().x() == 0 && object.orientation().y() == 0 && location.orientation().x() == 0 && + location.orientation().y() == 0) { // the objects must not be rotated around z // TODO improve float comparison with 0 object.mutable_pos()->set_x(location.pos().x()); diff --git a/src/ccf/util/scene_constructor_util.cpp b/src/ccf/util/scene_constructor_util.cpp index b589ed369a5df8581d6d2e5034d2bbde9c234e3b..b9a05e3ea4f9312df0ac9f0abdfd68e851223223 100644 --- a/src/ccf/util/scene_constructor_util.cpp +++ b/src/ccf/util/scene_constructor_util.cpp @@ -12,7 +12,7 @@ * @return */ moveit_msgs::CollisionObject buildTablePartFromMsg(const Object &part, std::string id, - std::vector<std_msgs::ColorRGBA> &collision_objects_colors) { + std::vector<std_msgs::ColorRGBA> &collision_objects_colors) { moveit_msgs::CollisionObject obj; std_msgs::ColorRGBA obj_color; @@ -51,12 +51,12 @@ moveit_msgs::CollisionObject buildTablePartFromMsg(const Object &part, std::stri } bool SceneConstructorUtil::constructTable(std::vector<SceneCollisionObject> &collision_objects, - std::vector<std_msgs::ColorRGBA> &collision_objects_colors, Scene &table) { + std::vector<std_msgs::ColorRGBA> &collision_objects_colors, Scene &table) { int partCount = 0; for (auto part: table.objects()) { - if(part.type() == Object::UNKNOWN) { + if (part.type() == Object::UNKNOWN) { if (part.id() == TABLE_PILLAR_1 || part.id() == TABLE_PILLAR_2 || part.id() == TABLE_PILLAR_3 || part.id() == TABLE_PILLAR_4 || part.id() == TABLE_PLATE) { @@ -141,8 +141,8 @@ bool SceneConstructorUtil::constructBins(std::vector<SceneCollisionObject> &coll * @param scene scene with collision objects * @return true if valid */ -bool validateObjects(const Scene& scene) { - for (const auto& obj : scene.objects()) { +bool validateObjects(const Scene &scene) { + for (const auto &obj : scene.objects()) { if (obj.size().length() == 0 || obj.size().width() == 0 || obj.size().height() == 0) { ROS_ERROR("[SCENE-UTIL] Validation of object failed (a dimension is 0)."); return false; @@ -158,13 +158,14 @@ bool validateObjects(const Scene& scene) { } bool SceneConstructorUtil::constructObjects(std::vector<SceneCollisionObject> &collision_objects, - std::vector<std_msgs::ColorRGBA> &collision_objects_colors, const Scene& scene) { + std::vector<std_msgs::ColorRGBA> &collision_objects_colors, + const Scene &scene) { if (!validateObjects(scene)) { return false; } - for (const auto& obj : scene.objects()) { + for (const auto &obj : scene.objects()) { if (obj.type() == Object::BOX) { moveit_msgs::CollisionObject c_obj; diff --git a/src/ccf/util/scene_controller_node.cpp b/src/ccf/util/scene_controller_node.cpp index d015dc91f202025b009ed519302b69c2f201b6cf..fe89962caeb17a51485cb084b2345ccf17847da4 100644 --- a/src/ccf/util/scene_controller_node.cpp +++ b/src/ccf/util/scene_controller_node.cpp @@ -388,8 +388,8 @@ bool pick(const std::string &objectId, gripper_transform.rotation); bool success = grasp_util.pickFromAbove(group, pick_pose, dimensions, world_state::open_amount, - TABLE_PLATE, objectId); - if(success){ + TABLE_PLATE, objectId); + if (success) { world_state::current_picked_object_id = objectId; return true; } @@ -460,15 +460,16 @@ bool place(geometry_msgs::Pose target_pose, support_collision_object.operation = support_collision_object.ADD; planning_scene_interface.applyCollisionObject(support_collision_object); - bool success = grasp_util.placeFromAbove(group, target_pose, world_state::open_amount, support_collision_object.id, - world_state::current_picked_object_id); + bool success = grasp_util.placeFromAbove(group, target_pose, world_state::open_amount, + support_collision_object.id, + world_state::current_picked_object_id); std::vector<std::string> col_id_vec; col_id_vec.push_back(support_collision_object.id); planning_scene_interface.removeCollisionObjects(col_id_vec); pushSceneUpdate(n); - if(success){ + if (success) { world_state::current_picked_object_id = ""; } return success; @@ -526,8 +527,8 @@ bool drop(const std::string &bin_id, object_to_pick_drop.orientation.z = bin_collision_object.primitive_poses[0].orientation.z; ROS_INFO("[SCENECONTROLLER] dropping object."); - if(!grasp_util.placeFromAbove(group, object_to_pick_drop, world_state::open_amount, bin_id, - world_state::current_picked_object_id)){ + if (!grasp_util.placeFromAbove(group, object_to_pick_drop, world_state::open_amount, bin_id, + world_state::current_picked_object_id)) { return false; } @@ -610,7 +611,7 @@ bool pickAndPlaceObjectCallback(ccf::PickPlaceService::Request &req, } ROS_INFO_STREAM("[SCENECONTROLLER] Picking object."); - if(!pick(id, planning_scene_interface, group, n)){ + if (!pick(id, planning_scene_interface, group, n)) { res.success = false; return false; } @@ -618,7 +619,7 @@ bool pickAndPlaceObjectCallback(ccf::PickPlaceService::Request &req, system("rosrun dynamic_reconfigure dynparam set /move_group/pick_place cartesian_motion_step_size 0.01"); ROS_INFO_STREAM("[SCENECONTROLLER] Placing object."); - if(!place(req.pose, planning_scene_interface, group, n)){ + if (!place(req.pose, planning_scene_interface, group, n)) { res.success = false; return false; } @@ -667,9 +668,9 @@ void manualScenePush(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &n) * @param n true on success */ bool pickAndDropObjectCallback(ccf::PickDropService::Request &req, - ccf::PickDropService::Response &res, ros::NodeHandle &n, - moveit::planning_interface::PlanningSceneInterface &planning_scene_interface, - moveit::planning_interface::MoveGroupInterface &group) { + ccf::PickDropService::Response &res, ros::NodeHandle &n, + moveit::planning_interface::PlanningSceneInterface &planning_scene_interface, + moveit::planning_interface::MoveGroupInterface &group) { world_state::current_bin_id = req.bin; @@ -692,7 +693,7 @@ bool pickAndDropObjectCallback(ccf::PickDropService::Request &req, } ROS_INFO_STREAM("[SCENECONTROLLER] Picking object."); - if(!pick(id, planning_scene_interface, group, n)){ + if (!pick(id, planning_scene_interface, group, n)) { res.success = false; return false; } diff --git a/src/dummies/dummy_cgv.cpp b/src/dummies/dummy_cgv.cpp index 74e6cc6c1c52c81c3035b68bdf7e07c06c51a3f5..aebccaa5716d23b6b0742d2530d54e2be5d0bdba 100644 --- a/src/dummies/dummy_cgv.cpp +++ b/src/dummies/dummy_cgv.cpp @@ -89,7 +89,8 @@ int main(int argc, char **argv) { ROS_DEBUG_STREAM("[dummyCgv] Content:" << std::endl << s); } else { ROS_WARN_STREAM("[dummyCgv] Received an invalid scene!" << std::endl - << "[dummyCgv] Partial content:" << std::endl << scene.ShortDebugString()); + << "[dummyCgv] Partial content:" << std::endl + << scene.ShortDebugString()); } // collect all object names diff --git a/src/dummy_cgv_controller.cpp b/src/dummy_cgv_controller.cpp index b07d6d64d62a1ce0ab4f5e178116ef257d4a27d2..fcd8d58ce8d92b6a85c5f73908e2e9d5b56556ce 100644 --- a/src/dummy_cgv_controller.cpp +++ b/src/dummy_cgv_controller.cpp @@ -9,8 +9,6 @@ #define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost -#include <fstream> - #include <ros/ros.h> #include <ros/package.h> #include <std_msgs/Empty.h> @@ -21,48 +19,10 @@ #include "ccf/connection/NngConnection.h" #include "ccf/util/NodeUtil.h" -#include <google/protobuf/text_format.h> -#include <google/protobuf/util/json_util.h> - const std::string NODE_NAME = "ros2cgv_dummy"; using CetiRosToolbox::getParameter; -void loadScene(DummyRobotArmController &connector) { - // read file into string: https://stackoverflow.com/questions/2602013/read-whole-ascii-file-into-c-stdstring - std::string path = ros::package::getPath("ccf") + "/config/config_scene.yaml"; - std::ifstream t(path); - - if (!t.is_open()) { - ROS_ERROR_STREAM("[" << NODE_NAME << "] Unable to open scene config file " << path); - } - - std::string str; - - t.seekg(0, std::ios::end); // skip to end of file - str.reserve(t.tellg()); // reserve memory in the string of the size of the file - t.seekg(0, std::ios::beg); // go back to the beginning - - // Note the double parentheses! F**k C++! https://en.wikipedia.org/wiki/Most_vexing_parse - str.assign((std::istreambuf_iterator<char>(t)), std::istreambuf_iterator<char>()); - - Scene newScene; - google::protobuf::util::Status status = google::protobuf::util::JsonStringToMessage(str, &newScene); - if (!status.ok()) { - ROS_ERROR_STREAM("[" << NODE_NAME << "] Unable to parse Json String: " << status.ToString()); - } else { - ROS_INFO_STREAM("[" << NODE_NAME << "] Parsed a scene with " << newScene.objects().size() << " objects."); - connector.initScene(newScene); - } - - std::string s; - if (google::protobuf::TextFormat::PrintToString(newScene, &s)) { - ROS_DEBUG_STREAM("[" << NODE_NAME << "] Received scene" << std::endl << s); - } else { - ROS_WARN_STREAM("[" << NODE_NAME << "] Scene invalid! partial content: " << newScene.ShortDebugString()); - } -} - int main(int argc, char **argv) { GOOGLE_PROTOBUF_VERIFY_VERSION; @@ -85,7 +45,7 @@ int main(int argc, char **argv) { connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update")); connector.addConnection(std::move(connection)); - loadScene(connector); + connector.loadScene(ros::package::getPath("ccf") + "/config/config_scene.yaml"); Object *robot = nullptr; Object *selectedBox = nullptr; @@ -147,7 +107,8 @@ int main(int argc, char **argv) { connector.resolveObject(currentlyPickedBox.value()); } catch (std::out_of_range &e) { ROS_INFO_STREAM( - "[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!"); + "[" << NODE_NAME << "] box " << currentlyPickedBox.value() + << " has been removed from the scene!"); currentlyPickedBox.reset(); } } diff --git a/src/dummy_rag_controller_a.cpp b/src/dummy_rag_controller_a.cpp index b471b3fb9794952e52377517635bc2a49ee451fe..e597bbadb1532da82f87fecebd5b5082c7e34156 100644 --- a/src/dummy_rag_controller_a.cpp +++ b/src/dummy_rag_controller_a.cpp @@ -62,8 +62,8 @@ int main(int argc, char **argv) { ros::Timer timer = n.createTimer(ros::Duration(10), [&connector]( const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds - Object* selectedBox{nullptr}; - Object* selectedLocation{nullptr}; + Object *selectedBox{nullptr}; + Object *selectedLocation{nullptr}; std::optional<std::string> currentlyPickedBox{}; auto selectionLambda = [¤tlyPickedBox, &connector, &selectedBox, &selectedLocation]( const Selection &selection) { @@ -97,7 +97,7 @@ int main(int argc, char **argv) { // we use the first robot we can find for our picking and placing std::optional<Object> robot; - for (auto& r : connector.getScene()->objects()) { + for (auto &r : connector.getScene()->objects()) { if (r.type() == Object::ARM) { robot = r; break; diff --git a/src/moveit_cgv_controller.cpp b/src/moveit_cgv_controller.cpp index cc1da1befcad4790a607adcaa39f332085278481..019a9634e42c400240706f9f50e876f026b0ddcf 100644 --- a/src/moveit_cgv_controller.cpp +++ b/src/moveit_cgv_controller.cpp @@ -106,7 +106,8 @@ int main(int argc, char **argv) { connector.resolveObject(currentlyPickedBox.value()); } catch (std::out_of_range &e) { ROS_INFO_STREAM( - "[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!"); + "[" << NODE_NAME << "] box " << currentlyPickedBox.value() + << " has been removed from the scene!"); currentlyPickedBox.reset(); } }