restructured the repository

Author: klaxalk

README.md

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.15.0)
+project(example_sweeping_generator)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+set(CATKIN_DEPENDENCIES
+  rospy
+  mrs_msgs
+  )
+
+find_package(catkin REQUIRED COMPONENTS
+  ${CATKIN_DEPENDENCIES}
+  )
+
+catkin_package(
+  CATKIN_DEPENDS ${CATKIN_DEPENDENCIES}
+  )
+
+## --------------------------------------------------------------
+## |                           Install                          |
+## --------------------------------------------------------------
+
+install(DIRECTORY launch config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+
+install(DIRECTORY scripts/
+  USE_SOURCE_PERMISSIONS
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+install(DIRECTORY scripts
+  USE_SOURCE_PERMISSIONS
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )

controller_plugin/README.md

@@ -0,0 +1,13 @@
+frame_id: "world_origin"
+
+timer_main:
+  rate: 1.0 # [Hz]
+
+center:
+  x: 0.0 # [m]
+  y: 0.0 # [m]
+  z: 2.0 # [m]
+
+dimensions:
+  x: 20.0 # [m]
+  y: 20.0 # [m]

cpp/README.md

@@ -0,0 +1,24 @@
+<launch>
+
+  <!-- defines name of the namespace of the drone -->
+  <arg name="UAV_NAME" default="$(optenv UAV_NAME uav1)" />
+
+    <!-- Namespace - all topics, services and parameters will be remapped using this namespace as a prefix (eg.: "/waypoint_flier_simple/odom_uav_in" to "/uav1/waypoint_flier_simple/odom_uav_in") -->
+  <group ns="$(arg UAV_NAME)">
+
+    <node pkg="example_sweeping_generator" type="sweeping_generator.py" name="sweeping_generator" output="screen">
+
+        <!-- ROS parameters config file -->
+      <rosparam file="$(find example_sweeping_generator)/config/sweeping_generator.yaml" />
+
+      <remap from="~control_manager_diag_in" to="control_manager/diagnostics" />
+
+      <remap from="~path_out" to="trajectory_generation/path" />
+
+      <remap from="~start_in" to="~start" />
+
+    </node>
+
+  </group>
+
+</launch>

controller_plugin/CMakeLists.txt renamed to cpp/controller_plugin/CMakeLists.txt

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+
+  <name>example_sweeping_generator</name>
+  <version>1.0.0</version>
+  <description>The Python Example for Sweeping Generator</description>
+
+  <maintainer email="[email protected]">Tomas Baca</maintainer>
+  <author email="[email protected]">Tomas Baca</author>
+
+  <license>MIT</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>rospy</depend>
+  <depend>mrs_msgs</depend>
+
+  <export>
+  </export>
+
+</package>

cpp/controller_plugin/README.md

@@ -0,0 +1,167 @@
+#!/usr/bin/python3
+
+import rospy
+import numpy
+
+from mrs_msgs.msg import ControlManagerDiagnostics,Reference
+from mrs_msgs.srv import PathSrv,PathSrvRequest
+from mrs_msgs.srv import Vec1,Vec1Response
+
+class Node:
+
+    # #{ __init__(self)
+
+    def __init__(self):
+
+        rospy.init_node("sweeping_generator", anonymous=True)
+
+        ## | --------------------- load parameters -------------------- |
+
+        self.frame_id = rospy.get_param("~frame_id")
+
+        self.center_x = rospy.get_param("~center/x")
+        self.center_y = rospy.get_param("~center/y")
+        self.center_z = rospy.get_param("~center/z")
+
+        self.dimensions_x = rospy.get_param("~dimensions/x")
+        self.dimensions_y = rospy.get_param("~dimensions/y")
+
+        self.timer_main_rate = rospy.get_param("~timer_main/rate")
+
+        rospy.loginfo('[SweepingGenerator]: initialized')
+
+        ## | ----------------------- subscribers ---------------------- |
+
+        self.sub_control_manager_diag = rospy.Subscriber("~control_manager_diag_in", ControlManagerDiagnostics, self.callbackControlManagerDiagnostics)
+
+        ## | --------------------- service servers -------------------- |
+
+        self.ss_start = rospy.Service('~start_in', Vec1, self.callbackStart)
+
+        ## | --------------------- service clients -------------------- |
+
+        self.sc_path = rospy.ServiceProxy('~path_out', PathSrv)
+
+        ## | ------------------------- timers ------------------------- |
+
+        self.timer_main = rospy.Timer(rospy.Duration(1.0/self.timer_main_rate), self.timerMain)
+
+        ## | -------------------- spin till the end ------------------- |
+
+        self.is_initialized = True
+
+        rospy.spin()
+
+    # #} end of __init__()
+
+    ## | ------------------------- methods ------------------------ |
+
+    # #{ planPath()
+
+    def planPath(self, step_size):
+
+        rospy.loginfo('[SweepingGenerator]: planning path')
+
+        # https://ctu-mrs.github.io/mrs_msgs/srv/PathSrv.html
+        # -> https://ctu-mrs.github.io/mrs_msgs/msg/Path.html
+        path_msg = PathSrvRequest()
+
+        path_msg.path.header.frame_id = self.frame_id
+        path_msg.path.header.stamp = rospy.Time.now()
+
+        path_msg.path.fly_now = True
+
+        path_msg.path.use_heading = True
+
+        sign = 1.0
+
+        # fill in the path with a sweeping pattern
+        for i in numpy.arange(-self.dimensions_x/2.0, self.dimensions_x/2.0, step_size):
+
+            for j in numpy.arange(-self.dimensions_y/2.0, self.dimensions_y/2.0, step_size):
+
+                # https://ctu-mrs.github.io/mrs_msgs/msg/Reference.html
+                point = Reference()
+
+                point.position.x = self.center_x + i
+                point.position.y = self.center_y + j*sign
+                point.position.z = self.center_z
+                point.heading = 0.0
+
+                path_msg.path.points.append(point)
+
+            if sign > 0.0:
+                sign = -1.0
+            else:
+                sign = 1.0
+
+        return path_msg
+
+    # #} end of planPath()
+
+    ## | ------------------------ callbacks ----------------------- |
+
+    # #{ callbackControlManagerDiagnostics():
+
+    def callbackControlManagerDiagnostics(self, msg):
+
+        if not self.is_initialized:
+            return
+
+        rospy.loginfo_once('[SweepingGenerator]: getting ControlManager diagnostics')
+
+        self.sub_control_manager_diag = msg
+
+    # #} end of
+
+    # #{ callbackStart():
+
+    def callbackStart(self, req):
+
+        if not self.is_initialized:
+            return Vec1Response(False, "not initialized")
+
+        # set the step size based on the service data
+        step_size = req.goal
+
+        path_msg = self.planPath(step_size)
+
+        try:
+            response = self.sc_path.call(path_msg)
+        except:
+            rospy.logerr('[SweepingGenerator]: path service not callable')
+            pass
+
+        if response.success:
+            rospy.loginfo('[SweepingGenerator]: path set')
+        else:
+            rospy.loginfo('[SweepingGenerator]: path setting failed, message: {}'.format(response.message))
+
+        return Vec1Response(True, "starting")
+
+    # #} end of
+
+    ## | ------------------------- timers ------------------------- |
+
+    # #{ timerMain()
+
+    def timerMain(self, event=None):
+
+        if not self.is_initialized:
+            return
+
+        rospy.loginfo_once('[SweepingGenerator]: main timer spinning')
+
+        if isinstance(self.sub_control_manager_diag, ControlManagerDiagnostics):
+            if self.sub_control_manager_diag.tracker_status.have_goal:
+                rospy.loginfo('[SweepingGenerator]: tracker has goal')
+            else:
+                rospy.loginfo('[SweepingGenerator]: waiting for command')
+
+    # #} end of timerMain()
+
+if __name__ == '__main__':
+    try:
+        node = Node()
+    except rospy.ROSInterruptException:
+        pass

controller_plugin/cfg/example_controller.cfg renamed to cpp/controller_plugin/cfg/example_controller.cfg

@@ -0,0 +1,5 @@
+# A timeout between the takeoff being triggered and the UAV actually taking off
+# while the timeout is counting down, the takeoff can be aborted by switching off
+# the offboard mode.
+# default = 5 sec
+safety_timeout: 1.0 # [s]