restructured the repository

README.md

@@ -4,11 +4,17 @@ This repository includes Core ROS examples for the [MRS UAV System](https://gith
 
 ## Packages
 
-* [waypoint_flier_simple](./waypoint_flier_simple) - Minimalistic C++ Nodelet with "_vanilla_" ROS features
-* [waypoint_flier](./waypoint_flier) - Full C++ Nodelet with "_MRS_" libraries and wrappers
-* [controller_plugin](./controller_plugin) - Example of Controller plugin for the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers)
-* [tracker_plugin](./tracker_plugin) - Example of Tracker plugin for the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers)
-* [pluginlib_example](./pluginlib_example) - Example of ROS Pluginlib, similar to how it is used in the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers)
+## C++
+
+* [waypoint_flier_simple](./cpp/waypoint_flier_simple) - Minimalistic C++ Nodelet with "_vanilla_" ROS features
+* [waypoint_flier](./cpp/waypoint_flier) - Full C++ Nodelet with "_MRS_" libraries and wrappers
+* [controller_plugin](./cpp/controller_plugin) - Example of Controller plugin for the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers)
+* [tracker_plugin](./cpp/tracker_plugin) - Example of Tracker plugin for the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers)
+* [pluginlib_example](./cpp/pluginlib_example) - Example of ROS Pluginlib, similar to how it is used in the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers)
+
+## Python
+
+* [sweeping_generator](./python/sweeping_generator) - Minimalistic Python Example that generates sweeping path for the UAV
 
 # Disclaimer
 

cpp/README.md

@@ -0,0 +1 @@
+# MRS C++ Examples

cpp/controller_plugin/README.md

@@ -0,0 +1,9 @@
+# Example Controller Plugin
+
+Example controller plugin for the Control Manager.
+
+## How to start
+
+```bash
+./tmux/start.sh
+```

cpp/tracker_plugin/README.md

@@ -0,0 +1,9 @@
+# Example Tracker Plugin
+
+Example tracker plugin for the MRS Control Manager.
+
+## How to start
+
+```bash
+./tmux/start.sh
+```

waypoint_flier/README.md → cpp/waypoint_flier/README.md

@@ -10,6 +10,14 @@ The package is written in C++ and features custom MRS libraries and msgs.
 * Service `start_waypoint_following` causes the UAV to start tracking the waypoints
 * Service `stop_waypoint_following` stops adding new waypoints. Flight to the current waypoint is not interrupted.
 
+## How to start
+
+```bash
+./tmux/start.sh
+```
+
+The call the services prepared in the terminal window.
+
 ## Package structure
 
 See [ROS packages](http://wiki.ros.org/Packages)

waypoint_flier_simple/README.md → cpp/waypoint_flier_simple/README.md

@@ -9,6 +9,14 @@ You can test the program in simulation (see our [simulation tutorial](https://ct
 * Service `start_waypoint_following` will activate the nodelet
 * The area in which random waypoints are generated is configurable with a separate config file See [.yaml files](http://wiki.ros.org/rosparam)
 
+## How to start
+
+```bash
+./tmux/start.sh
+```
+
+The call the services prepared in the terminal window.
+
 ## Package structure
 
 See [ROS packages](http://wiki.ros.org/Packages)

python/README.md

@@ -0,0 +1 @@
+# MRS Python Examples

python/sweeping_generator/CMakeLists.txt

@@ -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}
+  )

python/sweeping_generator/README.md

@@ -0,0 +1,12 @@
+# Sweeping Generator
+
+## How to start
+
+```bash
+./tmux/start.sh
+```
+
+When ready, call the service prepared in the bottom terminal window:
+```bash
+rosservice call /$UAV_NAME/sweeping_generator/start 3.0
+```

python/sweeping_generator/config/sweeping_generator.yaml

@@ -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]

python/sweeping_generator/launch/sweeping_generator.launch

@@ -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>

python/sweeping_generator/package.xml

@@ -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>

python/sweeping_generator/scripts/sweeping_generator.py

@@ -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

python/sweeping_generator/tmux/config/automatic_start.yaml

@@ -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]