From 73908282a8649793a84d4065857aa948a1589d03 Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Mon, 12 Sep 2022 09:59:00 +0200 Subject: [PATCH] add virtual scene provider --- CMakeLists.txt | 6 + .../virtual_scene_provider.launch | 13 ++ src/virtual_scene_provider.cpp | 145 ++++++++++++++++++ 3 files changed, 164 insertions(+) create mode 100644 launch/distributed-sorting/virtual_scene_provider.launch create mode 100644 src/virtual_scene_provider.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index cd1041f..f75dbf9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,6 +138,7 @@ add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_prov add_executable(${PROJECT_NAME}_dummy_pickplace_provider src/dummy_pickplace_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS}) add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS}) add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/cell.cpp src/action.cpp src/task.cpp src/cell_controller.cpp include/cell_controller.h) +add_executable(${PROJECT_NAME}_virtual_scene_provider src/virtual_scene_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS}) add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS}) add_executable(${PROJECT_NAME}_object_locator src/object_locator.cpp ${PROTO_SRCS} ${PROTO_HDRS}) @@ -150,6 +151,7 @@ set_target_properties(${PROJECT_NAME}_dummy_selection_provider PROPERTIES OUTPUT set_target_properties(${PROJECT_NAME}_dummy_pickplace_provider PROPERTIES OUTPUT_NAME dummy_pickplace_provider PREFIX "") set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "") set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME main_controller PREFIX "") +set_target_properties(${PROJECT_NAME}_virtual_scene_provider PROPERTIES OUTPUT_NAME virtual_scene_provider PREFIX "") set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "") set_target_properties(${PROJECT_NAME}_object_locator PROPERTIES OUTPUT_NAME object_locator PREFIX "") @@ -159,6 +161,7 @@ add_dependencies(${PROJECT_NAME}_dummy_selection_provider ${${PROJECT_NAME}_EXPO add_dependencies(${PROJECT_NAME}_dummy_pickplace_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${PROJECT_NAME}_virtual_scene_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_object_locator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -169,6 +172,9 @@ target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller target_link_libraries(${PROJECT_NAME}_main_controller ${catkin_LIBRARIES} ) +target_link_libraries(${PROJECT_NAME}_virtual_scene_provider + ${catkin_LIBRARIES} + ) target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller ${catkin_LIBRARIES} ) diff --git a/launch/distributed-sorting/virtual_scene_provider.launch b/launch/distributed-sorting/virtual_scene_provider.launch new file mode 100644 index 0000000..3bcf9ee --- /dev/null +++ b/launch/distributed-sorting/virtual_scene_provider.launch @@ -0,0 +1,13 @@ +<launch> + + <arg name="connection_address" default="tcp://*:6576" doc="connection address for NNG scene-based selection"/> + <arg name="mqtt_server" default="tcp://127.0.0.1:1883" doc="MQTT server for communication with client cells"/> + + <node pkg="ccf_immersive_sorting" type="virtual_scene_provider" name="main_controller" output="screen"> + <param name="connection_address" type="string" value="$(arg connection_address)"/> + <param name="mqtt_server" type="yaml" value="$(arg mqtt_server)"/> + <param name="arm" type="string" value="virtual-arm"/> + <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_virtual-tag-table-grey.json"/> + </node> + +</launch> diff --git a/src/virtual_scene_provider.cpp b/src/virtual_scene_provider.cpp new file mode 100644 index 0000000..b32fb4e --- /dev/null +++ b/src/virtual_scene_provider.cpp @@ -0,0 +1,145 @@ +#include <ros/ros.h> +#include <ros/package.h> +#include <std_msgs/Empty.h> + +#include <utility> +#include <cell_controller.h> + +#include "connector.pb.h" + +#include "ccf/controller/DummyRobotArmController.h" +#include "ccf/connection/NngConnection.h" +#include "ccf/connection/MqttConnection.h" +#include "ccf/util/NodeUtil.h" + +const char *DEFAULT_NODE_NAME = "virtual_scene_provider"; +std::string NODE_NAME; + +using CetiRosToolbox::getParameter; +using CetiRosToolbox::getPrivateParameter; + +const float UNSELECT = 0.8; +const float SELECT = 1/UNSELECT; +const float DELETING = 0.08; + +void highlight(Object *selected_bin, float factor) +{ + if (selected_bin) + { + 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) +{ + GOOGLE_PROTOBUF_VERIFY_VERSION; + + ros::init(argc, argv, DEFAULT_NODE_NAME); + NODE_NAME = ros::this_node::getName(); + + ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is + + auto connection_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576"); + + 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; + + // add an NNG connection + 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")); + controller.addConnection(std::move(connection)); + + auto mqtt_server = getPrivateParameter<std::string>("mqtt_server", "tcp://127.0.0.1:1883"); + std::shared_ptr<MqttConnection> client_connection = std::make_shared<MqttConnection>(mqtt_server, NODE_NAME); + controller.addConnection(client_connection); + client_connection->listen(getParameter<std::string>(n, "topics/selection", "selection")); + + auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-tag-table-grey.json"; + controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path)); + + Object *robot = controller.resolveObject(controller.getRobotName()); + Object *selected_box = nullptr; + Object *selected_bin = nullptr; + + 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 selection_message_callback = + [&controller, &selected_box, &selected_bin, client_connection](const Selection &selection) + { + + client_connection->send("vr_selection", selection.SerializeAsString()); + + 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()); + highlight(selected_bin, UNSELECT); + highlight(selected_box, UNSELECT); + controller.sendScene(); + + selected_box = nullptr; + selected_bin = nullptr; + } + }; + controller.reactToSelectionMessage(selection_message_callback); + + ros::spin(); + + return 0; +} \ No newline at end of file -- GitLab