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