added new wp to the test

Author: juandpenan

bt_nodes/configuration/include/configuration/init_receptionist.hpp

@@ -48,6 +48,7 @@ class InitReceptionist : public BT::ActionNodeBase
         BT::OutputPort<std::string>("manipulation_frame", "base_frame for manipulation"),
         BT::OutputPort<std::string>("party_wp", "frame to the party wp"),
         BT::OutputPort<std::string>("entrance_wp", "frame to the the entrance wp"),
+        BT::OutputPort<std::string>("follow_ready_wp", "frame to the follow ready wp"),
         BT::OutputPort<std::string>("host_name", "name of the host"),
         BT::OutputPort<std::string>("host_drink", "host's favorite drink"),
         BT::OutputPort<std::string>("current_guest", "current guest_number STARTING FROM 1"),

bt_nodes/configuration/src/configuration/init_receptionist.cpp

@@ -82,6 +82,10 @@ BT::NodeStatus InitReceptionist::tick()
           node_->get_logger(), "Entrance waypoint set to frame: [%s]", wp.c_str());
       } else if (wp.find("party")!=std::string::npos) {
         setOutput("party_wp", wp);
+      } else if (wp.find("follow_ready")!=std::string::npos) {
+        setOutput("follow_ready_wp", wp);
+        RCLCPP_INFO(
+          node_->get_logger(), "Follow ready waypoint set to frame: [%s]", wp.c_str());
       }
 
       q.setRPY(0, 0, wp_pos[2]);