diff --git a/README.md b/README.md index f68aff172a35ded08cbe68a07ce9719345213e9e..8a2718b2c0fb0681d91da0594643f318fa9eafc9 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,6 @@ -# CCF: The CeTI Cobot Framework +# CCF: The CeTI Cobotic Framework + +**TODO** the documentation does not represent the current state of the framework and needs to be updated! This ROS package provides a connection between a ROS-based model and the [CGV framework](https://github.com/sgumhold/cgv). The purpose of this connection is to use the CGV framework to select and move elements from a scene provided by ROS @@ -86,7 +88,7 @@ This demonstrator contains two mock-up nodes that can serve as stand-ins for fut #### Execution - pick an object by id: `rostopic pub -1 /robot/pick std_msgs/String "objectRed1"` -- place an picked object into a bin by id: `rostopic pub -1 /robot/place std_msgs/String "binRed"` +- place a picked object into a bin by id: `rostopic pub -1 /robot/place std_msgs/String "binRed"` ### Using the Example with Docker diff --git a/include/ccf/controller/MoveItRobotArmController.h b/include/ccf/controller/MoveItRobotArmController.h index 5bf63512ef3a1f368e3bc548005e264353e399b0..3c8a3b3f0141d761da5c0d424601d6020ec82d60 100644 --- a/include/ccf/controller/MoveItRobotArmController.h +++ b/include/ccf/controller/MoveItRobotArmController.h @@ -15,6 +15,8 @@ private: ros::ServiceClient pick_drop_client; ros::ServiceClient pick_place_client; + ros::ServiceClient bin_check_client; + ros::ServiceServer get_scene_service; public: explicit MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName); diff --git a/src/ccf/controller/MoveItRobotArmController.cpp b/src/ccf/controller/MoveItRobotArmController.cpp index 70e22554a430bf52ddb4380a9d82ecdbeaa7a59a..73fcf7ce7c9ec9574fb35d465ad4837fec0e20c7 100644 --- a/src/ccf/controller/MoveItRobotArmController.cpp +++ b/src/ccf/controller/MoveItRobotArmController.cpp @@ -130,7 +130,18 @@ bool MoveItRobotArmController::pickAndPlace(Object &robot, Object &object, Objec MoveItRobotArmController::MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName) : RobotArmController(nodeHandle, cellName), pick_drop_client(nodeHandle.serviceClient<ccf::PickDropService>("/pick_drop_service")), - pick_place_client(nodeHandle.serviceClient<ccf::PickPlaceService>("/pick_place_service")) {} + pick_place_client(nodeHandle.serviceClient<ccf::PickPlaceService>("/pick_place_service")) { + + bin_check_client = nodeHandle.serviceClient<ccf::BinCheck>("/check_bin_service"); + get_scene_service = nodeHandle.advertiseService<ccf::SceneUpdateRequest, ccf::SceneUpdateResponse>( + "updateCgvScene", + [this](auto &req, auto &res) { + ROS_INFO_STREAM("[MoveItRobotArmController] Received a scene update from the controller."); + updateScene(req, bin_check_client); + return true; + } + ); +} bool MoveItRobotArmController::reachableObject(const Object &robot, const Object &object) { return false; diff --git a/src/moveit_cgv_controller.cpp b/src/moveit_cgv_controller.cpp index afe697f3e7771c52a4562a48b07a58641a184fbd..c8884b928ddf5062d8e04f8faa9361e4ce9fbd59 100644 --- a/src/moveit_cgv_controller.cpp +++ b/src/moveit_cgv_controller.cpp @@ -14,10 +14,6 @@ #include "connector.pb.h" - -#include <ccf/BinCheck.h> -#include <ccf/SceneUpdate.h> - #include "ccf/controller/MoveItRobotArmController.h" #include "ccf/connection/NngConnection.h" #include "ccf/util/NodeUtil.h" @@ -50,9 +46,9 @@ int main(int argc, char **argv) { // scene loading is not required, the scene is updated by moveit - Object* robot = nullptr; - Object* selectedBox = nullptr; - Object* selectedBin = nullptr; + Object *robot = nullptr; + Object *selectedBox = nullptr; + Object *selectedBin = nullptr; std::optional<std::string> currentlyPickedBox{}; ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector]( @@ -60,7 +56,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 - auto lambda = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](const Selection &selection) { + auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin]( + const Selection &selection) { if (currentlyPickedBox.has_value()) { ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress."); @@ -94,24 +91,16 @@ int main(int argc, char **argv) { } } }; + connector.reactToSelectionMessage(selectionMessageCallback); - connector.reactToSelectionMessage(lambda); - - ros::ServiceClient bin_check_client = n.serviceClient<ccf::BinCheck>("/check_bin_service"); - ros::ServiceServer get_scene_service = n.advertiseService<ccf::SceneUpdateRequest, ccf::SceneUpdateResponse>( - "updateCgvScene", - [¤tlyPickedBox, &connector, &bin_check_client](auto &req, auto &res) { - ROS_INFO_STREAM("[" << NODE_NAME << "] Received a scene update from the controller."); - connector.updateScene(req, bin_check_client); - if (currentlyPickedBox.has_value() && - !connector.resolveObject(currentlyPickedBox.value())) { - ROS_INFO_STREAM("[" << NODE_NAME << "] box " << currentlyPickedBox.value() - << " has been removed from the scene!"); - currentlyPickedBox.reset(); - } - return true; - } - ); + auto sceneUpdateCallback = [¤tlyPickedBox, &connector]() { + if (currentlyPickedBox.has_value() && !connector.resolveObject(currentlyPickedBox.value())) { + ROS_INFO_STREAM( + "[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!"); + currentlyPickedBox.reset(); + } + }; + connector.reactToSceneUpdateMessage(sceneUpdateCallback); ros::spin();