Start implementation of TaskClient and TaskServer

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

src/task/include/task/RobotTask.hpp

@@ -4,10 +4,10 @@
 #ifndef TASK__ROBOTTASK_HPP_
 #define TASK__ROBOTTASK_HPP_
 
-#include "task/Task.hpp"
+#include "task/TaskServer.hpp"
 #include "robot/Robot.hpp"
 
-class RobotTask : public Task
+class RobotTask : public TaskServer
 {
 public:
   RobotTask(const std::string & name, Robot * robot);

src/task/include/task/TaskClient.hpp

@@ -0,0 +1,41 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright 2018 Intel Corporation. All Rights Reserved.
+
+#ifndef TASK__TASKCLIENT_HPP_
+#define TASK__TASKCLIENT_HPP_
+
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class TaskClient
+{
+public:
+  TaskClient(const std::string & name, rclcpp::Node * node);
+  virtual ~TaskClient();
+
+  typedef std_msgs::msg::String Goal;
+  typedef std_msgs::msg::String GoalID;
+  typedef std_msgs::msg::String Result;
+  typedef std_msgs::msg::String Feedback;
+  typedef std_msgs::msg::String Status;
+
+  void execute();
+  void cancel();
+
+protected:
+  void onResultReceived(const Result::SharedPtr msg);
+  void onFeedbackReceived(const Feedback::SharedPtr msg);
+  void onStatusReceived(const Status::SharedPtr msg);
+
+  rclcpp::Node * node_;
+
+  rclcpp::Publisher<Goal>::SharedPtr goalPub_;
+  rclcpp::Publisher<GoalID>::SharedPtr cancelPub_;
+
+  rclcpp::Subscription<Result>::SharedPtr resultSub_;
+  rclcpp::Subscription<Feedback>::SharedPtr feedbackSub_;
+  rclcpp::Subscription<Status>::SharedPtr statusSub_;
+};
+
+#endif  // TASK__TASKCLIENT_HPP_

src/task/include/task/TaskServer.hpp

@@ -0,0 +1,56 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright 2018 Intel Corporation. All Rights Reserved.
+
+#ifndef TASK__TASKSERVER_HPP_
+#define TASK__TASKSERVER_HPP_
+
+#include <atomic>
+#include <thread>
+#include <string>
+#include <functional>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class TaskServer: public rclcpp::Node
+{
+public:
+  TaskServer(const std::string & name);
+  virtual ~TaskServer();
+
+  // Execute
+  typedef std::function<void (const std_msgs::msg::String&)> ExecuteCallback;
+
+  typedef std_msgs::msg::String Goal;
+  typedef std_msgs::msg::String GoalID;
+  typedef std_msgs::msg::String Result;
+  typedef std_msgs::msg::String Feedback;
+  typedef std_msgs::msg::String Status;
+
+  // void publishStatus();
+  // void publishResult();
+  // void publishFeedback();
+
+  // Timer, status frequency
+
+protected:
+  void start(); 
+  void stop();
+
+  virtual void workerThread() = 0;
+
+  std::thread *workerThread_;
+  std::atomic<bool> stopWorkerThread_;
+  std::atomic<bool> running_;
+
+  void onGoalReceived(const Goal::SharedPtr msg);
+  void onCancelReceived(const GoalID::SharedPtr msg);
+
+  rclcpp::Subscription<Goal>::SharedPtr goalSub_;
+  rclcpp::Subscription<GoalID>::SharedPtr cancelSub_;
+
+  rclcpp::Publisher<Result>::SharedPtr resultPub_;
+  rclcpp::Publisher<Feedback>::SharedPtr feedbackPub_;
+  rclcpp::Publisher<Status>::SharedPtr statusPub_;
+};
+
+#endif  // TASK__TASKSERVER_HPP_

src/task/src/RobotTask.cpp

@@ -4,7 +4,7 @@
 #include "task/RobotTask.hpp"
 
 RobotTask::RobotTask(const std::string & name, Robot * robot)
-: Task(name)
+: TaskServer(name)
 {
   RCLCPP_INFO(get_logger(), "RobotTask::RobotTask");
   robot = robot;