From d314ec04e5bae2ab139e731523991fc2f6ef3c0a Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Wed, 5 Jan 2022 17:08:17 +0100
Subject: [PATCH] move files, apply formatting, work on demo
---
CMakeLists.txt | 4 +-
include/action.h | 124 ++++++++++
include/cell.h | 30 +++
include/cell_controller.h | 126 ++++++++++
include/task.h | 23 ++
src/action.cpp | 1 +
src/cell.cpp | 2 +
src/cell_controller.cpp | 1 +
src/main_controller.cpp | 478 ++++++++++++--------------------------
src/task.cpp | 1 +
10 files changed, 456 insertions(+), 334 deletions(-)
create mode 100644 include/action.h
create mode 100644 include/cell.h
create mode 100644 include/cell_controller.h
create mode 100644 include/task.h
create mode 100644 src/action.cpp
create mode 100644 src/cell.cpp
create mode 100644 src/cell_controller.cpp
create mode 100644 src/task.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c2491a8..66e85de 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -135,8 +135,8 @@ include_directories(
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
-add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
-add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
+add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
+add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/cell.cpp src/action.cpp src/task.cpp src/cell_controller.cpp include/cell_controller.h)
add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
diff --git a/include/action.h b/include/action.h
new file mode 100644
index 0000000..0e60abc
--- /dev/null
+++ b/include/action.h
@@ -0,0 +1,124 @@
+#ifndef CCF_IMMERSIVE_SORTING_ACTION_H_
+#define CCF_IMMERSIVE_SORTING_ACTION_H_
+
+#include <string>
+
+#include "connector.pb.h"
+
+class Action
+{
+public:
+ enum Type
+ {
+ UNDEFINED,
+ PICK_DROP
+ };
+
+private:
+ const Type type_;
+ const std::string source_;
+ const std::string target_;
+
+public:
+ bool operator==(const Action& rhs) const
+ {
+ return type_ == rhs.type_ && source_ == rhs.source_ && target_ == rhs.target_;
+ }
+ bool operator!=(const Action& rhs) const
+ {
+ return !(rhs == *this);
+ }
+
+private:
+ static constexpr Object::Type requiredSourceType(Type type)
+ {
+ switch (type)
+ {
+ case PICK_DROP:
+ return Object::BOX;
+ case UNDEFINED:
+ default:
+ return Object::UNKNOWN;
+ }
+ }
+ static constexpr Object::Type requiredTargetType(Type type)
+ {
+ switch (type)
+ {
+ case PICK_DROP:
+ return Object::BIN;
+ case UNDEFINED:
+ default:
+ return Object::UNKNOWN;
+ }
+ }
+
+ [[nodiscard]] static std::optional<Object> lookupObject(const Scene& scene, const std::string& id)
+ {
+ for (Object object : scene.objects())
+ {
+ if (object.id() == id)
+ {
+ return object;
+ }
+ }
+ return std::nullopt;
+ }
+
+public:
+ [[nodiscard]] Type getType() const
+ {
+ return type_;
+ }
+
+ [[nodiscard]] const std::string& getSource() const
+ {
+ return source_;
+ }
+
+ [[nodiscard]] const std::string& getTarget() const
+ {
+ return target_;
+ }
+
+public:
+ /**
+ * Constructor
+ * @param type
+ * @param source
+ * @param target
+ */
+ Action(Type type, std::string source, std::string target)
+ : type_(type), source_(std::move(source)), target_(std::move(target))
+ {
+ }
+
+ /**
+ * an action can be performed in a scene if source and target exist in the scene and have the correct type for the
+ * action
+ * @param scene
+ * @return true if the action can be performed
+ */
+ bool canBePerformed(const Scene& scene)
+ {
+ auto s = lookupObject(scene, source_);
+ auto t = lookupObject(scene, target_);
+ return s && t && s->type() == requiredSourceType(type_) && t->type() == requiredTargetType(type_);
+ }
+
+ [[nodiscard]] bool isCompleted(const Scene& scene) const
+ {
+ switch (type_)
+ {
+ case Action::Type::PICK_DROP:
+ // pick and drop is completed if the dropped element no longer exists
+ return !lookupObject(scene, source_).has_value();
+ case Action::Type::UNDEFINED:
+ // undefined actions are defined to be completed
+ default:
+ return true;
+ }
+ }
+};
+
+#endif // CCF_IMMERSIVE_SORTING_ACTION_H_
diff --git a/include/cell.h b/include/cell.h
new file mode 100644
index 0000000..7ac7677
--- /dev/null
+++ b/include/cell.h
@@ -0,0 +1,30 @@
+#ifndef CCF_IMMERSIVE_SORTING_CELL_H_
+#define CCF_IMMERSIVE_SORTING_CELL_H_
+
+#include <connector.pb.h>
+#include <optional>
+#include <string>
+
+struct Cell
+{
+ enum State
+ {
+ UNDEFINED,
+ DISCONNECTED,
+ IDLE,
+ WORKING
+ };
+
+ std::string id;
+ Scene scene;
+ State state = UNDEFINED;
+ std::optional<std::string> processed_object;
+
+ Cell() = default;
+
+ // one does not copy cells
+ Cell(const Cell&) = delete;
+ Cell& operator=(const Cell&) = delete;
+};
+
+#endif // CCF_IMMERSIVE_SORTING_CELL_H_
diff --git a/include/cell_controller.h b/include/cell_controller.h
new file mode 100644
index 0000000..13c41bd
--- /dev/null
+++ b/include/cell_controller.h
@@ -0,0 +1,126 @@
+#ifndef CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
+#define CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
+
+#include <utility>
+
+#include "cell.h"
+#include "action.h"
+#include "task.h"
+
+#include "ros/ros.h"
+
+class CellController
+{
+ Cell cell_;
+ std::shared_ptr<Action> current_action_;
+ std::shared_ptr<Task> current_task_;
+
+public:
+ CellController()
+ {
+ cell_.state = Cell::DISCONNECTED;
+ }
+
+ void setCellId(const std::string& id)
+ {
+ cell_.id = id;
+ }
+
+ const Cell& cell() const
+ {
+ return cell_;
+ }
+
+ bool hasCurrentAction() const
+ {
+ return current_action_ == nullptr;
+ }
+
+ const Action& getCurrentAction() const
+ {
+ return *current_action_;
+ }
+
+ const std::shared_ptr<Task>& getCurrentTask() const
+ {
+ return current_task_;
+ }
+
+ void proceedWithTask()
+ {
+ if (cell_.state == Cell::IDLE)
+ {
+ if (currentActionCompleted())
+ {
+ auto action = triggerNextAction();
+ }
+ }
+ }
+
+ void updateModel(const std::string& scene_string)
+ {
+ ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
+ if (cell_.state == Cell::DISCONNECTED)
+ {
+ cell_.state = Cell::IDLE;
+ }
+
+ cell_.scene.ParseFromString(scene_string);
+ proceedWithTask();
+ }
+
+ // TODO do we need this method?
+ void updateModel(const Scene& scene)
+ {
+ ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
+ if (cell_.state == Cell::DISCONNECTED)
+ {
+ cell_.state = Cell::IDLE;
+ }
+
+ cell_.scene.CopyFrom(scene);
+ proceedWithTask();
+ }
+
+ /**
+ * A current action is completed if there is no current action or the current action can no longer be performed
+ * @return
+ */
+ bool currentActionCompleted()
+ {
+ if (current_action_ == nullptr)
+ {
+ return true;
+ }
+ else if (current_action_->isCompleted(cell_.scene))
+ {
+ // the current action is completed, but this was not registered before, so we have to set the cell state
+ cell_.state = Cell::IDLE;
+ current_action_ = nullptr;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+ 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())
+ {
+ if (!action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
+ {
+ cell_.state = Cell::WORKING;
+ // TODO send selections to controller
+ 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
new file mode 100644
index 0000000..9ffcd42
--- /dev/null
+++ b/include/task.h
@@ -0,0 +1,23 @@
+#ifndef CCF_IMMERSIVE_SORTING_TASK_H_
+#define CCF_IMMERSIVE_SORTING_TASK_H_
+
+#include <vector>
+#include "action.h"
+
+class Task
+{
+ std::vector<Action> actions_;
+
+public:
+ void AddAction(const Action& action)
+ {
+ actions_.emplace_back(action);
+ }
+
+ [[nodiscard]] const std::vector<Action>& GetActions() const
+ {
+ return actions_;
+ }
+};
+
+#endif // CCF_IMMERSIVE_SORTING_TASK_H_
diff --git a/src/action.cpp b/src/action.cpp
new file mode 100644
index 0000000..1eba944
--- /dev/null
+++ b/src/action.cpp
@@ -0,0 +1 @@
+#include "action.h"
diff --git a/src/cell.cpp b/src/cell.cpp
new file mode 100644
index 0000000..f5f0a4f
--- /dev/null
+++ b/src/cell.cpp
@@ -0,0 +1,2 @@
+#include "cell.h"
+
diff --git a/src/cell_controller.cpp b/src/cell_controller.cpp
new file mode 100644
index 0000000..b6b7799
--- /dev/null
+++ b/src/cell_controller.cpp
@@ -0,0 +1 @@
+#include "cell_controller.h"
diff --git a/src/main_controller.cpp b/src/main_controller.cpp
index 892fafd..eae8c3a 100644
--- a/src/main_controller.cpp
+++ b/src/main_controller.cpp
@@ -1,17 +1,9 @@
-/*! \file dummy_sorting_controller.cpp
-
- A ROS node that simulates a node connected to a model-based cobotic application
-
- \author Johannes Mey
- \author Sebastian Ebert
- \date 07.07.20
-*/
-
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Empty.h>
#include <utility>
+#include <cell_controller.h>
#include "connector.pb.h"
@@ -20,7 +12,8 @@
#include "ccf/connection/MqttConnection.h"
#include "ccf/util/NodeUtil.h"
-std::string NODE_NAME = "main_controller";
+const char* DEFAULT_NODE_NAME = "main_controller";
+std::string NODE_NAME;
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
@@ -29,341 +22,162 @@ const float SELECT = 2.0;
const float UNSELECT = 0.5;
const float DELETING = 0.08;
-
-
-struct Cell {
- enum CELL_STATE {
- UNDEFINED,
- DISCONNECTED,
- IDLE,
- WORKING
- };
-
- Scene scene;
- CELL_STATE state = UNDEFINED;
- std::optional<std::string> processedObject;
-};
-
-class Action {
- public:
- enum Type {
- UNDEFINED,
- PICK_DROP
- };
-
- private:
- const Type type;
- const std::string source;
- const std::string target;
-
- public:
- bool operator==(const Action &rhs) const {
- return type == rhs.type &&
- source == rhs.source &&
- target == rhs.target;
- }
- bool operator!=(const Action &rhs) const {
- return !(rhs == *this);
- }
- private:
-
- static constexpr Object::Type requiredSourceType(Type type) {
- switch (type) {
- case UNDEFINED:return Object::UNKNOWN;
- case PICK_DROP:return Object::BOX;
- }
- }
- static constexpr Object::Type requiredTargetType(Type type) {
- switch (type) {
- case UNDEFINED:return Object::UNKNOWN;
- case PICK_DROP:return Object::BIN;
- }
- }
-
- std::optional<Object> lookupObject(const Scene &scene, const std::string &id) const {
- for (Object object: scene.objects()) {
- if (object.id() == id) {
- return object;
- }
- }
- return std::nullopt;
- }
-
- public:
- Type GetType() const {
- return type;
- }
-
- const std::string &GetSource() const {
- return source;
- }
-
- const std::string &GetTarget() const {
- return target;
- }
-
- public:
- /**
- * Constructor
- * @param type
- * @param source
- * @param target
- */
- Action(Type type, std::string source, std::string target)
- : type(type), source(std::move(source)), target(std::move(target)) {}
-
- /**
- * an action can be performed in a scene if source and target exist in the scene and have the correct type for the action
- * @param scene
- * @return true if the action can be performed
- */
- bool canBePerformed(const Scene &scene) {
- auto s = lookupObject(scene, source);
- auto t = lookupObject(scene, target);
- return s && t && s->type() == requiredSourceType(type) && t->type() == requiredTargetType(type);
- }
-
- bool isCompleted(const Scene &scene) const {
- switch (type) {
- case Action::Type::PICK_DROP:
- // pick and drop is completed if the dropped element no longer exists
- return !lookupObject(scene, source).has_value();
- case Action::Type::UNDEFINED:
- // undefined actions are defined to be completed
- return true;
- }
- }
-
-};
-
-class Task {
-
- std::vector<Action> actions;
-
- public:
-
- void addAction(const Action &action) {
- actions.emplace_back(action);
- }
-
- const std::vector<Action> &GetActions() const {
- return actions;
- }
-
-};
-
-class CellController {
- Cell cell;
- std::shared_ptr<Action> currentAction;
- std::shared_ptr<Task> currentTask;
-
- public:
- const Cell &GetCell() const {
- return cell;
- }
- void SetCell(const Cell &cell) {
- CellController::cell = cell;
- }
-
- bool HasCurrentAction() const {
- return currentAction == nullptr;
- }
-
- const Action &GetCurrentAction() const {
- return *currentAction;
- }
- void SetCurrentAction(Action current_action) {
- currentAction.reset(¤t_action);
- }
- const std::shared_ptr<Task> &GetCurrentTask() const {
- return currentTask;
- }
- void SetCurrentTask(const std::shared_ptr<Task> ¤t_task) {
- currentTask = current_task;
+void highlight(Object* selectedBin, float factor)
+{
+ if (selectedBin)
+ {
+ 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);
}
+}
- void proceedWithTask() {
- if (cell.state == Cell::IDLE) {
+int main(int argc, char** argv)
+{
+ GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+ ros::init(argc, argv, DEFAULT_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 };
+
+ 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);
+ 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 clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
+ auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost: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)
+ {
+ 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");
+ client_connection->listen(scene_update_topic);
+ connector.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));
+
+ Object* robot = nullptr;
+ Object* selected_box = 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
+
+ //
+ 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");
+ }
+ });
+
+ 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;
}
- }
-
- void updateModel(Scene scene) {
-
- if (cell.state == Cell::DISCONNECTED) {
- cell.state == Cell::IDLE;
+ else
+ {
+ selected_box = object;
+ highlight(selected_box, SELECT);
}
-
- cell.scene = std::move(scene);
- if (currentActionCompleted()) {
- auto action = triggerNextAction();
- // TODO send selections to controller
+ connector.sendScene();
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+ }
+ else if (object->type() == Object::BIN)
+ {
+ highlight(selectedBin, UNSELECT);
+ if (selectedBin == object)
+ {
+ selectedBin = nullptr;
}
- }
-
- /**
- * A current action is completed if there is no current action or the current action can no longer be performed
- * @return
- */
- bool currentActionCompleted() {
- return (currentAction == nullptr) || !currentAction->isCompleted(cell.scene);
- }
-
-
-
- std::optional<Action> triggerNextAction() {
- for (Action action : currentTask->GetActions()) {
- if (!action.isCompleted(cell.scene) && action.canBePerformed(cell.scene)) {
- return action;
- }
+ else
+ {
+ selectedBin = object;
+ highlight(selectedBin, SELECT);
}
- return std::nullopt;
- }
-
-};
-
-void highlight(Object *selectedBin, float factor) {
- if (selectedBin) {
- 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);
+ connector.sendScene();
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
}
-}
-
-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};
-
- std::map<std::string, Cell> clients;
- std::vector<std::pair<std::string, std::string>> actions;
-
- // 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 clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
- auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost: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) {
- ROS_INFO_STREAM("Connecting to client at " << client << ".");
- auto scene_update_topic = "/" + client + getParameter(n, "topics/scene", NODE_NAME + "/scene/update");
- client_connection->listen(scene_update_topic);
- connector.addCallback(scene_update_topic,
- [client, &clients](auto msg) { clients[client].scene.ParseFromString(msg); });
+ else
+ {
+ ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
}
- connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
- "/config/config_scene_virtual-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
-
-
- ros::Timer timer_scene_log = n.createTimer(ros::Duration(10), [&clients](
- const ros::TimerEvent &event) {
-
- for (const auto &[name, scene]: clients) {
- ROS_WARN_STREAM("CLIENT " << name << " has a scene with " << scene.objects_size() << "objects");
+ 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 << "'!");
}
-
- });
-
- 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) {
- highlight(selectedBox, UNSELECT);
- if (selectedBox == object) {
- selectedBox = nullptr;
- } else {
- selectedBox = object;
- highlight(selectedBox, SELECT);
- }
- connector.sendScene();
- ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
- } else if (object->type() == Object::BIN) {
- highlight(selectedBin, UNSELECT);
- if (selectedBin == object) {
- 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 && selectedBox) {
- auto boxId = selectedBox->id();
- ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
- currentlyPickedBox = boxId;
- highlight(selectedBin, UNSELECT);
- highlight(selectedBox, DELETING);
- connector.sendScene();
- if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
- ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
- } else {
- ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
- }
- selectedBox = nullptr;
- selectedBin = nullptr;
- connector.sendScene();
- } else {
-
+ else
+ {
+ ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
}
- };
- 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();
- }
+ selected_box = 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);
+ }
+ };
+ connector.reactToSceneUpdateMessage(sceneUpdateCallback);
- ros::spin();
+ ros::spin();
- return 0;
+ return 0;
}
\ No newline at end of file
diff --git a/src/task.cpp b/src/task.cpp
new file mode 100644
index 0000000..204c27c
--- /dev/null
+++ b/src/task.cpp
@@ -0,0 +1 @@
+#include "task.h"
--
GitLab