diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 59e9625ef..1f88de5f7 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -29,6 +29,7 @@ endif() set(dep_pkgs rclcpp rmf_utils + rmf_charger_msgs rmf_dispenser_msgs rmf_ingestor_msgs rmf_door_msgs @@ -72,6 +73,7 @@ target_link_libraries(rmf_fleet_adapter ${rmf_lift_msgs_LIBRARIES} ${rmf_dispenser_msgs_LIBRARIES} ${rmf_ingestor_msgs_LIBRARIES} + ${rmf_charger_msgs_LIBRARIES} ) target_include_directories(rmf_fleet_adapter @@ -88,6 +90,7 @@ target_include_directories(rmf_fleet_adapter ${rmf_lift_msgs_INCLUDE_DIRS} ${rmf_dispenser_msgs_INCLUDE_DIRS} ${rmf_ingestor_msgs_INCLUDE_DIRS} + ${rmf_charger_msgs_INCLUDE_DIRS} ) if (BUILD_TESTING) @@ -102,6 +105,7 @@ if (BUILD_TESTING) test/phases/DoorOpenTest.cpp test/phases/DoorCloseTest.cpp test/phases/RequestLiftTest.cpp + test/phases/ReadyToChargeTest.cpp test/phases/DispenseItemTest.cpp test/phases/IngestItemTest.cpp test/phases/test_GoToPlace.cpp @@ -122,6 +126,7 @@ if (BUILD_TESTING) ${rmf_dispenser_msgs_INCLUDE_DIRS} ${rmf_ingestor_msgs_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} + ${rmf_charger_msgs_INCLUDE_DIRS} ) target_link_libraries(rmf_fleet_adapter_test PRIVATE @@ -135,6 +140,7 @@ if (BUILD_TESTING) rmf_fleet_adapter rmf_utils::rmf_utils ${std_msgs_LIBRARIES} + ${rmf_charger_msgs_LIBRARIES} ) target_compile_definitions(rmf_fleet_adapter_test diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index e0576111c..6c9c4c15d 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -56,6 +56,9 @@ const std::string DispatchRequestTopicName = "rmf_task/dispatch_request"; const std::string DockSummaryTopicName = "dock_summary"; +const std::string ChargerCancelTopicName = "rmf_charger/cancel"; +const std::string ChargerRequestTopicName = "rmf_charger/requests"; +const std::string ChargerStateTopicName = "rmf_charger/state"; } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index fad1cd873..cfca49555 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -23,6 +23,7 @@ rmf_traffic_msgs rmf_battery rmf_task + rmf_charger_msgs eigen yaml-cpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 9e35fa9d0..e912c8ecc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -62,6 +62,10 @@ std::shared_ptr Node::make( IngestorStateTopicName, default_qos); node->_fleet_state_pub = node->create_publisher( FleetStateTopicName, default_qos); + node->_charger_state_obs = node->create_observable( + ChargerStateTopicName, default_qos); + node->_charger_request_pub = node->create_publisher( + ChargerRequestTopicName, default_qos); return node; } @@ -159,5 +163,17 @@ auto Node::fleet_state() const -> const FleetStatePub& return _fleet_state_pub; } +//============================================================================== +auto Node::charger_state() const -> const ChargerStateObs& +{ + return _charger_state_obs; +} + +//============================================================================== +auto Node::charger_request() const -> const ChargerRequestPub& +{ + return _charger_request_pub; +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 39e1811cc..7d926aa78 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -20,6 +20,9 @@ #include +#include +#include +#include #include #include #include @@ -101,6 +104,14 @@ class Node : public rmf_rxcpp::Transport using IngestorStateObs = rxcpp::observable; const IngestorStateObs& ingestor_state() const; + using ChargerState = rmf_charger_msgs::msg::ChargerState; + using ChargerStateObs = rxcpp::observable; + const ChargerStateObs& charger_state() const; + + using ChargerRequest = rmf_charger_msgs::msg::ChargerRequest; + using ChargerRequestPub = rclcpp::Publisher::SharedPtr; + const ChargerRequestPub& charger_request() const; + using FleetState = rmf_fleet_msgs::msg::FleetState; using FleetStatePub = rclcpp::Publisher::SharedPtr; const FleetStatePub& fleet_state() const; @@ -126,6 +137,8 @@ class Node : public rmf_rxcpp::Transport IngestorResultObs _ingestor_result_obs; IngestorStateObs _ingestor_state_obs; FleetStatePub _fleet_state_pub; + ChargerStateObs _charger_state_obs; + ChargerRequestPub _charger_request_pub; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp new file mode 100644 index 000000000..967cd0d4f --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.cpp @@ -0,0 +1,175 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "ReadyToCharge.hpp" + +namespace rmf_fleet_adapter { +namespace phases { + +//============================================================================== +auto ReadyToCharge::Active::observe() const -> const rxcpp::observable& +{ + return _status_obs; +} + +//============================================================================== +rmf_traffic::Duration ReadyToCharge::Active::estimate_remaining_time() const +{ + // TODO + return rmf_traffic::time::from_seconds(5); +} + +//============================================================================== +void ReadyToCharge::Active::emergency_alarm(const bool value) +{ + // Assume charging station is a holding point +} + +//============================================================================== +void ReadyToCharge::Active::cancel() +{ + // TODO +} + +//============================================================================== +const std::string& ReadyToCharge::Active::description() const +{ + return _description; +} + +//============================================================================== +ReadyToCharge::Active::Active( + agv::RobotContextPtr context, + const std::string request_id, + const std::string location) +: _context(std::move(context)), _id(request_id) +{ + using rmf_charger_msgs::msg::ChargerState; + using rmf_charger_msgs::msg::ChargerRequest; + _description = "Charger Negotiation"; + + //Place holder value for testing + std::string desired_charger_name = location; + + //INIT + _current_state = State::AWAITING_RESPONSE; + + _timer = _context->node()->create_wall_timer( + std::chrono::milliseconds(1000), + [me = this, desired_charger_name]() + { + /*auto me = weak.lock(); + if (!me) + { + std::cout << "no lock" <_context->description().owner(); + request.robot_name = me->_context->description().name(); + request.start_timeout = rclcpp::Duration {10,0}; + request.request_id = me->_id; + me->_context->node()->charger_request()->publish(request); + } + ); + + _status_obs = _context->node()->charger_state() + .filter([desired_charger_name](const auto& status_msg) + { + return status_msg->charger_name == desired_charger_name; + }) + .map([me =this](const auto& status_msg) + { + /*auto me = weak.lock(); + if (!me) + return Task::StatusMsg();*/ + std::cout << "status received" <_current_state == State::AWAITING_RESPONSE) + { + if((status_msg->state == ChargerState::CHARGER_ASSIGNED + || status_msg->state == ChargerState::CHARGER_CHARGING) + && status_msg->robot_name == me->_context->description().name() + && status_msg->robot_fleet == me->_context->description().owner() + && status_msg->request_id == me->_id) + { + me->_timer->cancel(); + Task::StatusMsg status; + status.state = Task::StatusMsg::STATE_COMPLETED; + status.status = "Negotiation Success"; + return status; + } + else if(status_msg->state == ChargerState::CHARGER_ERROR + && status_msg->request_id == me->_id) + { + me->_timer->cancel(); + Task::StatusMsg status; + status.state = Task::StatusMsg::STATE_FAILED; + status.status = status_msg->error_message; + return status; + } + } + Task::StatusMsg status; + status.state = Task::StatusMsg::STATE_ACTIVE; + status.status = "Negotiation In progress"; + return status; + }); +} + +//============================================================================== +std::shared_ptr ReadyToCharge::Pending::begin() +{ + auto active = + std::shared_ptr(new Active(_context, _id, _location)); + return active; +} + +//============================================================================== +rmf_traffic::Duration ReadyToCharge::Pending::estimate_phase_duration() const +{ + return rmf_traffic::Duration{0}; +} + +//============================================================================== +const std::string& ReadyToCharge::Pending::description() const +{ + return _description; +} + +//============================================================================== +ReadyToCharge::Pending::Pending( + agv::RobotContextPtr context, + const std::string id, + const std::string location) +: _context(std::move(context)), _id(id), _location(location) +{ + _description = + "Performing Charger Negotiation"; +} + +//============================================================================== +auto ReadyToCharge::make( + agv::RobotContextPtr context, + const std::string id, + const std::string location) -> std::unique_ptr +{ + return std::unique_ptr( + new Pending(context, id, location)); +} + +} +} diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp new file mode 100644 index 000000000..6a834ac6e --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReadyToCharge.hpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__PHASES__READYTOCHARGE_HPP +#define SRC__RMF_FLEET_ADAPTER__PHASES__READYTOCHARGE_HPP + + +#include "../Task.hpp" +#include "../agv/RobotContext.hpp" + +namespace rmf_fleet_adapter { +namespace phases { + +//============================================================================== +class ReadyToCharge +{ +public: + using StatusMsg = Task::StatusMsg; + class Pending; + + class Active + : public Task::ActivePhase, + public std::enable_shared_from_this + { + public: + Active(agv::RobotContextPtr context, + std::string request_id, + std::string location); + + // Documentation inherited from ActivePhase + const rxcpp::observable& observe() const final; + + // Documentation inherited from ActivePhase + rmf_traffic::Duration estimate_remaining_time() const final; + + // Documentation inherited from ActivePhase + void emergency_alarm(bool on) final; + + // Documentation inherited from ActivePhase + void cancel() final; + + // Documentation inherited from ActivePhase + const std::string & description() const final; + + private: + enum State + { + AWAITING_RESPONSE = 0, + REQUEST_SUCCESS + }; + + friend class Pending; + rxcpp::observable _status_obs; + rxcpp::subjects::subject _status_publisher; + agv::RobotContextPtr _context; + State _current_state; + rclcpp::TimerBase::SharedPtr _timer; + std::string _id; + std::string _description; + }; + + class Pending : public Task::PendingPhase + { + public: + // Documentation inherited + std::shared_ptr begin() final; + + // Documentation inherited + rmf_traffic::Duration estimate_phase_duration() const final; + + // Documentation inherited + const std::string& description() const final; + + private: + friend class ReadyToCharge; + Pending( + agv::RobotContextPtr context, + std::string id, + std::string location); + + agv::RobotContextPtr _context; + std::string _description; + std::string _id; + std::string _location; + }; + + static std::unique_ptr make( + agv::RobotContextPtr context, + const std::string id, + const std::string location); + }; +} +} +#endif \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index 9ef580863..441de4861 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -16,7 +16,7 @@ */ #include "../phases/GoToPlace.hpp" - +#include "../phases/ReadyToCharge.hpp" #include "../phases/WaitForCharge.hpp" #include "ChargeBattery.hpp" @@ -33,10 +33,15 @@ std::shared_ptr make_charge_battery( const rmf_task::agv::State finish_state) { rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint()}; + const auto location_waypoint = + context->navigation_graph().get_waypoint(finish_state.charging_waypoint()); + auto location = location_waypoint.name(); Task::PendingPhases phases; phases.push_back( phases::GoToPlace::make(context, std::move(start), goal)); + phases.push_back( + phases::ReadyToCharge::make(context, request->id(), *location)); phases.push_back( phases::WaitForCharge::make(context, request->battery_system(), 0.99)); diff --git a/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp b/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp new file mode 100644 index 000000000..facfff70f --- /dev/null +++ b/rmf_fleet_adapter/test/phases/ReadyToChargeTest.cpp @@ -0,0 +1,116 @@ + +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "MockAdapterFixture.hpp" + +#include +#include +#include + +namespace rmf_fleet_adapter { +namespace phases { +namespace test { + +using rmf_charger_msgs::msg::ChargerState; +using rmf_charger_msgs::msg::ChargerRequest; + +SCENARIO_METHOD(MockAdapterFixture, "Charger Request Phase", "[phases]") +{ + std::mutex m; + std::condition_variable received_requests_cv; + std::list received_requests; + + + const auto info = add_robot(); + const auto& context = info.context; + + auto charger_state_pub = adapter->node()->create_publisher( + ChargerStateTopicName, 10); + + auto request_guid = "test/charger"; + auto pending_phase = ReadyToCharge::make( + context, + request_guid, + "charger1" + ); + +#if 0 + WHEN("it is started") + { + auto rcl_subscription = adapter->node()->create_subscription( + ChargerRequestTopicName, + 10, + [&](ChargerRequest::UniquePtr ingestor_request) + { + std::unique_lock lk(m); + received_requests.emplace_back(*ingestor_request); + received_requests_cv.notify_all(); + }); + + THEN("it should continuously request a response") + { + std::unique_lock lk(m); + received_requests_cv.wait(lk, [&]() { return received_requests.size() >= 3; }); + } + + + //sub.unsubscribe(); + } +#endif + WHEN("a response is sent") + { + auto rcl_subscription = adapter->node()->create_subscription( + ChargerRequestTopicName, + 10, + [&](ChargerRequest::UniquePtr request) + { + std::cout << "received request" <robot_name; + state.robot_fleet = request->fleet_name; + state.request_id = request->request_id; + state.charger_name = request->charger_name; + charger_state_pub->publish(state); + }); + + auto active_phase = pending_phase->begin(); + std::condition_variable status_updates_cv; + std::list status_updates; + auto sub = active_phase->observe().subscribe( + [&](const auto& status) + { + std::cout << "status update" < lk(m); + status_updates.emplace_back(status); + status_updates_cv.notify_all(); + }); + + THEN("it should send a OK ") + { + std::unique_lock lk(m); + status_updates_cv.wait(lk, [&]() { return !status_updates.empty(); }); + REQUIRE(status_updates.begin()->state == Task::StatusMsg::STATE_COMPLETED); + } + //sub.unsubscribe(); + } +} + +} // namespace test +} // namespace phases +} // namespace rmf_fleet_adapter