Merge pull request #80 from vortexntnu/feature/task_manager

Feature/task manager

asv_setup/config/topics.yaml

@@ -1,6 +1,10 @@
 # This file defines the ROS topics used by the packeges in Vortex-ASV
 
 asv:
+  thruster_manager:
+    input: "/thrust/desired_forces" # 3DOF thrust vector
+    output: "/thrust/thruster_forces" # Actuator forces
+
   thruster_manager: 
     torque: "/thrust/torque_input"
     force: "/thrust/force_input"
@@ -16,3 +20,6 @@ guidance:
   LOS:
     odom_topic: "/odometry/filtered"
     action_server: "/guidance_interface/los_server"
+
+task_manager:
+  server: "/task_manager/task_manager_server"

mission/task_manager/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(task_manager)
+
+find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
+  roscpp
+  rospy
+)
+
+catkin_python_setup()
+
+generate_dynamic_reconfigure_options(
+  cfg/task_manager.cfg
+)
+
+catkin_package()
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

mission/task_manager/README.md

@@ -0,0 +1,12 @@
+# Task manager package
+=======================
+
+- The task manager package is a package containing functionality for setting up a gui using dynamic reconfigure.
+
+- The gui lets the user choose a task from a drop down menu, and is defined by the task_manager.cfg config file
+
+- The server defined in task_manager_server_node sets up the gui according to the config file, and listens for changes made in the gui by the user.
+
+- A template node for how one can set up a task as a client, so that it can be used together with the task manager, is defined in the folder test_nodes. This node is not runnable, as it tries to access a task that is not defined, and is therefore only meant to be used as a guide.
+
+- For simplicity, when it comes to defining a task, a python package is made. The package contains a python dataclass object called Task, which defines a task by it's name and id (the id is only necessary for the gui setup). All tasks are then grouped together witin a class Tasks. This way, the name (and id) of a task only has to be defined one place, meaning that when the task is added in the config file and the task node is integrated with the task manager, one can simply access the task name through the python package in both cases.

mission/task_manager/cfg/task_manager.cfg

@@ -0,0 +1,22 @@
+#!/usr/bin/python3
+
+from task_manager_defines import defines
+
+PACKAGE = "task_manager"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+tasks = gen.add_group("Tasks")
+
+drop_down_menu = gen.enum([
+    gen.const(defines.Tasks.joystick.name, int_t, defines.Tasks.joystick.id,
+              "Joystick"),
+    gen.const(defines.Tasks.collision_avoidance.name, int_t,
+              defines.Tasks.collision_avoidance.id, "collision_avoidance")
+], "Njord_tasks")
+
+#          Name           Type    Level     Description
+tasks.add("Njord_tasks", int_t, 0, "Njord tasks", edit_method=drop_down_menu)
+
+exit(gen.generate(PACKAGE, "task_manager", "NjordTasks"))

mission/task_manager/launch/task_manager_launch.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<launch>
+
+    <group ns="task_manager">
+        <node name="task_manager_server"                       pkg ="task_manager"          type="task_manager_server_node.py"   output="screen"/>
+    </group>
+
+</launch>

mission/task_manager/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>task_manager</name>
+  <version>0.0.0</version>
+  <description>The task_manager package</description>
+
+
+  <maintainer email="[email protected]">sigurdvb</maintainer>
+  <maintainer email="[email protected]">ronja kræmer</maintainer>
+
+  <license>MIT</license>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+
+  <export>
+
+  </export>
+</package>

mission/task_manager/scripts/task_manager_server_node.py

@@ -0,0 +1,16 @@
+#!/usr/bin/python3
+import rospy
+
+from dynamic_reconfigure.server import Server
+from task_manager.cfg import NjordTasksConfig
+
+
+def callback(config, level):
+    return config
+
+
+if __name__ == "__main__":
+    rospy.init_node("task_manager_server", anonymous=False)
+
+    srv = Server(NjordTasksConfig, callback)
+    rospy.spin()

mission/task_manager/setup.py

@@ -0,0 +1,12 @@
+#!/usr/bin/python3
+
+from setuptools import setup, find_packages
+
+setup(
+    name="task_manager_config",
+    version="0.0.0",
+    description="Defines for the task manager",
+    author="Sigurd von Brandis",
+    author_email="[email protected]",
+    packages=find_packages(),
+)

mission/task_manager/task_manager_defines/defines.py

@@ -0,0 +1,16 @@
+#!/usr/bin/python3
+
+from dataclasses import dataclass
+
+
+# Defines a type of object, Task, that has the attributes id and name
+@dataclass
+class Task:
+    id: int
+    name: str
+
+
+# Class containing all tasks that can be chosen in gui set up using dynamic reconfigure
+class Tasks:
+    joystick = Task(id=0, name="joystick")
+    collision_avoidance = Task(id=1, name="collision_avoidance")

mission/task_manager/test_nodes/template_task.py

@@ -0,0 +1,47 @@
+#!/usr/bin/python3
+
+import rospy
+import dynamic_reconfigure.client
+from task_manager_defines import defines
+
+
+class Template_task:
+    """
+    This task is a template for how one can set up a real task with the task manager
+    This task is not defined in the class Tasks in defines.py, as this is just an example
+    to use as template and not to actually run
+
+    The client task_manager_client subscribes to changes made in the gui set up by the server
+    """
+
+    def __init__(self):
+        rospy.init_node("template_task_node")
+
+        self.isEnabled = False
+
+        task_manager_server = rospy.get_param(
+            "/task_manager/task_manager_Server")
+
+        # Subscribing to changes in task manager gui
+        task_manager_client = dynamic_reconfigure.client.Client(
+            task_manager_server, timeout=5, config_callback=self.callback)
+
+    def callback(self, config):
+        rospy.loginfo(
+            """Client: task change request: {Njord_tasks}""".format(**config))
+        activated_task_id = config["Njord_tasks"]
+
+        # The template_task is not a real task defined in the Tasks class
+        if defines.Tasks.template_task.id == activated_task_id:
+            self.isEnabled = True
+        else:
+            self.isEnabled = False
+        print(f"isEnabled: {self.isEnabled} ")
+
+        return config
+
+
+if __name__ == "__main__":
+    colav = Template_task()
+
+    rospy.spin()