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

update logic for immersive sorting demo (selection by name prefix)

parent 3f02535f
No related branches found
No related tags found
No related merge requests found
...@@ -53,11 +53,17 @@ private: ...@@ -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) [[nodiscard]] static std::optional<Object> lookupObject(const Scene& scene, const std::string& id)
{ {
for (Object object : scene.objects()) 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; return object;
} }
...@@ -65,7 +71,6 @@ private: ...@@ -65,7 +71,6 @@ private:
return std::nullopt; return std::nullopt;
} }
public:
[[nodiscard]] Type getType() const [[nodiscard]] Type getType() const
{ {
return type_; return type_;
...@@ -121,7 +126,6 @@ public: ...@@ -121,7 +126,6 @@ public:
} }
friend std::ostream& operator<<(std::ostream& os, const Action& action); friend std::ostream& operator<<(std::ostream& os, const Action& action);
}; };
std::ostream& operator<<(std::ostream& os, const Action& action); std::ostream& operator<<(std::ostream& os, const Action& action);
......
...@@ -15,8 +15,10 @@ struct Cell ...@@ -15,8 +15,10 @@ struct Cell
WORKING WORKING
}; };
std::string stateString() const { std::string stateString() const
switch (state) { {
switch (state)
{
case DISCONNECTED: case DISCONNECTED:
return "DISCONNECTED"; return "DISCONNECTED";
case IDLE: case IDLE:
...@@ -31,10 +33,21 @@ struct Cell ...@@ -31,10 +33,21 @@ struct Cell
std::string id; std::string id;
Scene scene; Scene scene;
State state = UNDEFINED; State state = UNDEFINED;
std::optional<std::string> processed_object;
Cell() = default; 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 // one does not copy cells
Cell(const Cell&) = delete; Cell(const Cell&) = delete;
Cell& operator=(const Cell&) = delete; Cell& operator=(const Cell&) = delete;
......
...@@ -56,8 +56,8 @@ public: ...@@ -56,8 +56,8 @@ public:
{ {
if (!currentActionCompleted()) if (!currentActionCompleted())
{ {
ROS_WARN_STREAM( ROS_WARN_STREAM("Cell " << cell_.id
"Cell " << cell_.id << " got a new task although an action of the previous task is still ongoing."); << " got a new task although an action of the previous task is still ongoing.");
} }
current_task_.reset(new_task.get()); current_task_.reset(new_task.get());
} }
...@@ -73,25 +73,16 @@ public: ...@@ -73,25 +73,16 @@ 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); 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); cell_.scene.ParseFromString(scene_string);
proceedWithTask(); for (const auto& object : cell_.scene.objects())
}
// TODO do we need this method?
void updateModel(const Scene &scene)
{ {
ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id); if (object.type() == Object_Type_ARM)
if (cell_.state == Cell::DISCONNECTED)
{ {
cell_.state = Cell::IDLE; cell_.state = object.state() == Object_State_STATE_IDLE ? Cell::IDLE : Cell::WORKING;
break;
}
} }
cell_.scene.CopyFrom(scene);
proceedWithTask(); proceedWithTask();
} }
...@@ -120,20 +111,31 @@ public: ...@@ -120,20 +111,31 @@ public:
std::optional<Action> triggerNextAction() 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 // 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; cell_.state = Cell::WORKING;
ROS_INFO_STREAM("Triggering action " << action); 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 action;
} }
} }
return std::nullopt; return std::nullopt;
} }
}; };
#endif // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_ #endif // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
...@@ -18,8 +18,8 @@ std::string NODE_NAME; ...@@ -18,8 +18,8 @@ std::string NODE_NAME;
using CetiRosToolbox::getParameter; using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter; using CetiRosToolbox::getPrivateParameter;
const float SELECT = 2.0; const float UNSELECT = 0.8;
const float UNSELECT = 0.5; const float SELECT = 1/UNSELECT;
const float DELETING = 0.08; const float DELETING = 0.08;
void highlight(Object *selected_bin, float factor) void highlight(Object *selected_bin, float factor)
...@@ -43,7 +43,10 @@ int main(int argc, char **argv) ...@@ -43,7 +43,10 @@ int main(int argc, char **argv)
auto connection_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576"); 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::map<std::string, CellController> clients;
std::vector<std::pair<std::string, std::string>> actions; std::vector<std::pair<std::string, std::string>> actions;
...@@ -54,7 +57,7 @@ int main(int argc, char **argv) ...@@ -54,7 +57,7 @@ int main(int argc, char **argv)
connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update")); connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
controller.addConnection(std::move(connection)); 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"); auto mqtt_server = getPrivateParameter<std::string>("mqtt_server", "tcp://127.0.0.1:1883");
// create an empty task // create an empty task
...@@ -68,10 +71,11 @@ int main(int argc, char **argv) ...@@ -68,10 +71,11 @@ int main(int argc, char **argv)
ROS_INFO_STREAM("Connecting to client at " << client << "."); ROS_INFO_STREAM("Connecting to client at " << client << ".");
clients[client].setCellId(client); clients[client].setCellId(client);
clients[client].setCurrentTask(task); 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); ROS_INFO_STREAM("Action Callback Called for " << action);
Command command; Command command;
command.mutable_pickandplace()->set_idrobot(clients[client].cell().robot());
command.mutable_pickandplace()->set_idpick(action.getSource()); command.mutable_pickandplace()->set_idpick(action.getSource());
command.mutable_pickandplace()->set_idplace(action.getTarget()); command.mutable_pickandplace()->set_idplace(action.getTarget());
controller.sendToAll("/" + client + "/command", command.SerializeAsString()); controller.sendToAll("/" + client + "/command", command.SerializeAsString());
...@@ -87,7 +91,7 @@ int main(int argc, char **argv) ...@@ -87,7 +91,7 @@ int main(int argc, char **argv)
// uncomment in case things fail. clients[client].proceedWithTask(); // 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)); controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
Object *robot = controller.resolveObject(controller.getRobotName()); Object *robot = controller.resolveObject(controller.getRobotName());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment