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

update demo

parent 5fbb2f2a
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
include/cell_controller.h
+ 17
− 3
View file @ c57b6ff6
......@@ -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_
include/task.h
+ 2
− 2
View file @ c57b6ff6
......@@ -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_;
}
......
src/dummy_sorting_controller.cpp
+ 89
− 99
View file @ c57b6ff6
......@@ -23,7 +23,8 @@ std::string NODE_NAME = "dummy_sorting_controller";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
int main(int argc, char **argv) {
int main(int argc, char **argv)
{
GOOGLE_PROTOBUF_VERIFY_VERSION;
......@@ -34,90 +35,79 @@ int main(int argc, char **argv) {
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));
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());
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));
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)
connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
controller.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 = [&currentlyPickedBox, &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.");
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 *object = connector.resolveObject(selection.id());
if (!object) {
ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
Object *place_object = controller.resolveObject(pick_place.idplace());
if (!place_object)
{
ROS_ERROR_STREAM(
"Selected unknown place object '" << pick_place.idplace() << "'");
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 (pick_object->type() != Object::BOX)
{
ROS_WARN_STREAM(
"Selected unsupported pick object of type " << pick_object->type());
}
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();
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();
}
};
connector.reactToSelectionMessage(selectionMessageCallback);
auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value()) {
auto resolved = connector.resolveObject(currentlyPickedBox.value());
if (!resolved) {
controller.reactToPickAndPlaceMessage(pick_place_callback);
auto sceneUpdateCallback = [&currentlyPickedBox, &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();
}
}
};
connector.reactToSceneUpdateMessage(sceneUpdateCallback);
controller.reactToSceneUpdateMessage(sceneUpdateCallback);
ros::spin();
......
src/main_controller.cpp
+ 138
− 109
View file @ c57b6ff6
......@@ -22,13 +22,13 @@ 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);
}
}
......@@ -41,65 +41,83 @@ 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 *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) {
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 = [&currentlyPickedBox, &connector, &robot, &selected_box,
&selectedBin](const Selection& selection) {
if (currentlyPickedBox.has_value())
auto selection_message_callback =
[&picked_box, &controller, &robot, &selected_box, &selected_bin, &task, &clients](const Selection &selection)
{
if (picked_box.has_value())
{
ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
return;
}
Object* object = connector.resolveObject(selection.id());
Object *object = controller.resolveObject(selection.id());
if (!object)
{
ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
......@@ -118,22 +136,22 @@ int main(int argc, char** argv)
selected_box = object;
highlight(selected_box, SELECT);
}
connector.sendScene();
controller.sendScene();
ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
}
else if (object->type() == Object::BIN)
{
highlight(selectedBin, UNSELECT);
if (selectedBin == object)
highlight(selected_bin, UNSELECT);
if (selected_bin == object)
{
selectedBin = nullptr;
selected_bin = nullptr;
}
else
{
selectedBin = object;
highlight(selectedBin, SELECT);
selected_bin = object;
highlight(selected_bin, SELECT);
}
connector.sendScene();
controller.sendScene();
ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
}
else
......@@ -141,15 +159,25 @@ int main(int argc, char** argv)
ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
}
if (selectedBin && selected_box)
if (selected_bin && selected_box)
{
auto boxId = selected_box->id();
ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
currentlyPickedBox = boxId;
highlight(selectedBin, UNSELECT);
ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selected_bin->id());
picked_box = boxId;
highlight(selected_bin, UNSELECT);
highlight(selected_box, DELETING);
connector.sendScene();
if (!connector.pickAndDrop(*robot, *selected_box, *selectedBin, false))
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 << "'!");
}
......@@ -158,24 +186,25 @@ int main(int argc, char** argv)
ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
}
selected_box = nullptr;
selectedBin = nullptr;
connector.sendScene();
selected_bin = nullptr;
controller.sendScene();
}
};
connector.reactToSelectionMessage(selectionMessageCallback);
controller.reactToSelectionMessage(selection_message_callback);
auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value())
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();
......
src/moveit_sorting_controller.cpp
+ 89
− 99
View file @ c57b6ff6
......@@ -23,7 +23,8 @@ std::string NODE_NAME = "moveit_sorting_controller";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
int main(int argc, char **argv) {
int main(int argc, char **argv)
{
GOOGLE_PROTOBUF_VERIFY_VERSION;
......@@ -34,90 +35,79 @@ int main(int argc, char **argv) {
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));
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());
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));
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)
connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
controller.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 = [&currentlyPickedBox, &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.");
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 *object = connector.resolveObject(selection.id());
if (!object) {
ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
Object *place_object = controller.resolveObject(pick_place.idplace());
if (!place_object)
{
ROS_ERROR_STREAM(
"Selected unknown place object '" << pick_place.idplace() << "'");
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 (pick_object->type() != Object::BOX)
{
ROS_WARN_STREAM(
"Selected unsupported pick object of type " << pick_object->type());
}
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();
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();
}
};
connector.reactToSelectionMessage(selectionMessageCallback);
auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value()) {
auto resolved = connector.resolveObject(currentlyPickedBox.value());
if (!resolved) {
controller.reactToPickAndPlaceMessage(pick_place_callback);
auto sceneUpdateCallback = [&currentlyPickedBox, &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();
}
}
};
connector.reactToSceneUpdateMessage(sceneUpdateCallback);
controller.reactToSceneUpdateMessage(sceneUpdateCallback);
ros::spin();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment