diff --git a/launch/distributed-sorting/virtual_scene_provider.launch b/launch/distributed-sorting/virtual_scene_provider.launch index 3bcf9ee445536ddca15b8052f77c7e532ae0fa8f..dd2d764d8e2357f0a66dac323ce6dd6c9bdb65bd 100644 --- a/launch/distributed-sorting/virtual_scene_provider.launch +++ b/launch/distributed-sorting/virtual_scene_provider.launch @@ -3,7 +3,7 @@ <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"> + <node pkg="ccf_immersive_sorting" type="virtual_scene_provider" name="main_controller" output="screen" respawn="true" respawn_delay="3"> <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"/> diff --git a/src/virtual_scene_provider.cpp b/src/virtual_scene_provider.cpp index 310c5c09018d61f1b52b2002f1fc761359b5501a..1942b6abb5840abd150123d008895825dae1f162 100644 --- a/src/virtual_scene_provider.cpp +++ b/src/virtual_scene_provider.cpp @@ -12,17 +12,17 @@ #include "ccf/connection/MqttConnection.h" #include "ccf/util/NodeUtil.h" -const char *DEFAULT_NODE_NAME = "virtual_scene_provider"; +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 SELECT = 1 / UNSELECT; const float DELETING = 0.08; -void highlight(Object *selected_bin, float factor) +void highlight(Object* selected_bin, float factor) { if (selected_bin) { @@ -32,7 +32,7 @@ void highlight(Object *selected_bin, float factor) } } -int main(int argc, char **argv) +int main(int argc, char** argv) { GOOGLE_PROTOBUF_VERIFY_VERSION; @@ -46,7 +46,7 @@ int main(int argc, char **argv) auto robotName = getPrivateParameter<std::string>("arm", "virtual-arm"); ROS_INFO_STREAM("This cell controls arm " << robotName); - DummyRobotArmController controller{n, NODE_NAME, robotName}; + DummyRobotArmController controller{ n, NODE_NAME, robotName }; std::map<std::string, CellController> clients; std::vector<std::pair<std::string, std::string>> actions; @@ -62,81 +62,77 @@ int main(int argc, char **argv) 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"; + 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; + 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) - { + "send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr& msg) { controller.sendScene(); }); + ros::Timer timer = n.createTimer(ros::Duration(3), [&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) + }); // send a scene every three 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) { - - 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; - } - }; + 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();