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
......@@ -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_
......@@ -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_;
}
......
......@@ -23,103 +23,93 @@ std::string NODE_NAME = "dummy_sorting_controller";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
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};
// 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 mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
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));
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") +
"/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.");
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) {
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 (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();
}
}
};
connector.reactToSelectionMessage(selectionMessageCallback);
auto sceneUpdateCallback = [&currentlyPickedBox, &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);
ros::spin();
return 0;
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 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());
mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
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)
controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
"/config/config_scene_st-table.json"));
std::optional<std::string> currentlyPickedBox{};
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 *place_object = controller.resolveObject(pick_place.idplace());
if (!place_object)
{
ROS_ERROR_STREAM(
"Selected unknown place object '" << pick_place.idplace() << "'");
return;
}
if (pick_object->type() != Object::BOX)
{
ROS_WARN_STREAM(
"Selected unsupported pick object of type " << pick_object->type());
}
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();
}
};
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();
}
}
};
controller.reactToSceneUpdateMessage(sceneUpdateCallback);
ros::spin();
return 0;
}
\ No newline at end of file
......@@ -12,7 +12,7 @@
#include "ccf/connection/MqttConnection.h"
#include "ccf/util/NodeUtil.h"
const char* DEFAULT_NODE_NAME = "main_controller";
const char *DEFAULT_NODE_NAME = "main_controller";
std::string NODE_NAME;
using CetiRosToolbox::getParameter;
......@@ -22,17 +22,17 @@ 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);
}
}
int main(int argc, char** argv)
int main(int argc, char **argv)
{
GOOGLE_PROTOBUF_VERIFY_VERSION;
......@@ -41,141 +41,170 @@ 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 *robot = nullptr;
Object *selected_box = nullptr;
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) {
for (const auto& [name, cell_controller] : clients)
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())
{
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;
}
else
{
selected_box = object;
highlight(selected_box, SELECT);
}
connector.sendScene();
ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
}
else if (object->type() == Object::BIN)
{
highlight(selectedBin, UNSELECT);
if (selectedBin == object)
auto selection_message_callback =
[&picked_box, &controller, &robot, &selected_box, &selected_bin, &task, &clients](const Selection &selection)
{
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 && 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 << "'!");
}
else
{
ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
}
selected_box = nullptr;
selectedBin = nullptr;
connector.sendScene();
}
};
connector.reactToSelectionMessage(selectionMessageCallback);
auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value())
if (picked_box.has_value())
{
ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
return;
}
Object *object = controller.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;
}
else
{
selected_box = object;
highlight(selected_box, SELECT);
}
controller.sendScene();
ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
}
else if (object->type() == Object::BIN)
{
highlight(selected_bin, UNSELECT);
if (selected_bin == object)
{
selected_bin = nullptr;
}
else
{
selected_bin = object;
highlight(selected_bin, SELECT);
}
controller.sendScene();
ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
}
else
{
ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
}
if (selected_bin && selected_box)
{
auto boxId = selected_box->id();
ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selected_bin->id());
picked_box = boxId;
highlight(selected_bin, UNSELECT);
highlight(selected_box, DELETING);
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 << "'!");
}
else
{
ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
}
selected_box = nullptr;
selected_bin = nullptr;
controller.sendScene();
}
};
controller.reactToSelectionMessage(selection_message_callback);
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();
......
......@@ -23,103 +23,93 @@ std::string NODE_NAME = "moveit_sorting_controller";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
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");
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));
auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
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));
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") +
"/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.");
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) {
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 (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();
}
}
};
connector.reactToSelectionMessage(selectionMessageCallback);
auto sceneUpdateCallback = [&currentlyPickedBox, &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);
ros::spin();
return 0;
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");
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());
mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
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)
controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
"/config/config_scene_st-table.json"));
std::optional<std::string> currentlyPickedBox{};
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 *place_object = controller.resolveObject(pick_place.idplace());
if (!place_object)
{
ROS_ERROR_STREAM(
"Selected unknown place object '" << pick_place.idplace() << "'");
return;
}
if (pick_object->type() != Object::BOX)
{
ROS_WARN_STREAM(
"Selected unsupported pick object of type " << pick_object->type());
}
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();
}
};
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();
}
}
};
controller.reactToSceneUpdateMessage(sceneUpdateCallback);
ros::spin();
return 0;
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment