From c57b6ff631f973d1652dc0495aa4a9080b383292 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Mon, 10 Jan 2022 10:18:27 +0100
Subject: [PATCH] update demo
---
include/cell_controller.h | 20 ++-
include/task.h | 4 +-
src/dummy_sorting_controller.cpp | 188 +++++++++++------------
src/main_controller.cpp | 247 +++++++++++++++++-------------
src/moveit_sorting_controller.cpp | 188 +++++++++++------------
5 files changed, 335 insertions(+), 312 deletions(-)
diff --git a/include/cell_controller.h b/include/cell_controller.h
index 13c41bd..e1e6567 100644
--- a/include/cell_controller.h
+++ b/include/cell_controller.h
@@ -14,6 +14,7 @@ class CellController
Cell cell_;
std::shared_ptr<Action> current_action_;
std::shared_ptr<Task> current_task_;
+ std::function<void(const Action&)> action_callback_;
public:
CellController()
@@ -26,6 +27,11 @@ public:
cell_.id = id;
}
+ void setActionCallback(const std::function<void(const Action &)> &action_callback)
+ {
+ action_callback_ = action_callback;
+ }
+
const Cell& cell() const
{
return cell_;
@@ -46,6 +52,14 @@ public:
return current_task_;
}
+ void setCurrentTask(const std::shared_ptr<Task> &new_task)
+ {
+ if (!currentActionCompleted()) {
+ ROS_WARN_STREAM("Cell " << cell_.id << " got a new task although an action of the previous task is still ongoing.");
+ }
+ current_task_.reset(new_task.get());
+ }
+
void proceedWithTask()
{
if (cell_.state == Cell::IDLE)
@@ -108,19 +122,19 @@ public:
std::optional<Action> triggerNextAction()
{
// TODO this assumes each action can be performed only once, which holds for pick-and-drop tasks but not in general
- for (Action action : current_task_->GetActions())
+ for (Action action : current_task_->getActions())
{
if (!action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
{
cell_.state = Cell::WORKING;
- // TODO send selections to controller
+ ROS_INFO_STREAM("Triggering action " << action);
+ action_callback_(action);
return action;
}
}
return std::nullopt;
}
- friend class CellController;
};
#endif // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
diff --git a/include/task.h b/include/task.h
index 9ffcd42..0c78047 100644
--- a/include/task.h
+++ b/include/task.h
@@ -9,12 +9,12 @@ class Task
std::vector<Action> actions_;
public:
- void AddAction(const Action& action)
+ void addAction(const Action& action)
{
actions_.emplace_back(action);
}
- [[nodiscard]] const std::vector<Action>& GetActions() const
+ [[nodiscard]] const std::vector<Action>& getActions() const
{
return actions_;
}
diff --git a/src/dummy_sorting_controller.cpp b/src/dummy_sorting_controller.cpp
index 52b4147..587450a 100644
--- a/src/dummy_sorting_controller.cpp
+++ b/src/dummy_sorting_controller.cpp
@@ -23,103 +23,93 @@ std::string NODE_NAME = "dummy_sorting_controller";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
-int main(int argc, char **argv) {
-
- GOOGLE_PROTOBUF_VERIFY_VERSION;
-
- ros::init(argc, argv, NODE_NAME);
- NODE_NAME = ros::this_node::getName();
-
- ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
-
- auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
-
- DummyRobotArmController connector{n, NODE_NAME};
-
- // add an NNG connection
- std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
- connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
- connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
- connector.addConnection(std::move(connection));
-
- auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
- std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
- mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
- connector.addConnection(std::move(mqtt_connection));
-
- ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
-
- connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
- "/config/config_scene_st-table.json"));
-
- 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](
- const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
- ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
- const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
-
- auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
- const Selection &selection) {
-
- // forward the selection to the clients
- connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"),
- selection.SerializeAsString());
-
- if (currentlyPickedBox.has_value()) {
- ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
- return;
- }
-
- Object *object = connector.resolveObject(selection.id());
- if (!object) {
- ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
- return;
- }
- auto type = Object::Type_Name(object->type());
- if (object->type() == Object::BOX) {
- selectedBox = object;
- ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
- } else if (object->type() == Object::BIN) {
- selectedBin = object;
- ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
- } else {
- ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
- }
-
- if (selectedBin && selectedBox) {
- auto boxId = selectedBox->id();
- ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
- currentlyPickedBox = boxId;
- if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
- ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
- selectedBox = nullptr;
- selectedBin = nullptr;
- } else {
- ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
- selectedBox = nullptr;
- selectedBin = nullptr;
- connector.sendScene();
- }
- }
- };
- connector.reactToSelectionMessage(selectionMessageCallback);
-
- auto sceneUpdateCallback = [¤tlyPickedBox, &connector]() {
- if (currentlyPickedBox.has_value()) {
- auto resolved = connector.resolveObject(currentlyPickedBox.value());
- if (!resolved) {
- ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
- currentlyPickedBox.reset();
- }
- }
- };
- connector.reactToSceneUpdateMessage(sceneUpdateCallback);
-
- ros::spin();
-
- return 0;
+int main(int argc, char **argv)
+{
+
+ GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+ ros::init(argc, argv, NODE_NAME);
+ NODE_NAME = ros::this_node::getName();
+
+ ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
+
+ auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
+
+ DummyRobotArmController controller{n, NODE_NAME};
+
+ auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
+ std::unique_ptr<MqttConnection>
+ mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
+ mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
+ mqtt_connection->listen(getParameter(n, "topics/command", "/" + ros::this_node::getName() + "/command"));
+ controller.addConnection(std::move(mqtt_connection));
+
+ ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
+
+ controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+ "/config/config_scene_st-table.json"));
+
+ std::optional<std::string> currentlyPickedBox{};
+
+ ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
+ const std_msgs::EmptyConstPtr &msg)
+ { controller.sendScene(); });
+ ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](
+ const ros::TimerEvent &event)
+ { controller.sendScene(); }); // send a scene every ten seconds
+
+
+ auto pick_place_callback = [&controller](const PickPlace& pick_place)
+ {
+ Object *pick_object = controller.resolveObject(pick_place.idpick());
+ if (!pick_object)
+ {
+ ROS_ERROR_STREAM(
+ "Selected unknown pick object '" << pick_place.idpick() << "'");
+ return;
+ }
+ Object *place_object = controller.resolveObject(pick_place.idplace());
+ if (!place_object)
+ {
+ ROS_ERROR_STREAM(
+ "Selected unknown place object '" << pick_place.idplace() << "'");
+ return;
+ }
+ if (pick_object->type() != Object::BOX)
+ {
+ ROS_WARN_STREAM(
+ "Selected unsupported pick object of type " << pick_object->type());
+ }
+ if (place_object->type() != Object::BIN)
+ {
+ ROS_WARN_STREAM(
+ "Selected unsupported place object of type " << place_object->type());
+ }
+ Object *robot = nullptr; // TODO
+ if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
+ ROS_WARN_STREAM("Unable to remove box '" << pick_object->id() << "'!");
+ } else {
+ ROS_INFO_STREAM("Job is done! '" << pick_object->id() << "' is no more.");
+ controller.sendScene();
+ }
+ };
+ controller.reactToPickAndPlaceMessage(pick_place_callback);
+
+ auto sceneUpdateCallback = [¤tlyPickedBox, &controller]()
+ {
+ if (currentlyPickedBox.has_value())
+ {
+ auto resolved = controller.resolveObject(currentlyPickedBox.value());
+ if (!resolved)
+ {
+ ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
+ currentlyPickedBox.reset();
+ }
+ }
+ };
+ controller.reactToSceneUpdateMessage(sceneUpdateCallback);
+
+ ros::spin();
+
+ return 0;
}
\ No newline at end of file
diff --git a/src/main_controller.cpp b/src/main_controller.cpp
index eae8c3a..778ceb4 100644
--- a/src/main_controller.cpp
+++ b/src/main_controller.cpp
@@ -12,7 +12,7 @@
#include "ccf/connection/MqttConnection.h"
#include "ccf/util/NodeUtil.h"
-const char* DEFAULT_NODE_NAME = "main_controller";
+const char *DEFAULT_NODE_NAME = "main_controller";
std::string NODE_NAME;
using CetiRosToolbox::getParameter;
@@ -22,17 +22,17 @@ const float SELECT = 2.0;
const float UNSELECT = 0.5;
const float DELETING = 0.08;
-void highlight(Object* selectedBin, float factor)
+void highlight(Object *selected_bin, float factor)
{
- if (selectedBin)
+ if (selected_bin)
{
- selectedBin->mutable_color()->set_r(selectedBin->color().r() * factor);
- selectedBin->mutable_color()->set_g(selectedBin->color().g() * factor);
- selectedBin->mutable_color()->set_b(selectedBin->color().b() * factor);
+ selected_bin->mutable_color()->set_r(selected_bin->color().r() * factor);
+ selected_bin->mutable_color()->set_g(selected_bin->color().g() * factor);
+ selected_bin->mutable_color()->set_b(selected_bin->color().b() * factor);
}
}
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
GOOGLE_PROTOBUF_VERIFY_VERSION;
@@ -41,141 +41,170 @@ int main(int argc, char** argv)
ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
- auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
+ auto connection_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
- DummyRobotArmController connector{ n, NODE_NAME };
+ DummyRobotArmController controller{n, NODE_NAME};
std::map<std::string, CellController> clients;
std::vector<std::pair<std::string, std::string>> actions;
// add an NNG connection
- std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
+ std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connection_address);
connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
- connector.addConnection(std::move(connection));
+ controller.addConnection(std::move(connection));
- auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
- auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
+ auto client_controllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
+ auto mqtt_server = getPrivateParameter<std::string>("mqtt_server", "tcp://127.0.0.1:1883");
- ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
- std::shared_ptr<MqttConnection> client_connection = std::make_shared<MqttConnection>(NODE_NAME, mqttServer);
- connector.addConnection(client_connection);
- for (const auto& client : clientControllers)
+ // create an empty task
+ auto task = std::make_shared<Task>();
+
+ ROS_INFO_STREAM("Connecting to " << client_controllers.size() << " client controllers.");
+ std::shared_ptr<MqttConnection> client_connection = std::make_shared<MqttConnection>(mqtt_server, NODE_NAME);
+ controller.addConnection(client_connection);
+ for (const auto &client: client_controllers)
{
ROS_INFO_STREAM("Connecting to client at " << client << ".");
clients[client].setCellId(client);
- auto scene_update_topic = "/" + client + getParameter(n, "topics/scene", NODE_NAME + "/scene/update");
+ clients[client].setCurrentTask(task);
+ clients[client].setActionCallback([client, &controller](const Action &action)
+ {
+ ROS_INFO_STREAM("Action Callback Called for " << action);
+ PickPlace command;
+ command.set_idpick(action.getSource());
+ command.set_idplace(action.getTarget());
+ controller.sendToAll(client + "/command", command.SerializeAsString());
+ });
+ auto scene_update_topic = "/" + client + getParameter<std::string>(n, "topics/scene", "/scene/update");
+ ROS_INFO_STREAM("Listening to MQTT topic " << scene_update_topic);
client_connection->listen(scene_update_topic);
- connector.addCallback(scene_update_topic, [client, &clients](auto msg) { clients[client].updateModel(msg); });
+ controller.addCallback(scene_update_topic, [client, &clients](auto msg)
+ { clients[client].updateModel(msg); });
}
auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-table.json";
- connector.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
+ controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
- Object* robot = nullptr;
- Object* selected_box = nullptr;
- Object* selectedBin = nullptr;
- std::optional<std::string> currentlyPickedBox{};
+ Object *robot = nullptr;
+ Object *selected_box = nullptr;
+ Object *selected_bin = nullptr;
+ std::optional<std::string> picked_box{};
ros::Subscriber sub = n.subscribe<std_msgs::Empty>(
- "send_scene", 1000, [&connector](const std_msgs::EmptyConstPtr& msg) { connector.sendScene(); });
- ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](const ros::TimerEvent& event) {
- connector.sendScene();
+ "send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr &msg)
+ { controller.sendScene(); });
+ ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](const ros::TimerEvent &event)
+ {
+ controller.sendScene();
}); // send a scene every ten seconds
//
- ros::Timer timer_scene_log = n.createTimer(ros::Duration(10), [&clients](const ros::TimerEvent& event) {
- for (const auto& [name, cell_controller] : clients)
+ ros::Timer timer_scene_log = n.createTimer(ros::Duration(10), [&clients](const ros::TimerEvent &event)
+ {
+ for (const auto&[name, cell_controller]: clients)
{
- ROS_WARN_STREAM("CLIENT " << name << " has a cell_controller with " << cell_controller.cell().scene.objects_size()
- << "objects");
+ ROS_WARN_STREAM("CLIENT " << name << " has a cell controller with " << cell_controller.cell().scene.objects_size()
+ << " objects which is " << cell_controller.cell().stateString());
}
});
- auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selected_box,
- &selectedBin](const Selection& selection) {
- if (currentlyPickedBox.has_value())
- {
- ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
- return;
- }
-
- Object* object = connector.resolveObject(selection.id());
- if (!object)
- {
- ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
- return;
- }
- auto type = Object::Type_Name(object->type());
- if (object->type() == Object::BOX)
- {
- highlight(selected_box, UNSELECT);
- if (selected_box == object)
- {
- selected_box = nullptr;
- }
- else
- {
- selected_box = object;
- highlight(selected_box, SELECT);
- }
- connector.sendScene();
- ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
- }
- else if (object->type() == Object::BIN)
- {
- highlight(selectedBin, UNSELECT);
- if (selectedBin == object)
+ auto selection_message_callback =
+ [&picked_box, &controller, &robot, &selected_box, &selected_bin, &task, &clients](const Selection &selection)
{
- selectedBin = nullptr;
- }
- else
- {
- selectedBin = object;
- highlight(selectedBin, SELECT);
- }
- connector.sendScene();
- ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
- }
- else
- {
- ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
- }
-
- if (selectedBin && selected_box)
- {
- auto boxId = selected_box->id();
- ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
- currentlyPickedBox = boxId;
- highlight(selectedBin, UNSELECT);
- highlight(selected_box, DELETING);
- connector.sendScene();
- if (!connector.pickAndDrop(*robot, *selected_box, *selectedBin, false))
- {
- ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
- }
- else
- {
- ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
- }
- selected_box = nullptr;
- selectedBin = nullptr;
- connector.sendScene();
- }
- };
- connector.reactToSelectionMessage(selectionMessageCallback);
-
- auto sceneUpdateCallback = [¤tlyPickedBox, &connector]() {
- if (currentlyPickedBox.has_value())
+ if (picked_box.has_value())
+ {
+ ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
+ return;
+ }
+
+ Object *object = controller.resolveObject(selection.id());
+ if (!object)
+ {
+ ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
+ return;
+ }
+ auto type = Object::Type_Name(object->type());
+ if (object->type() == Object::BOX)
+ {
+ highlight(selected_box, UNSELECT);
+ if (selected_box == object)
+ {
+ selected_box = nullptr;
+ }
+ else
+ {
+ selected_box = object;
+ highlight(selected_box, SELECT);
+ }
+ controller.sendScene();
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+ }
+ else if (object->type() == Object::BIN)
+ {
+ highlight(selected_bin, UNSELECT);
+ if (selected_bin == object)
+ {
+ selected_bin = nullptr;
+ }
+ else
+ {
+ selected_bin = object;
+ highlight(selected_bin, SELECT);
+ }
+ controller.sendScene();
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+ }
+ else
+ {
+ ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
+ }
+
+ if (selected_bin && selected_box)
+ {
+ auto boxId = selected_box->id();
+ ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selected_bin->id());
+ picked_box = boxId;
+ highlight(selected_bin, UNSELECT);
+ highlight(selected_box, DELETING);
+ controller.sendScene();
+
+ // create an action and add it to the current task
+ Action pick_drop_action(Action::PICK_DROP, selected_box->id(), selected_bin->id());
+ task->addAction(pick_drop_action);
+
+ for (auto &[name, client]: clients)
+ {
+ client.proceedWithTask();
+ }
+
+ if (!controller.pickAndDrop(*robot, *selected_box, *selected_bin, false))
+ {
+ ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
+ }
+ else
+ {
+ ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
+ }
+ selected_box = nullptr;
+ selected_bin = nullptr;
+ controller.sendScene();
+ }
+ };
+ controller.reactToSelectionMessage(selection_message_callback);
+
+ auto scene_update_callback = [&picked_box, &controller]()
+ {
+ if (picked_box.has_value())
{
- auto resolved = connector.resolveObject(currentlyPickedBox.value());
+ auto resolved = controller.resolveObject(picked_box.value());
if (!resolved)
{
- ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
- currentlyPickedBox.reset();
+ ROS_INFO_STREAM("box " << picked_box.value() << " has been removed from the scene!");
+ picked_box.reset();
}
}
};
- connector.reactToSceneUpdateMessage(sceneUpdateCallback);
+ controller.reactToSceneUpdateMessage(scene_update_callback);
ros::spin();
diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp
index d6c538d..8946fe4 100644
--- a/src/moveit_sorting_controller.cpp
+++ b/src/moveit_sorting_controller.cpp
@@ -23,103 +23,93 @@ std::string NODE_NAME = "moveit_sorting_controller";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
-int main(int argc, char **argv) {
-
- GOOGLE_PROTOBUF_VERIFY_VERSION;
-
- ros::init(argc, argv, NODE_NAME);
- NODE_NAME = ros::this_node::getName();
-
- ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
-
- auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
-
- MoveItRobotArmController connector{n, NODE_NAME};
-
- // add an NNG connection
- std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
- connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
- connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
- connector.addConnection(std::move(connection));
-
- auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
- std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
- mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
- connector.addConnection(std::move(mqtt_connection));
-
- ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
-
- connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
- "/config/config_scene_st-table.json"));
-
- 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](
- const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
- ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
- const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
-
- auto selectionMessageCallback = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
- const Selection &selection) {
-
- // forward the selection to the clients
- connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"),
- selection.SerializeAsString());
-
- if (currentlyPickedBox.has_value()) {
- ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
- return;
- }
-
- Object *object = connector.resolveObject(selection.id());
- if (!object) {
- ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
- return;
- }
- auto type = Object::Type_Name(object->type());
- if (object->type() == Object::BOX) {
- selectedBox = object;
- ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
- } else if (object->type() == Object::BIN) {
- selectedBin = object;
- ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
- } else {
- ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
- }
-
- if (selectedBin && selectedBox) {
- auto boxId = selectedBox->id();
- ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
- currentlyPickedBox = boxId;
- if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
- ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
- selectedBox = nullptr;
- selectedBin = nullptr;
- } else {
- ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
- selectedBox = nullptr;
- selectedBin = nullptr;
- connector.sendScene();
- }
- }
- };
- connector.reactToSelectionMessage(selectionMessageCallback);
-
- auto sceneUpdateCallback = [¤tlyPickedBox, &connector]() {
- if (currentlyPickedBox.has_value()) {
- auto resolved = connector.resolveObject(currentlyPickedBox.value());
- if (!resolved) {
- ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
- currentlyPickedBox.reset();
- }
- }
- };
- connector.reactToSceneUpdateMessage(sceneUpdateCallback);
-
- ros::spin();
-
- return 0;
+int main(int argc, char **argv)
+{
+
+ GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+ ros::init(argc, argv, NODE_NAME);
+ NODE_NAME = ros::this_node::getName();
+
+ ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
+
+ auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
+
+ MoveItRobotArmController controller{n, NODE_NAME};
+
+ auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
+ std::unique_ptr<MqttConnection>
+ mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
+ mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
+ mqtt_connection->listen(getParameter(n, "topics/command", "/" + ros::this_node::getName() + "/command"));
+ controller.addConnection(std::move(mqtt_connection));
+
+ ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
+
+ controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+ "/config/config_scene_st-table.json"));
+
+ std::optional<std::string> currentlyPickedBox{};
+
+ ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
+ const std_msgs::EmptyConstPtr &msg)
+ { controller.sendScene(); });
+ ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](
+ const ros::TimerEvent &event)
+ { controller.sendScene(); }); // send a scene every ten seconds
+
+
+ auto pick_place_callback = [&controller](const PickPlace& pick_place)
+ {
+ Object *pick_object = controller.resolveObject(pick_place.idpick());
+ if (!pick_object)
+ {
+ ROS_ERROR_STREAM(
+ "Selected unknown pick object '" << pick_place.idpick() << "'");
+ return;
+ }
+ Object *place_object = controller.resolveObject(pick_place.idplace());
+ if (!place_object)
+ {
+ ROS_ERROR_STREAM(
+ "Selected unknown place object '" << pick_place.idplace() << "'");
+ return;
+ }
+ if (pick_object->type() != Object::BOX)
+ {
+ ROS_WARN_STREAM(
+ "Selected unsupported pick object of type " << pick_object->type());
+ }
+ if (place_object->type() != Object::BIN)
+ {
+ ROS_WARN_STREAM(
+ "Selected unsupported place object of type " << place_object->type());
+ }
+ Object *robot = nullptr; // TODO
+ if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
+ ROS_WARN_STREAM("Unable to remove box '" << pick_object->id() << "'!");
+ } else {
+ ROS_INFO_STREAM("Job is done! '" << pick_object->id() << "' is no more.");
+ controller.sendScene();
+ }
+ };
+ controller.reactToPickAndPlaceMessage(pick_place_callback);
+
+ auto sceneUpdateCallback = [¤tlyPickedBox, &controller]()
+ {
+ if (currentlyPickedBox.has_value())
+ {
+ auto resolved = controller.resolveObject(currentlyPickedBox.value());
+ if (!resolved)
+ {
+ ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
+ currentlyPickedBox.reset();
+ }
+ }
+ };
+ controller.reactToSceneUpdateMessage(sceneUpdateCallback);
+
+ ros::spin();
+
+ return 0;
}
\ No newline at end of file
--
GitLab