Skip to content
Snippets Groups Projects
Commit a8b47f48 authored by Johannes Mey's avatar Johannes Mey
Browse files

move code into class and callback

parent 1b8e9b77
No related branches found
No related tags found
No related merge requests found
# 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). 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 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 ...@@ -86,7 +88,7 @@ This demonstrator contains two mock-up nodes that can serve as stand-ins for fut
#### Execution #### Execution
- pick an object by id: `rostopic pub -1 /robot/pick std_msgs/String "objectRed1"` - 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 ### Using the Example with Docker
......
...@@ -15,6 +15,8 @@ private: ...@@ -15,6 +15,8 @@ private:
ros::ServiceClient pick_drop_client; ros::ServiceClient pick_drop_client;
ros::ServiceClient pick_place_client; ros::ServiceClient pick_place_client;
ros::ServiceClient bin_check_client;
ros::ServiceServer get_scene_service;
public: public:
explicit MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName); explicit MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName);
......
...@@ -130,7 +130,18 @@ bool MoveItRobotArmController::pickAndPlace(Object &robot, Object &object, Objec ...@@ -130,7 +130,18 @@ bool MoveItRobotArmController::pickAndPlace(Object &robot, Object &object, Objec
MoveItRobotArmController::MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName) MoveItRobotArmController::MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName)
: RobotArmController(nodeHandle, cellName), : RobotArmController(nodeHandle, cellName),
pick_drop_client(nodeHandle.serviceClient<ccf::PickDropService>("/pick_drop_service")), 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) { bool MoveItRobotArmController::reachableObject(const Object &robot, const Object &object) {
return false; return false;
......
...@@ -14,10 +14,6 @@ ...@@ -14,10 +14,6 @@
#include "connector.pb.h" #include "connector.pb.h"
#include <ccf/BinCheck.h>
#include <ccf/SceneUpdate.h>
#include "ccf/controller/MoveItRobotArmController.h" #include "ccf/controller/MoveItRobotArmController.h"
#include "ccf/connection/NngConnection.h" #include "ccf/connection/NngConnection.h"
#include "ccf/util/NodeUtil.h" #include "ccf/util/NodeUtil.h"
...@@ -60,7 +56,8 @@ int main(int argc, char **argv) { ...@@ -60,7 +56,8 @@ int main(int argc, char **argv) {
ros::Timer timer = n.createTimer(ros::Duration(10), [&connector]( ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
auto lambda = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](const Selection &selection) { auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](
const Selection &selection) {
if (currentlyPickedBox.has_value()) { if (currentlyPickedBox.has_value()) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress."); ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
...@@ -94,24 +91,16 @@ int main(int argc, char **argv) { ...@@ -94,24 +91,16 @@ int main(int argc, char **argv) {
} }
} }
}; };
connector.reactToSelectionMessage(selectionMessageCallback);
connector.reactToSelectionMessage(lambda); auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value() && !connector.resolveObject(currentlyPickedBox.value())) {
ros::ServiceClient bin_check_client = n.serviceClient<ccf::BinCheck>("/check_bin_service"); ROS_INFO_STREAM(
ros::ServiceServer get_scene_service = n.advertiseService<ccf::SceneUpdateRequest, ccf::SceneUpdateResponse>( "[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!");
"updateCgvScene",
[&currentlyPickedBox, &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(); currentlyPickedBox.reset();
} }
return true; };
} connector.reactToSceneUpdateMessage(sceneUpdateCallback);
);
ros::spin(); ros::spin();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment