Start implementation of TaskClient and TaskServer (#10)

* Move some of the task base classes out to separate directories.
* Implement start/stop for TaskClient and TaskServer

Author: Michael Jeronimo

src/control/src/DwaController.cpp

@@ -2,6 +2,7 @@
 // Copyright 2018 Intel Corporation. All Rights Reserved.
 
 #include "control/DwaController.hpp"
+#include <chrono>
 
 DwaController::DwaController(const std::string & name, Robot * robot)
 : ControlTask(name, robot)
@@ -24,4 +25,11 @@ void
 DwaController::workerThread()
 {
   RCLCPP_INFO(get_logger(), "DwaController::workerThread");
+
+  while (!stopWorkerThread_)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    RCLCPP_INFO(get_logger(), "DwaController::workerThread: doing work");
+  }
 }
+

src/mission_execution/CMakeLists.txt

@@ -29,6 +29,7 @@ add_executable(mission_execution
 ament_target_dependencies(mission_execution 
   rclcpp 
   std_msgs
+  task
   )
 
 install(TARGETS ${PROJECT_NAME}

src/navigation/include/navigation/SimpleNavigator.hpp

@@ -7,6 +7,7 @@
 #include "navigation/NavigateToPoseTask.hpp"
 #include "planning/AStarPlanner.hpp"
 #include "control/DwaController.hpp"
+#include "task/TaskClient.hpp"
 
 class SimpleNavigator : public NavigateToPoseTask
 {
@@ -21,8 +22,11 @@ class SimpleNavigator : public NavigateToPoseTask
   void workerThread();
 
   // TODO: These will be the client-side proxies (like SimpleActionClient):
-  AStarPlanner * planner_;
-  DwaController * controller_;
+  //AStarPlanner * planner_;
+  //DwaController * controller_;
+
+  TaskClient * planner_;
+  TaskClient * controller_;
 };
 
 #endif  // NAVIGATION__SIMPLENAVIGATOR_HPP_

src/navigation/src/SimpleNavigator.cpp

@@ -2,11 +2,16 @@
 // Copyright 2018 Intel Corporation. All Rights Reserved.
 
 #include "navigation/SimpleNavigator.hpp"
+#include <chrono>
 
 SimpleNavigator::SimpleNavigator(const std::string & name, Robot * robot)
-: NavigateToPoseTask(name, robot), planner_(nullptr), controller_(nullptr)
+: NavigateToPoseTask(name, robot)
 {
   RCLCPP_INFO(get_logger(), "SimpleNavigator::SimpleNavigator");
+
+// TODO: make into C++ smart pointers
+  planner_ = new TaskClient("AStarPlanner", this);
+  controller_ = new TaskClient("DwaController", this);
 }
 
 SimpleNavigator::~SimpleNavigator()
@@ -24,4 +29,18 @@ void
 SimpleNavigator::workerThread()
 {
   RCLCPP_INFO(get_logger(), "SimpleNavigator::workerThread");
+
+  RCLCPP_INFO(get_logger(), "SimpleNavigator::workerThread: sending executes");
+  planner_->execute();
+  controller_->execute();
+
+  while (!stopWorkerThread_ /* && rclcpp::ok() */ )
+  {
+    RCLCPP_INFO(get_logger(), "SimpleNavigator::workerThread: doing work");
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+  }
+
+  RCLCPP_INFO(get_logger(), "SimpleNavigator::workerThread: sending cancels");
+  planner_->cancel();
+  controller_->cancel();
 }

src/planning/include/planning/PlanningTask.hpp

@@ -4,10 +4,10 @@
 #ifndef PLANNING__PLANNINGTASK_HPP_
 #define PLANNING__PLANNINGTASK_HPP_
 
-#include "task/Task.hpp"
+#include "task/TaskServer.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
-class PlanningTask : public Task
+class PlanningTask : public TaskServer
 {
 public:
   PlanningTask(const std::string & name);

src/planning/src/AStarPlanner.cpp

@@ -2,6 +2,7 @@
 // Copyright 2018 Intel Corporation. All Rights Reserved.
 
 #include "planning/AStarPlanner.hpp"
+#include <chrono>
 
 AStarPlanner::AStarPlanner(const std::string & name)
 : PlanningTask(name)
@@ -25,4 +26,11 @@ void
 AStarPlanner::workerThread()
 {
   RCLCPP_INFO(get_logger(), "AStarPlanner::workerThread");
+
+  while (!stopWorkerThread_)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    RCLCPP_INFO(get_logger(), "AStarPlanner::workerThread: doing work");
+  }
 }
+

src/planning/src/PlanningTask.cpp

@@ -4,7 +4,7 @@
 #include "planning/PlanningTask.hpp"
 
 PlanningTask::PlanningTask(const std::string & name)
-: Task(name)
+: TaskServer(name)
 {
   RCLCPP_INFO(get_logger(), "PlanningTask::PlanningTask");
 }

src/task/CMakeLists.txt

@@ -23,7 +23,8 @@ set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "../libs/")
 
 add_library(task STATIC
   src/RobotTask.cpp
-  src/Task.cpp
+  src/TaskServer.cpp
+  src/TaskClient.cpp
   )
 
 ament_target_dependencies(task