PerformAction Event Description (#49)

* Cherry pick diff for PerformAction from new_descriptions

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add missing utils

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add category to PerformAction make

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Fix bug in estimate_remaining_time()

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Update utils

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Update copyright

Signed-off-by: Yadunund <yadunund@openrobotics.org>

rmf_task_sequence/CMakeLists.txt

@@ -26,7 +26,7 @@ find_package(nlohmann_json REQUIRED)
 find_package(nlohmann_json_schema_validator_vendor REQUIRED)
 
 find_package(ament_cmake_catch2 QUIET)
-find_package(rmf_cmake_uncrustify QUIET)
+find_package(ament_cmake_uncrustify QUIET)
 
 # ===== RMF Task Sequence Library
 file(GLOB_RECURSE lib_srcs
@@ -51,10 +51,8 @@ target_include_directories(rmf_task_sequence
     $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}/rmf_api_generate_schema_headers/include> # for auto-generated schema headers
 )
 
-if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND)
-
-  file(GLOB_RECURSE test_srcs "test/*.cpp")
-
+if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND ament_cmake_uncrustify_FOUND)
+  file(GLOB_RECURSE unit_test_srcs "test/*.cpp")
   ament_add_catch2(
     test_rmf_task_sequence test/main.cpp ${test_srcs}
     TIMEOUT 300)
@@ -71,7 +69,7 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND)
     NAMES "rmf_code_style.cfg"
     PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/")
 
-  rmf_uncrustify(
+  ament_uncrustify(
     ARGN include src test
     CONFIG_FILE ${uncrustify_config_file}
     MAX_LINE_LENGTH 80
@@ -136,4 +134,3 @@ export(
   NAMESPACE rmf_task_sequence::
 )
 
-export(PACKAGE rmf_task_sequence)

rmf_task_sequence/include/rmf_task_sequence/events/PerformAction.hpp

@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2022 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 RMF_TASK_SEQUENCE__EVENTS__PERFORMACTION_HPP
+#define RMF_TASK_SEQUENCE__EVENTS__PERFORMACTION_HPP
+
+#include <rmf_traffic/agv/Planner.hpp>
+
+#include <rmf_task_sequence/Event.hpp>
+
+#include <nlohmann/json.hpp>
+
+namespace rmf_task_sequence {
+namespace events {
+
+//==============================================================================
+class PerformAction
+{
+public:
+  using Location = rmf_traffic::agv::Plan::Goal;
+
+  class Description;
+  using DescriptionPtr = std::shared_ptr<Description>;
+  using ConstDescriptionPtr = std::shared_ptr<const Description>;
+
+  class Model;
+};
+
+//==============================================================================
+class PerformAction::Description : public Event::Description
+{
+public:
+
+  /// Make a PerformAction description.
+  ///
+  /// \param[in] category
+  ///   A category for this action
+  ///
+  /// \param[in] description
+  ///   A json description of the action to perform
+  ///
+  /// \param[in] action_duration_estimate
+  ///   An estimate for how long it will take for the action to complete
+  ///
+  /// \param[in] use_tool_sink
+  ///   If true, battery drain from peripheral tools will be accounted for
+  ///   while performing the action
+  ///
+  /// \param[in] expected_finish_location
+  ///   An optional location to indicate where the robot will end up after
+  ///   performing the action. Use nullopt to indicate that after performing
+  ///   the action, the robot will be at its initial location.
+  static DescriptionPtr make(
+    const std::string& category,
+    nlohmann::json description,
+    rmf_traffic::Duration action_duration_estimate,
+    bool use_tool_sink,
+    std::optional<Location> expected_finish_location = std::nullopt);
+
+  /// Get the category
+  const std::string& category() const;
+
+  /// Set the category
+  Description& category(const std::string& new_category);
+
+  /// Get the description
+  const nlohmann::json& description() const;
+
+  /// Set the description
+  Description& description(const nlohmann::json& new_description);
+
+  /// Get the action duration estimate
+  const rmf_traffic::Duration& action_duration_estimate() const;
+
+  /// Set the action duration estimate
+  Description& action_duration_estimate(rmf_traffic::Duration new_duration);
+
+  /// Check whether to account for battery drain from tools
+  bool use_tool_sink() const;
+
+  /// Set whether to account for battery drain from tools
+  Description& use_tool_sink(bool use_tool);
+
+  /// Get the expected finish location
+  std::optional<Location> expected_finish_location() const;
+
+  /// Set the expected finish location
+  Description& expected_finish_location(std::optional<Location> new_location);
+
+  // Documentation inherited
+  Activity::ConstModelPtr make_model(
+    State invariant_initial_state,
+    const Parameters& parameters) const final;
+
+  // Documentation inherited
+  Header generate_header(
+    const rmf_task::State& initial_state,
+    const Parameters& parameters) const final;
+
+  class Implementation;
+private:
+  Description();
+  rmf_utils::impl_ptr<Implementation> _pimpl;
+};
+
+} // namespace events
+} // namespace rmf_task_sequence
+
+#endif // RMF_TASK_SEQUENCE__EVENTS__PERFORMACTION_HPP

rmf_task_sequence/include/rmf_task_sequence/events/WaitFor.hpp

@@ -29,7 +29,7 @@ namespace events {
 /// A WaitFor event encompasses having the robot sit in place and wait for a
 /// period of time to pass.
 ///
-/// The Model of this phase may be useful for calculating the Models of other
+/// The Model of this event may be useful for calculating the Models of other
 /// phases that include a period of time where the robot is waiting for a
 /// process to finish. E.g. the PickUp and DropOff Models use WaitFor::Model to
 /// calculate how much the robot's battery drains while waiting for the payload

rmf_task_sequence/package.xml

@@ -15,7 +15,7 @@
   <depend>nlohmann_json_schema_validator_vendor</depend>
 
   <test_depend>ament_cmake_catch2</test_depend>
-  <test_depend>rmf_cmake_uncrustify</test_depend>
+  <test_depend>ament_cmake_uncrustify</test_depend>
 
   <export>
     <build_type>cmake</build_type>

rmf_task_sequence/src/rmf_task_sequence/Task.cpp

@@ -464,7 +464,9 @@ const Task::ConstTagPtr& Task::Active::tag() const
 //==============================================================================
 rmf_traffic::Duration Task::Active::estimate_remaining_time() const
 {
-  auto remaining_time = _active_phase->estimate_remaining_time();
+  auto remaining_time =
+    _active_phase ? _active_phase->estimate_remaining_time() :
+    rmf_traffic::Duration(0);
   for (const auto& p : _pending_phases)
     remaining_time += p.tag()->header().original_duration_estimate();
 

rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp

@@ -17,6 +17,8 @@
 
 #include <rmf_task_sequence/events/GoToPlace.hpp>
 
+#include "utils.hpp"
+
 namespace rmf_task_sequence {
 namespace events {
 
@@ -209,22 +211,18 @@ Header GoToPlace::Description::generate_header(
   const State& initial_state,
   const Parameters& parameters) const
 {
-  const auto fail = [](const std::string& msg)
-    {
-      throw std::runtime_error(
-              "[GoToPlace::Description::generate_header] " + msg);
-    };
+  const std::string& fail_header = "[GoToPlace::Description::generate_header]";
 
   const auto start_wp_opt = initial_state.waypoint();
   if (!start_wp_opt)
-    fail("Initial state is missing a waypoint");
+    utils::fail(fail_header, "Initial state is missing a waypoint");
 
   const auto start_wp = *start_wp_opt;
 
   const auto& graph = parameters.planner()->get_configuration().graph();
   if (graph.num_waypoints() <= start_wp)
   {
-    fail("Initial waypoint [" + std::to_string(start_wp)
+    utils::fail(fail_header, "Initial waypoint [" + std::to_string(start_wp)
       + "] is outside the graph [" + std::to_string(graph.num_waypoints())
       + "]");
   }
@@ -233,7 +231,7 @@ Header GoToPlace::Description::generate_header(
 
   if (graph.num_waypoints() <= _pimpl->destination.waypoint())
   {
-    fail("Destination waypoint ["
+    utils::fail(fail_header, "Destination waypoint ["
       + std::to_string(_pimpl->destination.waypoint())
       + "] is outside the graph [" + std::to_string(graph.num_waypoints())
       + "]");
@@ -246,7 +244,8 @@ Header GoToPlace::Description::generate_header(
 
   if (!estimate.has_value())
   {
-    fail("Cannot find a path from " + start_name + " to " + goal_name_);
+    utils::fail(fail_header, "Cannot find a path from ["
+      + start_name + "] to [" + goal_name_ + "]");
   }
 
   return Header(