Ros2 devel

src/task/CMakeLists.txt

@@ -26,6 +26,9 @@ add_library(task STATIC
   src/NavigationTask.cpp
   src/PlanningTask.cpp
   src/RobotTask.cpp
+  src/Task.cpp
+  src/AStarPlanner.cpp
+  src/DwaController.cpp
   )
 
 ament_target_dependencies(task

src/task/include/task/AStarPlanner.hpp

@@ -0,0 +1,23 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright 2018 Intel Corporation. All Rights Reserved.
+
+#ifndef TASK__ASTARPLANNER_HPP_
+#define TASK__ASTARPLANNER_HPP_
+
+#include "task/PlanningTask.hpp"
+
+class AStarPlanner : public PlanningTask
+{
+public:
+  AStarPlanner(const std::string & name);
+  AStarPlanner() = delete;
+  ~AStarPlanner();
+
+  void createPlan(const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal);
+
+protected:
+  void workerThread() override;
+};
+
+#endif  // TASK__ASTARPLANNER_HPP_

src/task/include/task/ControlTask.hpp

@@ -9,11 +9,11 @@
 class ControlTask : public RobotTask
 {
 public:
-  explicit ControlTask(Robot * robot);
+  explicit ControlTask(const std::string & name, Robot * robot);
   ControlTask() = delete;
   ~ControlTask();
 
-  // executePlan()
+  virtual void executePlan() = 0;
 };
 
 #endif  // TASK__CONTROLTASK_HPP_

src/task/include/task/DwaController.hpp

@@ -0,0 +1,22 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright 2018 Intel Corporation. All Rights Reserved.
+
+#ifndef TASK__SPECIFICCONTROLLER_HPP_
+#define TASK__SPECIFICCONTROLLER_HPP_
+
+#include "task/ControlTask.hpp"
+
+class DwaController : public ControlTask
+{
+public:
+  DwaController(const std::string & name, Robot * robot);
+  DwaController() = delete;
+  ~DwaController();
+
+  void executePlan() override; 
+
+protected:
+  void workerThread() override; 
+};
+
+#endif  // TASK__SPECIFICCONTROLLER_HPP_

src/task/include/task/NavigationTask.hpp

@@ -5,19 +5,19 @@
 #define TASK__NAVIGATIONTASK_HPP_
 
 #include "task/RobotTask.hpp"
-#include "task/PlanningTask.hpp"
-#include "task/ControlTask.hpp"
+#include "task/AStarPlanner.hpp"
+#include "task/DwaController.hpp"
 
 class NavigationTask : public RobotTask
 {
 public:
-  explicit NavigationTask(Robot * robot);
+  explicit NavigationTask(const std::string & name, Robot * robot);
   NavigationTask() = delete;
   ~NavigationTask();
 
 protected:
-  PlanningTask planner_;
-  ControlTask controller_;
+  AStarPlanner planner_;
+  DwaController controller_;
 };
 
 #endif  // TASK__NAVIGATIONTASK_HPP_

src/task/include/task/PlanningTask.hpp

@@ -4,16 +4,18 @@
 #ifndef TASK__PLANNINGTASK_HPP_
 #define TASK__PLANNINGTASK_HPP_
 
-#include "task/RobotTask.hpp"
+#include "task/Task.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
-class PlanningTask : public RobotTask
+class PlanningTask : public Task
 {
 public:
-  explicit PlanningTask(Robot * robot);
+  PlanningTask(const std::string & name);
   PlanningTask() = delete;
   ~PlanningTask();
 
-  // getPlan()
+  virtual void createPlan(const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal) = 0;
 };
 
 #endif  // TASK__PLANNINGTASK_HPP_

src/task/include/task/RobotTask.hpp

@@ -10,7 +10,7 @@
 class RobotTask : public Task
 {
 public:
-  explicit RobotTask(Robot * robot);
+  RobotTask(const std::string & name, Robot * robot);
   RobotTask() = delete;
   ~RobotTask();
 

src/task/include/task/Task.hpp

@@ -4,22 +4,30 @@
 #ifndef TASK__TASK_HPP_
 #define TASK__TASK_HPP_
 
+#include <atomic>
+#include <thread>
+#include <string>
 #include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
 
-class Task
+class Task: public rclcpp::Node
 {
 public:
-  virtual ~Task() {}
+  Task(const std::string & name);
+  virtual ~Task();
+
+  void execute();
+  void cancel();
 
 protected:
-  // Emulate ROS Action for now:
-  // C++ templates for (Our)SimpleActionServer, (Our)SimpleActionClient
-  //
-  //   * Subscriber for ROS topic for commands w/ parameters
-  //      * Execute Command
-  //      * Cancel Command
-  //   * Publisher for status updates
-  //   * Publisher for completion
+  virtual void workerThread() = 0;
+
+  std::thread *workerThread_;
+  std::atomic<bool> stopWorkerThread_;
+
+  void onCmdReceived(const std_msgs::msg::String::SharedPtr msg);
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr cmdSub_;
 };
 
 #endif  // TASK__TASK_HPP_

src/task/src/AStarPlanner.cpp

@@ -0,0 +1,28 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright 2018 Intel Corporation. All Rights Reserved.
+
+#include "task/AStarPlanner.hpp"
+
+AStarPlanner::AStarPlanner(const std::string & name)
+: PlanningTask(name)
+{
+  RCLCPP_INFO(get_logger(), "AStarPlanner::AStarPlanner");
+}
+
+AStarPlanner::~AStarPlanner()
+{
+  RCLCPP_INFO(get_logger(), "AStarPlanner::~AStarPlanner");
+}
+
+void
+AStarPlanner::createPlan(const geometry_msgs::msg::PoseStamped & start,
+  const geometry_msgs::msg::PoseStamped & goal)
+{
+  RCLCPP_INFO(get_logger(), "AStarPlanner::createPlan");
+}
+
+void
+AStarPlanner::workerThread()
+{
+  RCLCPP_INFO(get_logger(), "AStarPlanner::workerThread");
+}

src/task/src/ControlTask.cpp

@@ -3,11 +3,13 @@
 
 #include "task/ControlTask.hpp"
 
-ControlTask::ControlTask(Robot * robot)
-: RobotTask(robot)
+ControlTask::ControlTask(const std::string & name, Robot * robot)
+: RobotTask(name, robot)
 {
+  RCLCPP_INFO(get_logger(), "ControlTask::ControlTask");
 }
 
 ControlTask::~ControlTask()
 {
+  RCLCPP_INFO(get_logger(), "ControlTask::~ControlTask");
 }

src/task/src/DwaController.cpp

@@ -0,0 +1,27 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright 2018 Intel Corporation. All Rights Reserved.
+
+#include "task/DwaController.hpp"
+
+DwaController::DwaController(const std::string & name, Robot * robot)
+: ControlTask(name, robot)
+{
+  RCLCPP_INFO(get_logger(), "DwaController::DwaController");
+}
+
+DwaController::~DwaController()
+{
+  RCLCPP_INFO(get_logger(), "DwaController::~DwaController");
+}
+
+void
+DwaController::executePlan()
+{
+  RCLCPP_INFO(get_logger(), "DwaController::executePlan");
+}
+
+void
+DwaController::workerThread()
+{
+  RCLCPP_INFO(get_logger(), "DwaController::workerThread");
+}

src/task/src/NavigationTask.cpp

@@ -3,11 +3,13 @@
 
 #include "task/NavigationTask.hpp"
 
-NavigationTask::NavigationTask(Robot * robot)
-: RobotTask(robot), planner_(robot), controller_(robot)
+NavigationTask::NavigationTask(const std::string & name, Robot * robot)
+: RobotTask(name, robot), planner_(name + "Planner"), controller_(name + "Controller", robot)
 {
+  RCLCPP_INFO(get_logger(), "NavigationTask::NavigationTask");
 }
 
 NavigationTask::~NavigationTask()
 {
+  RCLCPP_INFO(get_logger(), "NavigationTask::~NavigationTask");
 }

src/task/src/PlanningTask.cpp

@@ -3,11 +3,13 @@
 
 #include "task/PlanningTask.hpp"
 
-PlanningTask::PlanningTask(Robot * robot)
-: RobotTask(robot)
+PlanningTask::PlanningTask(const std::string & name)
+: Task(name)
 {
+  RCLCPP_INFO(get_logger(), "PlanningTask::PlanningTask");
 }
 
 PlanningTask::~PlanningTask()
 {
+  RCLCPP_INFO(get_logger(), "PlanningTask::~PlanningTask");
 }

src/task/src/RobotTask.cpp

@@ -3,11 +3,14 @@
 
 #include "task/RobotTask.hpp"
 
-RobotTask::RobotTask(Robot * robot)
+RobotTask::RobotTask(const std::string & name, Robot * robot)
+: Task(name)
 {
+  RCLCPP_INFO(get_logger(), "RobotTask::RobotTask");
   robot = robot;
 }
 
 RobotTask::~RobotTask()
 {
+  RCLCPP_INFO(get_logger(), "RobotTask::~RobotTask");
 }

src/task/src/Task.cpp

@@ -0,0 +1,54 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright 2018 Intel Corporation. All Rights Reserved.
+
+#include "task/Task.hpp"
+
+Task::Task(const std::string & name)
+: Node(name), workerThread_(nullptr), stopWorkerThread_(false)
+{
+  RCLCPP_INFO(get_logger(), "Task::Task");
+
+  cmdSub_ = create_subscription<std_msgs::msg::String>("TaskCmd",
+    std::bind(&Task::onCmdReceived, this, std::placeholders::_1));
+}
+
+Task::~Task()
+{
+  RCLCPP_INFO(get_logger(), "Task::~Task");
+  if (workerThread_ != nullptr)
+    cancel();
+}
+
+void
+Task::execute()
+{
+  RCLCPP_INFO(get_logger(), "Task::execute");
+  stopWorkerThread_ = false;
+  workerThread_ = new std::thread(&Task::workerThread, this);
+}
+
+void
+Task::cancel()
+{
+  RCLCPP_INFO(get_logger(), "Task::cancel");
+  stopWorkerThread_ = true;
+  workerThread_->join();
+
+  delete workerThread_;
+  workerThread_ = nullptr;
+}
+
+void
+Task::onCmdReceived(const std_msgs::msg::String::SharedPtr msg)
+{
+  RCLCPP_INFO(get_logger(), "Task::onCmdReceived: \"%s\"", msg->data.c_str())
+
+  if (msg->data.compare("ExecuteTask") == 0) {
+    execute();
+  } else if (msg->data.compare("CancelTask") == 0) {
+    cancel();
+  } else {
+    RCLCPP_INFO(get_logger(), "Task::onCmdReceived: invalid command: \"%s\"",
+      msg->data.c_str())
+  }
+}