diff --git a/include/action.h b/include/action.h
index 480b6c47682ec0638124904925f9dc457ba9bae4..1a1317fef3d0ac9131449c1430f0d6ffa3ef8739 100644
--- a/include/action.h
+++ b/include/action.h
@@ -53,11 +53,17 @@ private:
}
}
+public:
+ /// Lookup for an object starting with a prefix
+ /// \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 (object.id() == id)
+ // if the id can be found at position 0
+ if (object.id().rfind(id, 0) == 0)
{
return object;
}
@@ -65,7 +71,6 @@ private:
return std::nullopt;
}
-public:
[[nodiscard]] Type getType() const
{
return type_;
@@ -121,7 +126,6 @@ public:
}
friend std::ostream& operator<<(std::ostream& os, const Action& action);
-
};
std::ostream& operator<<(std::ostream& os, const Action& action);
diff --git a/include/cell.h b/include/cell.h
index 915a7b0009061f51ac38ddc21ceb97ec20ee06a6..505e1c27d21f89f6cd047625d8469abec9328c0d 100644
--- a/include/cell.h
+++ b/include/cell.h
@@ -15,8 +15,10 @@ struct Cell
WORKING
};
- std::string stateString() const {
- switch (state) {
+ std::string stateString() const
+ {
+ switch (state)
+ {
case DISCONNECTED:
return "DISCONNECTED";
case IDLE:
@@ -31,10 +33,21 @@ struct Cell
std::string id;
Scene scene;
State state = UNDEFINED;
- std::optional<std::string> processed_object;
Cell() = default;
+ std::string robot() const
+ {
+ for (const auto& object : scene.objects())
+ {
+ if (object.type() == Object_Type_ARM)
+ {
+ return object.id();
+ }
+ }
+ return "<no robot found>";
+ }
+
// one does not copy cells
Cell(const Cell&) = delete;
Cell& operator=(const Cell&) = delete;
diff --git a/include/cell_controller.h b/include/cell_controller.h
index de53c91085fc807c9f4351e52291d534758cbb01..784f299213258fd2bd6aed5cdb97345725df1b35 100644
--- a/include/cell_controller.h
+++ b/include/cell_controller.h
@@ -14,7 +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_;
+ std::function<void(const Action&)> action_callback_;
public:
CellController()
@@ -22,17 +22,17 @@ public:
cell_.state = Cell::DISCONNECTED;
}
- void setCellId(const std::string &id)
+ void setCellId(const std::string& id)
{
cell_.id = id;
}
- void setActionCallback(const std::function<void(const Action &)> &action_callback)
+ void setActionCallback(const std::function<void(const Action&)>& action_callback)
{
action_callback_ = action_callback;
}
- const Cell &cell() const
+ const Cell& cell() const
{
return cell_;
}
@@ -42,22 +42,22 @@ public:
return current_action_ == nullptr;
}
- const Action &getCurrentAction() const
+ const Action& getCurrentAction() const
{
return *current_action_;
}
- const std::shared_ptr<Task> &getCurrentTask() const
+ const std::shared_ptr<Task>& getCurrentTask() const
{
return current_task_;
}
- void setCurrentTask(const std::shared_ptr<Task> &new_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.");
+ 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());
}
@@ -70,28 +70,19 @@ public:
}
}
- void updateModel(const std::string &scene_string)
+ 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)
+ for (const auto& object : cell_.scene.objects())
{
- cell_.state = Cell::IDLE;
+ if (object.type() == Object_Type_ARM)
+ {
+ cell_.state = object.state() == Object_State_STATE_IDLE ? Cell::IDLE : Cell::WORKING;
+ break;
+ }
}
-
- cell_.scene.CopyFrom(scene);
proceedWithTask();
}
@@ -120,20 +111,31 @@ public:
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())
+ for (Action action : current_task_->getActions())
{
- if (!action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
+ 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);
- action_callback_(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;
}
-
};
#endif // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
diff --git a/src/main_controller.cpp b/src/main_controller.cpp
index 1849c6268177b78c885bbd76cf90906d66d93bd2..2b5271fb3c828fe828bc02440dedbcbe0ba106d9 100644
--- a/src/main_controller.cpp
+++ b/src/main_controller.cpp
@@ -18,8 +18,8 @@ std::string NODE_NAME;
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
-const float SELECT = 2.0;
-const float UNSELECT = 0.5;
+const float UNSELECT = 0.8;
+const float SELECT = 1/UNSELECT;
const float DELETING = 0.08;
void highlight(Object *selected_bin, float factor)
@@ -43,7 +43,10 @@ int main(int argc, char **argv)
auto connection_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
- DummyRobotArmController controller{n, NODE_NAME, "arm"};
+ auto robotName = getPrivateParameter<std::string>("arm", "virtual-arm");
+ ROS_INFO_STREAM("This cell controls arm " << robotName);
+
+ DummyRobotArmController controller{n, NODE_NAME, robotName};
std::map<std::string, CellController> clients;
std::vector<std::pair<std::string, std::string>> actions;
@@ -54,7 +57,7 @@ int main(int argc, char **argv)
connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
controller.addConnection(std::move(connection));
- auto client_controllers = getPrivateParameter<std::vector<std::string>>("client_controllers", {"ceti_cell_1", "ceti_cell_2", "ads-cell", "st-cell"});
+ auto client_controllers = getPrivateParameter<std::vector<std::string>>("client_controllers", {"ceti_cell_1", "ceti_cell_2", "ads-cell", "st-cell", "ceti_cell_empty"});
auto mqtt_server = getPrivateParameter<std::string>("mqtt_server", "tcp://127.0.0.1:1883");
// create an empty task
@@ -68,10 +71,11 @@ int main(int argc, char **argv)
ROS_INFO_STREAM("Connecting to client at " << client << ".");
clients[client].setCellId(client);
clients[client].setCurrentTask(task);
- clients[client].setActionCallback([client, &controller](const Action &action)
+ clients[client].setActionCallback([client, &controller, &clients](const Action &action)
{
ROS_INFO_STREAM("Action Callback Called for " << action);
Command command;
+ command.mutable_pickandplace()->set_idrobot(clients[client].cell().robot());
command.mutable_pickandplace()->set_idpick(action.getSource());
command.mutable_pickandplace()->set_idplace(action.getTarget());
controller.sendToAll("/" + client + "/command", command.SerializeAsString());
@@ -87,7 +91,7 @@ int main(int argc, char **argv)
// uncomment in case things fail. clients[client].proceedWithTask();
});
}
- auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-table.json";
+ auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-tag-table.json";
controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
Object *robot = controller.resolveObject(controller.getRobotName());