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

update controllers

parent d0e15453
No related branches found
No related tags found
No related merge requests found
......@@ -41,13 +41,15 @@ int main(int argc, char **argv)
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"));
std::string commandTopic = getParameter(n, "topics/command", ros::this_node::getName() + "/command");
mqtt_connection->listen(commandTopic);
controller.setCommandTopic(commandTopic);
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"));
"/config/config_scene_ceti-table.json"));
std::optional<std::string> currentlyPickedBox{};
......@@ -61,6 +63,7 @@ int main(int argc, char **argv)
auto pick_place_callback = [&controller](const PickPlace& pick_place)
{
ROS_INFO_STREAM("Got Command to pick object " << pick_place.idpick() << " and place it into " << pick_place.idplace());
Object *pick_object = controller.resolveObject(pick_place.idpick());
if (!pick_object)
{
......@@ -87,9 +90,9 @@ int main(int argc, char **argv)
}
Object *robot = nullptr; // TODO
if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
ROS_WARN_STREAM("Unable to remove box '" << pick_object->id() << "'!");
ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!");
} else {
ROS_INFO_STREAM("Job is done! '" << pick_object->id() << "' is no more.");
ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more.");
controller.sendScene();
}
};
......
......@@ -74,13 +74,17 @@ int main(int argc, char **argv)
PickPlace command;
command.set_idpick(action.getSource());
command.set_idplace(action.getTarget());
controller.sendToAll(client + "/command", command.SerializeAsString());
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);
controller.addCallback(scene_update_topic, [client, &clients](auto msg)
{ clients[client].updateModel(msg); });
{
ROS_INFO_STREAM("Got an updated scene from client " << client);
clients[client].updateModel(msg);
// uncomment in case things fail. clients[client].proceedWithTask();
});
}
auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-table.json";
controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
......
......@@ -41,13 +41,15 @@ int main(int argc, char **argv)
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"));
std::string commandTopic = getParameter(n, "topics/command", ros::this_node::getName() + "/command");
mqtt_connection->listen(commandTopic);
controller.setCommandTopic(commandTopic);
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"));
"/config/config_scene_ceti-table.json"));
std::optional<std::string> currentlyPickedBox{};
......@@ -61,6 +63,7 @@ int main(int argc, char **argv)
auto pick_place_callback = [&controller](const PickPlace& pick_place)
{
ROS_INFO_STREAM("Got Command to pick object " << pick_place.idpick() << " and place it into " << pick_place.idplace());
Object *pick_object = controller.resolveObject(pick_place.idpick());
if (!pick_object)
{
......@@ -87,9 +90,9 @@ int main(int argc, char **argv)
}
Object *robot = nullptr; // TODO
if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
ROS_WARN_STREAM("Unable to remove box '" << pick_object->id() << "'!");
ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!");
} else {
ROS_INFO_STREAM("Job is done! '" << pick_object->id() << "' is no more.");
ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more.");
controller.sendScene();
}
};
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment