diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index cbb8710a9..8f258d74d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -19,10 +19,53 @@ #include "EndLiftSession.hpp" #include "RxOperators.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include "rclcpp/rclcpp.hpp" + namespace rmf_fleet_adapter { namespace phases { //============================================================================== + +std::string msg_rec; // global variable, not ideal + +using std::placeholders::_1; + +bool su_status; + +class MinimalSubscriber : public rclcpp::Node +{ +public: + MinimalSubscriber() + : Node("su_subscriber") + { + //auto sub1_opt = rclcpp::SubscriptionOptions(); + subscription_ = this->create_subscription( + "su_topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "inside subscriber"); + } + +private: + void topic_callback(const std_msgs::msg::String::SharedPtr msg) + { + msg_rec = msg->data.c_str(); //save the data in the callback + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "holding lift"); + if (msg_rec == "go"){ + su_status = true; + //subscription_.reset(); + rclcpp::shutdown(); + } + else + { + su_status = false; + } + + } + rclcpp::Subscription::SharedPtr subscription_; +}; + + std::shared_ptr RequestLift::ActivePhase::make( agv::RobotContextPtr context, std::string lift_name, @@ -95,6 +138,7 @@ RequestLift::ActivePhase::ActivePhase( //============================================================================== void RequestLift::ActivePhase::_init_obs() { + std::make_shared(); using rmf_lift_msgs::msg::LiftState; _obs = _context->node()->lift_state() @@ -136,7 +180,7 @@ void RequestLift::ActivePhase::_init_obs() auto me = weak.lock(); if (!me) return Task::StatusMsg(); - + return me->_get_status(v); }) .lift(grab_while([weak = weak_from_this()](const Task::StatusMsg& status) @@ -188,26 +232,21 @@ Task::StatusMsg RequestLift::ActivePhase::_get_status( using rmf_lift_msgs::msg::LiftRequest; Task::StatusMsg status{}; status.state = Task::StatusMsg::STATE_ACTIVE; - if (lift_state->lift_name == _lift_name && - lift_state->current_floor == _destination && + if (lift_state->current_floor == _destination && lift_state->door_state == LiftState::DOOR_OPEN && lift_state->session_id == _context->requester_id()) { - status.state = Task::StatusMsg::STATE_COMPLETED; - status.status = "success"; - _timer.reset(); - } - else if (lift_state->lift_name == _lift_name) - { - // TODO(MXG): Make this a more human-friendly message - status.status = "[" + _context->name() + "] still waiting for lift [" - + _lift_name + "] current state: " - + lift_state->current_floor + " vs " + _destination + " | " - + std::to_string(static_cast(lift_state->door_state)) - + " vs " + std::to_string(static_cast(LiftState::DOOR_OPEN)) - + " | " + lift_state->session_id + " vs " + _context->requester_id(); + // listen for SU msg before publishing the request end + rclcpp::spin(std::make_shared()); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "moving past sub"); + if (su_status == true){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"heard SU msg"); + status.state = Task::StatusMsg::STATE_COMPLETED; + status.status = "success"; + _timer.reset(); + //rclcpp::shutdown(); + } } - return status; }