diff --git a/CMakeLists.txt b/CMakeLists.txt
index e947f59e331fe9d71940752abee60abc4307884b..f5c9aeeb7aea7c0d029f9531af41a85f6600d4a9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -137,7 +137,7 @@ include_directories(
add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_dummy_pickplace_provider src/dummy_pickplace_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} src/cell.cpp src/action.cpp src/task.cpp src/cell_controller.cpp include/cell_controller.h)
+add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/action.cpp src/cell_controller.cpp include/cell_controller.h)
add_executable(${PROJECT_NAME}_virtual_scene_provider src/virtual_scene_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_object_locator src/object_locator.cpp ${PROTO_SRCS} ${PROTO_HDRS})
diff --git a/include/action.h b/include/action.h
index 1a1317fef3d0ac9131449c1430f0d6ffa3ef8739..32a84656a1d1d3217f069f9e9a08e2f3ba6fc034 100644
--- a/include/action.h
+++ b/include/action.h
@@ -58,18 +58,7 @@ public:
/// \param scene
/// \param id the prefix the object starts with
/// \return
- [[nodiscard]] static std::optional<Object> lookupObject(const Scene& scene, const std::string& id)
- {
- for (Object object : scene.objects())
- {
- // if the id can be found at position 0
- if (object.id().rfind(id, 0) == 0)
- {
- return object;
- }
- }
- return std::nullopt;
- }
+ [[nodiscard]] static std::optional<Object> lookupObject(const Scene& scene, const std::string& id);
[[nodiscard]] Type getType() const
{
@@ -93,10 +82,7 @@ public:
* @param source
* @param target
*/
- Action(Type type, std::string source, std::string target)
- : type_(type), source_(std::move(source)), target_(std::move(target))
- {
- }
+ Action(Type type, std::string source, std::string target);
/**
* an action can be performed in a scene if source and target exist in the scene and have the correct type for the
@@ -104,26 +90,9 @@ public:
* @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 canBePerformed(const Scene& scene);
- [[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;
- }
- }
+ [[nodiscard]] bool isCompleted(const Scene& scene) const;
friend std::ostream& operator<<(std::ostream& os, const Action& action);
};
diff --git a/include/cell_controller.h b/include/cell_controller.h
index 784f299213258fd2bd6aed5cdb97345725df1b35..617e96169d6c1da812b2e3b65c7d7e8e9f5ab664 100644
--- a/include/cell_controller.h
+++ b/include/cell_controller.h
@@ -37,105 +37,34 @@ public:
return cell_;
}
- bool hasCurrentAction() const
+ [[maybe_unused]] bool hasCurrentAction() const
{
return current_action_ == nullptr;
}
- const Action& getCurrentAction() const
+ [[maybe_unused]] const Action& getCurrentAction() const
{
return *current_action_;
}
- const std::shared_ptr<Task>& getCurrentTask() const
+ [[maybe_unused]] const std::shared_ptr<Task>& getCurrentTask() const
{
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 setCurrentTask(const std::shared_ptr<Task>& new_task);
- void proceedWithTask()
- {
- if (currentActionCompleted())
- {
- auto action = triggerNextAction();
- }
- }
+ void proceedWithTask();
- void updateModel(const std::string& scene_string)
- {
- ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
-
- cell_.scene.ParseFromString(scene_string);
- for (const auto& object : cell_.scene.objects())
- {
- if (object.type() == Object_Type_ARM)
- {
- cell_.state = object.state() == Object_State_STATE_IDLE ? Cell::IDLE : Cell::WORKING;
- break;
- }
- }
- proceedWithTask();
- }
+ void updateModel(const std::string& scene_string);
/**
* 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;
- }
- }
+ bool currentActionCompleted();
- std::optional<Action> triggerNextAction()
- {
- if (current_action_ != nullptr && !current_action_->isCompleted(cell_.scene))
- {
- return std::nullopt;
- }
- // 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())
- {
- ROS_WARN_STREAM("Action completed: " << !action.isCompleted(cell_.scene));
- if (cell_.state != Cell::WORKING && !action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
- {
- cell_.state = Cell::WORKING;
- ROS_INFO_STREAM("Triggering action " << action);
- auto source = Action::lookupObject(cell_.scene, action.getSource());
- auto target = Action::lookupObject(cell_.scene, action.getTarget());
- if (!source || !target)
- {
- return std::nullopt;
- }
- current_action_ = std::make_shared<Action>(action.getType(), source->id(), target->id());
- action_callback_(*current_action_);
- return action;
- }
- }
- return std::nullopt;
- }
+ std::optional<Action> triggerNextAction();
};
#endif // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
diff --git a/src/action.cpp b/src/action.cpp
index 419ba79ddaa6d16ea1e4cc87ca50a2bb78d1ad1a..94229d7837649e00e0c386c3c82e0af1547df2ac 100644
--- a/src/action.cpp
+++ b/src/action.cpp
@@ -11,4 +11,39 @@ std::ostream& operator<<(std::ostream& os, const Action& action)
os << "[unknown action with source " << action.source_ << " and target " << action.target_ << "]";
}
return os;
-}
\ No newline at end of file
+}
+std::optional<Object> Action::lookupObject(const Scene& scene, const std::string& id)
+{
+ for (Object object : scene.objects())
+ {
+ // if the id can be found at position 0
+ if (object.id().rfind(id, 0) == 0)
+ {
+ return object;
+ }
+ }
+ return std::nullopt;
+}
+Action::Action(Action::Type type, std::string source, std::string target)
+ : type_(type), source_(std::move(source)), target_(std::move(target))
+{
+}
+bool Action::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 Action::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;
+ }
+}
diff --git a/src/cell.cpp b/src/cell.cpp
deleted file mode 100644
index 6c6de79faaad2b3b0c85acdd309a9a7f81526a3a..0000000000000000000000000000000000000000
--- a/src/cell.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "cell.h"
diff --git a/src/cell_controller.cpp b/src/cell_controller.cpp
index b6b779996ef36c46233ebd037811446c4c76c5c6..99b070521b4ac6822c7b4b66cb2080bf430ebe84 100644
--- a/src/cell_controller.cpp
+++ b/src/cell_controller.cpp
@@ -1 +1,77 @@
#include "cell_controller.h"
+std::optional<Action> CellController::triggerNextAction()
+{
+ if (current_action_ != nullptr && !current_action_->isCompleted(cell_.scene))
+ {
+ return std::nullopt;
+ }
+ // 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())
+ {
+ ROS_WARN_STREAM("Action completed: " << !action.isCompleted(cell_.scene));
+ if (cell_.state != Cell::WORKING && !action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
+ {
+ cell_.state = Cell::WORKING;
+ ROS_INFO_STREAM("Triggering action " << action);
+ auto source = Action::lookupObject(cell_.scene, action.getSource());
+ auto target = Action::lookupObject(cell_.scene, action.getTarget());
+ if (!source || !target)
+ {
+ return std::nullopt;
+ }
+ current_action_ = std::make_shared<Action>(action.getType(), source->id(), target->id());
+ action_callback_(*current_action_);
+ return action;
+ }
+ }
+ return std::nullopt;
+}
+bool CellController::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;
+ }
+}
+void CellController::updateModel(const std::string& scene_string)
+{
+ ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
+
+ cell_.scene.ParseFromString(scene_string);
+ for (const auto& object : cell_.scene.objects())
+ {
+ if (object.type() == Object_Type_ARM)
+ {
+ cell_.state = object.state() == Object_State_STATE_IDLE ? Cell::IDLE : Cell::WORKING;
+ break;
+ }
+ }
+ proceedWithTask();
+}
+void CellController::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 CellController::proceedWithTask()
+{
+ if (currentActionCompleted())
+ {
+ auto action = triggerNextAction();
+ }
+}
diff --git a/src/task.cpp b/src/task.cpp
deleted file mode 100644
index 204c27c5737a9215ca00e5dc65d81329fa4a6c40..0000000000000000000000000000000000000000
--- a/src/task.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "task.h"