diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b95f9f2690d6633666354ecb25224db3940cff3..71db6cf8e4d639fa081e9499a09af86cd6977592 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,6 +136,7 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_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}_dummy_main_controller src/dummy_main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} ) add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS}) @@ -145,18 +146,23 @@ add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_cont ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" set_target_properties(${PROJECT_NAME}_dummy_selection_provider PROPERTIES OUTPUT_NAME dummy_selection_provider PREFIX "") set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "") +set_target_properties(${PROJECT_NAME}_dummy_main_controller PROPERTIES OUTPUT_NAME dummy_main_controller PREFIX "") set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "") ## Add cmake target dependencies of the executable ## same as for the library above add_dependencies(${PROJECT_NAME}_dummy_selection_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}_dummy_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller ${catkin_LIBRARIES} ) +target_link_libraries(${PROJECT_NAME}_dummy_main_controller + ${catkin_LIBRARIES} + ) target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller ${catkin_LIBRARIES} ) diff --git a/launch/virtual-table_dummy_main.launch b/launch/virtual-table_dummy_main.launch new file mode 100644 index 0000000000000000000000000000000000000000..c8ab675828c25f539c2871182b620b0cc45c23c9 --- /dev/null +++ b/launch/virtual-table_dummy_main.launch @@ -0,0 +1,13 @@ +<launch> + + <arg name="connection_address" default="tcp://*:6576" /> + <arg name="client_controllers" default="[]" /> + + + <node pkg="ccf_immersive_sorting" type="dummy_main_controller" name="dummy_main_controller_instance" output="screen" > + <param name="connection_address" type="string" value="$(arg connection_address)" /> + <param name="client_controllers" type="yaml" value="$(arg client_controllers)" /> + <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_virtual-table.json" /> + </node> + +</launch> diff --git a/src/dummy_main_controller.cpp b/src/dummy_main_controller.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ecfea69d0610fa7698c6c5ba15709f7fbfe101f3 --- /dev/null +++ b/src/dummy_main_controller.cpp @@ -0,0 +1,157 @@ +/*! \file dummy_sorting_controller.cpp + + A ROS node that simulates a node connected to a model-based cobotic application + + \author Johannes Mey + \author Sebastian Ebert + \date 07.07.20 +*/ + +#include <ros/ros.h> +#include <ros/package.h> +#include <std_msgs/Empty.h> + +#include "connector.pb.h" + +#include "ccf/controller/DummyRobotArmController.h" +#include "ccf/connection/NngConnection.h" +#include "ccf/util/NodeUtil.h" + +std::string NODE_NAME = "dummy_sorting_controller"; + +using CetiRosToolbox::getParameter; +using CetiRosToolbox::getPrivateParameter; + +const float SELECT = 2.0; +const float UNSELECT = 0.5; +const float DELETING = 0.08; + +void highlight(Object *selectedBin, float factor) { + if (selectedBin) { + 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); + } +} + +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 clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers"); + + ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers."); + for (const auto &client: clientControllers) { + ROS_INFO_STREAM("Connecting to client at " << client << "."); + std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false); + client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection")); + client_connection->setReceiveTopic("client_scene"); + connector.addConnection(std::move(client_connection)); + } + + connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + + "/config/config_scene.yaml")); + + 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 = [¤tlyPickedBox, &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) { + highlight(selectedBox, UNSELECT); + if (selectedBox == object) { + selectedBox = nullptr; + } else { + selectedBox = object; + highlight(selectedBox, SELECT); + } + connector.sendScene(); + ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'"); + } else if (object->type() == Object::BIN) { + highlight(selectedBin, UNSELECT); + if (selectedBin == object) { + 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 && selectedBox) { + auto boxId = selectedBox->id(); + ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id()); + currentlyPickedBox = boxId; + highlight(selectedBin, UNSELECT); + highlight(selectedBox, DELETING); + connector.sendScene(); + if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { + ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!"); + } else { + ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more."); + } + selectedBox = nullptr; + selectedBin = nullptr; + connector.sendScene(); + } else { + + } + }; + connector.reactToSelectionMessage(selectionMessageCallback); + + auto sceneUpdateCallback = [¤tlyPickedBox, &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; +} \ No newline at end of file