Continue implementation of the architectural shell for the command task hierarchy

src/control/CMakeLists.txt

@@ -13,7 +13,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
-find_package(task REQUIRED)
+find_package(nav2_msgs REQUIRED)
 
 include_directories(
   include
@@ -24,20 +24,18 @@ include_directories(
 link_directories(${CMAKE_BINARY_DIR}/../libs)
 
 add_executable(dwa_controller
-  src/ControlTask.cpp
   src/DwaController.cpp
   src/main.cpp
   )
 
 target_link_libraries(dwa_controller
   robot
-  task
   )
 
 ament_target_dependencies(dwa_controller 
   rclcpp 
   std_msgs
-  task
+  nav2_msgs
   robot
   )
 

src/control/include/control/DwaController.hpp

@@ -1,22 +1,31 @@
-// License: Apache 2.0. See LICENSE file in root directory.
-// Copyright 2018 Intel Corporation. All Rights Reserved.
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 CONTROL__DWACONTROLLER_HPP_
 #define CONTROL__DWACONTROLLER_HPP_
 
-#include "control/ControlTask.hpp"
+#include <string>
+#include "control/FollowPathTaskServer.hpp"
 
-class DwaController : public ControlTask
+class DwaController : public FollowPathTaskServer
 {
 public:
-  DwaController(const std::string & name, Robot * robot);
+  explicit DwaController(const std::string & name);
   DwaController() = delete;
   ~DwaController();
 
-  void executePlan() override; 
-
-protected:
-  void workerThread() override; 
+  TaskStatus executeAsync(const FollowPathCommand::SharedPtr path) override;
 };
 
 #endif  // CONTROL__DWACONTROLLER_HPP_

src/control/include/control/FollowPathTaskClient.hpp

@@ -0,0 +1,23 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 CONTROL__FOLLOWPATHTASKCLIENT_HPP_
+#define CONTROL__FOLLOWPATHTASKCLIENT_HPP_
+
+#include "task/TaskClient.hpp"
+#include "control/FollowPathTaskMessages.hpp"
+
+using FollowPathTaskClient = TaskClient<FollowPathCommand, FollowPathResult>;
+
+#endif  // CONTROL__FOLLOWPATHTASKCLIENT_HPP_

src/control/include/control/FollowPathTaskMessages.hpp

@@ -0,0 +1,24 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 CONTROL__FOLLOWPATHTASKMESSAGES_HPP_
+#define CONTROL__FOLLOWPATHTASKMESSAGES_HPP_
+
+#include "nav2_msgs/msg/path.hpp"
+#include "std_msgs/msg/empty.hpp"
+
+using FollowPathCommand = nav2_msgs::msg::Path;
+using FollowPathResult = std_msgs::msg::Empty;
+
+#endif  // CONTROL__FOLLOWPATHTASKMESSAGES_HPP_

src/control/include/control/FollowPathTaskServer.hpp

@@ -0,0 +1,23 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 CONTROL__FOLLOWPATHTASKSERVER_HPP_
+#define CONTROL__FOLLOWPATHTASKSERVER_HPP_
+
+#include "task/TaskServer.hpp"
+#include "control/FollowPathTaskMessages.hpp"
+
+using FollowPathTaskServer = TaskServer<FollowPathCommand, FollowPathResult>;
+
+#endif  // CONTROL__FOLLOWPATHTASKSERVER_HPP_

src/control/package.xml

@@ -9,9 +9,12 @@
   <license>Apache License 2.0</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>rclcpp</build_depend>
-  <build_depend>task</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>nav2_msgs</build_depend>
   <build_depend>robot</build_depend>
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>nav2_msgs</exec_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/control/src/DwaController.cpp

@@ -1,11 +1,25 @@
-// License: Apache 2.0. See LICENSE file in root directory.
-// Copyright 2018 Intel Corporation. All Rights Reserved.
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 "control/DwaController.hpp"
+#include <string>
 #include <chrono>
+#include "control/DwaController.hpp"
+
+using namespace std::chrono_literals;
 
-DwaController::DwaController(const std::string & name, Robot * robot)
-: ControlTask(name, robot)
+DwaController::DwaController(const std::string & name)
+: FollowPathTaskServer(name)
 {
   RCLCPP_INFO(get_logger(), "DwaController::DwaController");
 }
@@ -15,21 +29,30 @@ DwaController::~DwaController()
   RCLCPP_INFO(get_logger(), "DwaController::~DwaController");
 }
 
-void
-DwaController::executePlan()
+TaskStatus
+DwaController::executeAsync(const FollowPathCommand::SharedPtr command)
 {
-  RCLCPP_INFO(get_logger(), "DwaController::executePlan");
-}
+  RCLCPP_INFO(get_logger(), "DwaController::executeAsync");
 
-void
-DwaController::workerThread()
-{
-  RCLCPP_INFO(get_logger(), "DwaController::workerThread");
+  // Spin here for a bit to fake out some processing time
+  for (int i = 0; i < 10; i++) {
+    // Do a bit of the task
+    RCLCPP_INFO(get_logger(), "DwaController::executeAsync: doing work: %d", i);
+    std::this_thread::sleep_for(250ms);
 
-  while (!stopWorkerThread_)
-  {
-    std::this_thread::sleep_for(std::chrono::milliseconds(500));
-    RCLCPP_INFO(get_logger(), "DwaController::workerThread: doing work");
+    // Before we loop again to do more work, check if we've been canceled
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "DwaController::executeAsync: task has been canceled");
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
   }
-}
 
+  // We've successfully completed the task, so return the result
+  RCLCPP_INFO(get_logger(), "DwaController::executeAsync: task completed");
+
+  FollowPathResult result;
+  setResult(result);
+
+  return TaskStatus::SUCCEEDED;
+}

src/control/src/main.cpp

@@ -1,19 +1,26 @@
-// License: Apache 2.0. See LICENSE file in root directory.
-// Copyright 2018 Intel Corporation. All Rights Reserved.
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 <string>
 #include <memory>
 #include "rclcpp/rclcpp.hpp"
 #include "control/DwaController.hpp"
-#include "robot/RosRobot.hpp"
 
 int main(int argc, char ** argv)
 {
-  std::string urdf_filename;
-  RosRobot robot(urdf_filename);
-
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<DwaController>("DwaController", &robot));
+  rclcpp::spin(std::make_shared<DwaController>("DwaController"));
   rclcpp::shutdown();
 
   return 0;

src/mission_execution/CMakeLists.txt

@@ -13,10 +13,12 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
 
 include_directories(
   include
-  ../libs/task/include
+  ../task/include
+  ../navigation/include
   )
 
 link_directories(${CMAKE_BINARY_DIR}/../libs)
@@ -26,10 +28,13 @@ add_executable(mission_execution
   src/MissionExecution.cpp
   )
 
+target_link_libraries(mission_execution
+  )
+
 ament_target_dependencies(mission_execution 
   rclcpp 
   std_msgs
-  task
+  nav2_msgs
   )
 
 install(TARGETS ${PROJECT_NAME}

src/mission_execution/include/mission_execution/ExecuteMissionTaskClient.hpp

@@ -0,0 +1,23 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 MISSION_EXECUTION__EXECUTEMISSIONTASKCLIENT_HPP_
+#define MISSION_EXECUTION__EXECUTEMISSIONTASKCLIENT_HPP_
+
+#include "task/TaskClient.hpp"
+#include "mission_execution/ExecuteMissionTaskMessages.hpp"
+
+using ExecuteMissionTaskClient = TaskClient<ExecuteMissionCommand, ExecuteMissionResult>;
+
+#endif  // MISSION_EXECUTION__EXECUTEMISSIONTASKCLIENT_HPP_

src/mission_execution/include/mission_execution/ExecuteMissionTaskMessages.hpp

@@ -0,0 +1,24 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 MISSION_EXECUTION__EXECUTEMISSIONTASKMESSAGES_HPP_
+#define MISSION_EXECUTION__EXECUTEMISSIONTASKMESSAGES_HPP_
+
+#include "nav2_msgs/msg/mission_plan.hpp"
+#include "std_msgs/msg/empty.hpp"
+
+using ExecuteMissionCommand = nav2_msgs::msg::MissionPlan;
+using ExecuteMissionResult = std_msgs::msg::Empty;
+
+#endif  // MISSION_EXECUTION__EXECUTEMISSIONTASKMESSAGES_HPP_