Merge branch 'master' into recent-px4

PX4 1.12.3 doesn’t init by flow without mag
force_init runs vpe_publisher to force init using vpe

.github/workflows/docs.yml

@@ -38,11 +38,7 @@ jobs:
          gitbook install
          gitbook build
       - name: Generate PDF
-        id: generate-pdf
+        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
-        env:
-          GITBOOK_SKIP_PDF: ${{ secrets.GITBOOK_SKIP_PDF }}
-        continue-on-error: ${{ env.GITBOOK_SKIP_PDF != '' }}
-        if: ${{ github.event_name == 'push' }}
         run: |
           for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
           sudo apt-get -q install ghostscript
@@ -51,13 +47,6 @@ jobs:
           rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
           rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
           ls -lah _book/clover*.pdf
-          echo '::set-output name=GITBOOK_PDF_OK::1'
-      - name: Download older PDFs
-        if: ${{ !steps.generate-pdf.outputs.GITBOOK_PDF_OK }}
-        run: |
-          rm -f _book/clover*.pdf
-          wget --no-verbose https://clover.coex.tech/clover_ru.pdf -P _book/
-          wget --no-verbose https://clover.coex.tech/clover_en.pdf -P _book/
       - name: Deploy
         uses: JamesIves/github-pages-deploy-action@4.1.3
         if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}

aruco_pose/CMakeLists.txt

@@ -4,8 +4,6 @@ project(aruco_pose)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
-add_compile_options(-Wall -Wextra -Werror)
-
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -85,10 +83,11 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-add_service_files(
+# add_service_files(
-  FILES
+#   FILES
-  SetMarkers.srv
+#   Service1.srv
-)
+#   Service2.srv
+# )
 
 ## Generate actions in the 'action' folder
 # add_action_files(

aruco_pose/README.md

@@ -51,7 +51,6 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 
 * `image_raw` (*sensor_msgs/Image*) – camera image
 * `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
-* `map_markers` (*aruco_pose/MarkerArray*) – list of markers to disable TF transform publishing
 
 #### Published
 
@@ -98,7 +97,6 @@ See examples in [`map`](map/) directory.
 #### Published
 
 * `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
-* `~map` (*aruco_pose/MarkerArray*) – list of markers in the loaded map
 * `~image` (*sensor_msgs/Image*) – planarized map image
 * `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
 * `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis

aruco_pose/cfg/Detector.cfg

@@ -10,8 +10,6 @@ gen = ParameterGenerator()
 
 gen.add("enabled", bool_t, 0, "if detection enabled", True)
 
-gen.add("length", double_t, 0, "markers' side length", min=0, max=10)
-
 gen.add("adaptiveThreshConstant",                 double_t, 0,
         "Constant for adaptive thresholding before finding contours",
          p.adaptiveThreshConstant, 0, 100)

aruco_pose/map/cmit.txt

@@ -1,4 +1,3 @@
-# id	length	x	y	z	rot_z	rot_y	rot_x
 0	0.33	0.0	9.0	0	0	0	0	
 1	0.33	1.0	9.0	0	0	0	0	
 2	0.33	2.0	9.0	0	0	0	0	

aruco_pose/map/office.txt

@@ -1,4 +1,3 @@
-# id	length	x	y	z	rot_z	rot_y	rot_x
 107	0.33	0	0	0	0	0	0
 106	0.33	0.77	0	0	0	0	0
 105	0.33	0	0.77	0	0	0	0

aruco_pose/map/office_ceiling.txt

@@ -1,4 +1,3 @@
-# id	length	x	y	z	rot_z	rot_y	rot_x
 14	0.365	0.000	0.0	0	0	0	0
 15	0.365	1.335	0.0	0	0	0	0
 30	0.365	2.865	0.0	0	0	0	0

aruco_pose/src/aruco_detect.cpp

@@ -48,7 +48,6 @@
 #include <aruco_pose/Marker.h>
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/DetectorConfig.h>
-#include <aruco_pose/SetMarkers.h>
 
 #include "utils.h"
 #include <memory>
@@ -70,10 +69,8 @@ private:
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	ros::Subscriber map_markers_sub_;
-	ros::ServiceServer set_markers_srv_;
 	bool estimate_poses_, send_tf_, auto_flip_;
 	double length_;
-	ros::Duration transform_timeout_;
 	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, known_tilt_;
 	Mat camera_matrix_, dist_coeffs_;
@@ -100,7 +97,6 @@ public:
 			ros::shutdown();
 		}
 		readLengthOverride(nh_priv_);
-		transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
 
 		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
@@ -118,8 +114,6 @@ public:
 		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
 		dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2));
 
-		set_markers_srv_ = nh_priv_.advertiseService("set_length_override", &ArucoDetect::setMarkers, this);
-
 		debug_pub_ = it_priv.advertise("debug", 1);
 		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
@@ -178,7 +172,7 @@ private:
 				if (!known_tilt_.empty()) {
 					try {
 						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
-						                                      msg->header.stamp, transform_timeout_);
+						                                      msg->header.stamp, ros::Duration(0.02));
 					} catch (const tf2::TransformException& e) {
 						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
 					}
@@ -352,29 +346,6 @@ private:
 		}
 	}
 
-	bool setMarkers(aruco_pose::SetMarkers::Request& req, aruco_pose::SetMarkers::Response& res)
-	{
-		for (auto const& marker : req.markers) {
-			if (marker.id > 999) {
-				res.message = "Invalid marker id: " + std::to_string(marker.id);
-				ROS_ERROR("%s", res.message.c_str());
-				return true;
-			}
-			if (!std::isfinite(marker.length) || marker.length <= 0) {
-				res.message = "Invalid marker " + std::to_string(marker.id) + " length: " + std::to_string(marker.length);
-				ROS_ERROR("%s", res.message.c_str());
-				return true;
-			}
-		}
-
-		for (auto const& marker : req.markers) {
-			length_override_[marker.id] = marker.length;
-		}
-
-		res.success = true;
-		return true;
-	}
-
 	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
 	{
 		map_markers_ids_.clear();
@@ -385,8 +356,7 @@ private:
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
 	{
-		enabled_ = config.enabled && config.length > 0;
+		enabled_ = config.enabled;
-		length_ = config.length;
 		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
 		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
 		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;

aruco_pose/src/aruco_map.cpp

@@ -89,7 +89,7 @@ public:
 
 		// TODO: why image_transport doesn't work here?
 		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
-		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("map", 1, true);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
 
 		board_ = cv::makePtr<cv::aruco::Board>();
 		board_->dictionary = cv::aruco::getPredefinedDictionary(

aruco_pose/srv/SetMarkers.srv

@@ -1,7 +0,0 @@
-# * Add or change markers in the map
-# * Change markers' properties, e. g. lengths
-
-Marker[] markers # if length or pose is nan - remove from map
----
-bool success
-string message

aruco_pose/test/basic.py

@@ -143,7 +143,7 @@ def test_map_image(node):
     assert img.encoding in ('mono8', 'rgb8')
 
 def test_map_markers(node):
-    markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5)
+    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
     assert markers.markers[0].id == 1
     assert markers.markers[1].id == 2
     assert markers.markers[2].id == 3

clover/CMakeLists.txt

@@ -4,8 +4,6 @@ project(clover)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
-add_compile_options(-Wall -Wextra -Werror)
-
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -26,7 +24,6 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   image_transport
   cv_bridge
-  dynamic_reconfigure
 )
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
@@ -129,9 +126,10 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-generate_dynamic_reconfigure_options(
+# generate_dynamic_reconfigure_options(
-  cfg/Flow.cfg
+#   cfg/DynReconf1.cfg
-)
+#   cfg/DynReconf2.cfg
+# )
 
 ###################################
 ## catkin specific configuration ##
@@ -213,8 +211,6 @@ add_dependencies(clover_led ${PROJECT_NAME}_generate_messages_cpp)
 
 add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
 
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
-
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use

clover/cfg/Flow.cfg

@@ -1,10 +0,0 @@
-#!/usr/bin/env python
-PACKAGE = "clover"
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-gen.add("enabled", bool_t, 0, "if optical flow enabled", True)
-
-exit(gen.generate(PACKAGE, "clover", "Flow"))

clover/launch/aruco.launch

@@ -15,13 +15,12 @@
     <node name="aruco_detect" pkg="nodelet" if="$(eval aruco_detect and not disable)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="map_markers" to="aruco_map/map"/>
+        <remap from="map_markers" to="aruco_map/markers"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
         <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
         <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="length" value="$(arg length)"/>
-        <param name="transform_timeout" value="0.1"/>
         <!-- aruco detector parameters -->
         <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
         <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->

clover/launch/mavros.launch

@@ -39,7 +39,7 @@
         <rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
 
         <!-- remap rangefinder -->
-        <remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
+        <remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
 
         <rosparam param="plugin_whitelist">
             - altitude

clover/launch/mavros_config.yaml

@@ -78,9 +78,6 @@ distance_sensor:
       field_of_view: 0.5
   rangefinder_sub:
       subscriber: true
-      id: 1
-      orientation: PITCH_270
-      covariance: 1  # cm
 
 # fake_gps
 fake_gps:

clover/package.xml

@@ -39,7 +39,6 @@
   <depend>tf2_web_republisher</depend>
   <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
   <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
-  <depend>dynamic_reconfigure</depend>
   <exec_depend>python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->

clover/src/autotest/autotest_aruco.py

@@ -1,87 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-import math
-import signal
-import sys
-import dynamic_reconfigure.client
-from clover import srv
-from std_srvs.srv import Trigger
-from sensor_msgs.msg import Range
-from aruco_pose.msg import MarkerArray
-from util import handle_response
-
-rospy.init_node('autotest_aruco', disable_signals=True) # disable signals to allow interrupting with ctrl+c
-
-flow_client = dynamic_reconfigure.client.Client('optical_flow')
-get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
-navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
-land = handle_response(rospy.ServiceProxy('land', Trigger))
-
-def interrupt(sig, frame):
-    print('\nInterrupted, landing...')
-    land()
-    sys.exit(0)
-
-signal.signal(signal.SIGINT, interrupt)
-
-def print_current_map_position():
-    telem = get_telemetry()
-    dist = rospy.wait_for_message('rangefinder/range', Range).range
-    print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist))
-
-def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
-        frame_id='body', tolerance=0.2, auto_arm=False):
-
-    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
-        frame_id=frame_id, auto_arm=auto_arm)
-
-    if not res.success:
-        return res
-
-    while not rospy.is_shutdown():
-        telem = get_telemetry(frame_id='navigate_target')
-        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
-            return res
-        rospy.sleep(0.2)
-
-markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=3)
-left = min(marker.pose.position.x for marker in markers.markers)
-bottom = min(marker.pose.position.y for marker in markers.markers)
-width = max(marker.pose.position.x for marker in markers.markers)
-height = max(marker.pose.position.y for marker in markers.markers)
-center_x = left + width / 2
-center_y = bottom + height / 2
-
-print('Map rect: %g %g - %g %g' % (left, bottom, width, height))
-
-input('Take off and hover 1 m [enter] ')
-navigate_wait(x=0, y=0, z=1, frame_id='body', auto_arm=True)
-print_current_map_position()
-
-input('Go to corner %g %g 1.5 speed 1 [enter] ' % (width, height))
-navigate_wait(x=width, y=height, z=1.5, speed=1, frame_id='aruco_map')
-print_current_map_position()
-
-input('Go to center %g %g 1.5 speed 5 [enter] ' % (center_x, center_y))
-navigate_wait(x=center_x, y=center_y, z=1.5, speed=5, frame_id='aruco_map')
-print_current_map_position()
-
-input('Disable optical flow and keep hovering [enter] ')
-flow_client.update_configuration({'enabled': False})
-rospy.sleep(5)
-
-input('Enable optical flow back [enter] ')
-flow_client.update_configuration({'enabled': True})
-
-input('Go to side 1 %g 2 heading top [enter] ' % (center_y))
-navigate_wait(x=1, y=center_y, z=2, yaw=1.57, frame_id='aruco_map')
-print_current_map_position()
-
-marker_id = markers.markers[0].id
-input('Go to marker %d z=1.5 [enter] ' % marker_id)
-navigate_wait(x=0, y=0, z=1.5, yaw=0, frame_id='aruco_%d' % marker_id)
-print_current_map_position()
-
-input('Perform landing [enter] ')
-land()

clover/src/autotest/autotest_flight.py

@@ -1,100 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-import math
-from math import nan
-import signal
-import sys
-from clover import srv
-from std_srvs.srv import Trigger
-from sensor_msgs.msg import Range
-from util import handle_response
-
-rospy.init_node('autotest_flight', disable_signals=True) # disable signals to allow interrupting with ctrl+c
-
-get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
-navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
-navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal))
-set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition))
-set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity))
-set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude))
-set_rates = handle_response(rospy.ServiceProxy('set_rates', srv.SetRates))
-land = handle_response(rospy.ServiceProxy('land', Trigger))
-
-def interrupt(sig, frame):
-    print('\nInterrupted, landing...')
-    land()
-    sys.exit(0)
-
-signal.signal(signal.SIGINT, interrupt)
-
-def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \
-        frame_id='body', tolerance=0.2, auto_arm=False):
-
-    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
-        frame_id=frame_id, auto_arm=auto_arm)
-
-    if not res.success:
-        return res
-
-    while not rospy.is_shutdown():
-        telem = get_telemetry(frame_id='navigate_target')
-        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
-            return res
-        rospy.sleep(0.2)
-
-def print_distance():
-    dist = rospy.wait_for_message('rangefinder/range', Range).range
-    print('Distance: {:.2f}'.format(dist))
-
-input('Take off and hover 1 m [enter] ')
-navigate_wait(z=1, frame_id='body', auto_arm=True)
-print_distance()
-start = get_telemetry()
-
-input('Fly forward 2 m [enter] ')
-navigate_wait(x=2, frame_id='navigate_target')
-print_distance()
-
-input('Climb 0.5 m [enter] ')
-navigate_wait(z=0.5, frame_id='navigate_target')
-print_distance()
-
-input('Rotate left 90° [enter] ')
-navigate(yaw=math.pi / 2, frame_id='navigate_target')
-rospy.sleep(3)
-
-input('Use set_velocity to fly forward 2 m speed 1 [enter]')
-set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
-rospy.sleep(2)
-set_position(frame_id='body')
-
-input('Rotate right 90° [enter] ')
-navigate(yaw=-math.pi / 2, frame_id='navigate_target')
-rospy.sleep(3)
-
-input('Use set_attitude to fly backwards [enter]')
-set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body')
-rospy.sleep(0.3)
-set_position(frame_id='body')
-
-input('Use set_attitude to fly right [enter]')
-set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
-rospy.sleep(0.5)
-set_position(frame_id='body')
-
-input('Use set_rates to fly right [enter]')
-set_rates(roll_rate=1.2, thrust=0.5)
-rospy.sleep(0.4)
-set_position(frame_id='body')
-
-input('Rotate 360° to the right using yaw_rate [enter]')
-set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
-rospy.sleep(2 * math.pi)
-set_position(frame_id='body')
-
-input('Return to start point [enter]')
-navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
-
-input('Land [enter]')
-land()

clover/src/autotest/autotest_led.py

@@ -1,72 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-import functools
-from clover.srv import SetLEDEffect
-from led_msgs.srv import SetLEDs
-from led_msgs.msg import LEDStateArray, LEDState
-from util import handle_response
-
-rospy.init_node('autotest_led', disable_signals=True)
-
-set_leds = handle_response(rospy.ServiceProxy('led/set_leds', SetLEDs))
-set_effect = handle_response(rospy.ServiceProxy('led/set_effect', SetLEDEffect))
-
-led_count = len(rospy.wait_for_message('led/state', LEDStateArray, timeout=10).leds)
-print('LED count =', led_count)
-
-print('== Testing effects ==')
-
-input('Fill red [enter] ')
-set_effect(r=255, g=0, b=0)
-
-input('Fill green [enter] ')
-set_effect(r=0, g=100, b=0)
-
-input('Blink white [enter] ')
-set_effect(effect='blink', r=255, g=255, b=255)
-rospy.sleep(3)
-
-input('Blink fast violet [enter] ')
-set_effect(effect='blink_fast', r=220, g=20, b=250)
-rospy.sleep(3)
-
-input('Fade to blue [enter] ')
-set_effect(effect='fade', r=0, g=0, b=255)
-
-input('Wipe to yellow [enter] ')
-set_effect(effect='wipe', r=255, g=255, b=40)
-
-input('Flash red [enter] ')
-set_effect(effect='flash', r=255, g=0, b=0)
-rospy.sleep(1)
-
-input('Rainbow [enter] ')
-set_effect(effect='rainbow')
-rospy.sleep(4)
-
-input('Rainbow fill [enter] ')
-set_effect(effect='rainbow_fill')
-rospy.sleep(4)
-
-input('Turn off [enter] ')
-set_effect()
-
-print('== Testing low-level control ==')
-
-input('Fill orange [enter] ')
-set_leds(leds=[LEDState(index=i, r=245, g=155, b=0) for i in range(led_count)])
-
-input('Fill blue gradient [enter] ')
-set_leds(leds=[LEDState(index=i, r=0, g=20, b=int(255 * i / led_count)) for i in range(led_count)])
-
-input('Animate green dot [enter] ')
-set_effect()
-for i in range(led_count):
-    if i > 0:
-        set_leds(leds=[LEDState(index=i - 1, r=0, g=0, b=0)])
-    set_leds(leds=[LEDState(index=i, r=0, g=255, b=0)])
-    rospy.sleep(0.05)
-
-input('Turn off [enter] ')
-set_effect()

clover/src/autotest/util.py

@@ -1,11 +0,0 @@
-import functools
-
-# decorator to handle response and print error message
-def handle_response(fn):
-    @functools.wraps(fn)
-    def wrapper(*args, **kwargs):
-        res = fn(*args, **kwargs)
-        if not res.success:
-            print('\033[91mError:\033[0m {}'.format(res.message))
-        return res
-    return wrapper

clover/src/optical_flow.cpp

@@ -22,13 +22,11 @@
 #include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <dynamic_reconfigure/server.h>
 #include <mavros_msgs/OpticalFlowRad.h>
 #include <sensor_msgs/Imu.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
-#include <clover/FlowConfig.h>
 
 using cv::Mat;
 
@@ -40,7 +38,6 @@ public:
 	{}
 
 private:
-	bool enabled_;
 	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
 	ros::Time prev_stamp_;
 	std::string fcu_frame_id_, local_frame_id_;
@@ -57,7 +54,6 @@ private:
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
 	float flow_gyro_default_;
-	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
 
 	void onInit()
 	{
@@ -87,12 +83,6 @@ private:
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 
-		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
-		dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
-
-		cb = std::bind(&OpticalFlow::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
-		dyn_srv_->setCallback(cb);
-
 		NODELET_INFO("Optical Flow initialized");
 	}
 
@@ -119,8 +109,6 @@ private:
 
 	void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
 	{
-		if (!enabled_) return;
-
 		parseCameraInfo(cinfo);
 
 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -236,6 +224,7 @@ private:
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 
+publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
 				// publish debug image
@@ -275,14 +264,6 @@ private:
 
 		return flow;
 	}
-
-	void paramCallback(clover::FlowConfig &config, [[maybe_unused]] uint32_t level)
-	{
-		enabled_ = config.enabled;
-		if (!enabled_) {
-			prev_ = Mat(); // clear previous frame
-		}
-	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clover/src/selfcheck.py

@@ -30,7 +30,6 @@ from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
 from mavros import mavlink
-import locale
 
 
 # TODO: check attitude is present
@@ -46,8 +45,6 @@ rospy.init_node('selfcheck')
 
 os.environ['ROSCONSOLE_FORMAT']='[${severity}]: ${message}'
 
-# use user's locale to convert numbers, etc
-locale.setlocale(locale.LC_ALL, '')
 
 tf_buffer = tf2_ros.Buffer()
 tf_listener = tf2_ros.TransformListener(tf_buffer)
@@ -641,7 +638,7 @@ def check_cpu_usage():
             continue
         pid, cpu, cmd = process.split('\t')
 
-        if cmd.strip() not in WHITELIST and locale.atof(cpu) > 30:
+        if cmd.strip() not in WHITELIST and float(cpu) > 30:
             failure('high CPU usage (%s%%) detected: %s (PID %s)',
                     cpu.strip(), cmd.strip(), pid.strip())
 
@@ -746,14 +743,6 @@ def check_network():
 
 @check('RPi health')
 def check_rpi_health():
-    try:
-        import shutil
-        total, used, free = shutil.disk_usage('/')
-        if free < 1024 * 1024 * 1024:
-            failure('disk space is less than 1 GB; consider removing logs (~/.ros/log/)')
-    except Exception as e:
-        info('could not check the disk free space: %s', str(e))
-
     # `vcgencmd get_throttled` output codes taken from
     # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md#get_throttled
     # TODO: support more base platforms?

clover/src/simple_offboard.cpp

@@ -61,7 +61,6 @@ std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
 std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;
 
 // Parameters
-string mavros;
 string local_frame;
 string fcu_frame;
 ros::Duration transform_timeout;
@@ -503,10 +502,10 @@ inline void checkManualControl()
 
 	if (check_kill_switch) {
 		// switch values: https://github.com/PX4/PX4-Autopilot/blob/c302514a0809b1765fafd13c014d705446ae1113/msg/manual_control_setpoint.msg#L3
-		[[maybe_unused]] const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
+		const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
-		[[maybe_unused]] const uint8_t SWITCH_POS_ON = 1; // switch activated
+		const uint8_t SWITCH_POS_ON = 1; // switch activated
-		[[maybe_unused]] const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
+		const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
-		[[maybe_unused]] const uint8_t SWITCH_POS_OFF = 3; // switch not activated
+		const uint8_t SWITCH_POS_OFF = 3; // switch not activated
 
 		const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
 		uint8_t kill_switch = (manual_control.buttons & (0b11 << KILL_SWITCH_BIT)) >> KILL_SWITCH_BIT;
@@ -691,7 +690,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		// }
 
 		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
-			// destination point and/or attitude
+			// destination point and/or yaw
 			PoseStamped ps;
 			ps.header.frame_id = frame_id;
 			ps.header.stamp = stamp;
@@ -700,12 +699,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			ps.pose.position.z = z;
 			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
 
-			if (sp_type == ATTITUDE) {
+			if (std::isnan(yaw)) {
-				ps.pose.position.x = 0;
-				ps.pose.position.y = 0;
-				ps.pose.position.z = 0;
-				ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
-			} else if (std::isnan(yaw)) {
 				setpoint_yaw_type = YAW_RATE;
 				setpoint_yaw_rate = yaw_rate;
 			} else if (std::isinf(yaw) && yaw > 0) {
@@ -808,7 +802,7 @@ bool setRates(SetRates::Request& req, SetRates::Response& res) {
 	return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
 }
 
-bool land([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	try {
 		if (busy)
@@ -867,9 +861,8 @@ int main(int argc, char **argv)
 	static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
 
 	// Params
-	nh_priv.param("mavros", mavros, string("mavros")); // for case of using multiple connections
+	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
-	nh.param<string>(mavros + "/local_position/tf/frame_id", local_frame, "map");
+	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
-	nh.param<string>(mavros + "/local_position/tf/child_frame_id", fcu_frame, "base_link");
 	nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
 	nh_priv.param("setpoint", setpoint.child_frame_id, string("setpoint"));
 	nh_priv.param("auto_release", auto_release, true);
@@ -901,25 +894,25 @@ int main(int argc, char **argv)
 	arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0));
 
 	// Service clients
-	arming = nh.serviceClient<mavros_msgs::CommandBool>(mavros + "/cmd/arming");
+	arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
-	set_mode = nh.serviceClient<mavros_msgs::SetMode>(mavros + "/set_mode");
+	set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
 
 	// Telemetry subscribers
-	auto state_sub = nh.subscribe(mavros + "/state", 1, &handleState);
+	auto state_sub = nh.subscribe("mavros/state", 1, &handleState);
-	auto velocity_sub = nh.subscribe(mavros + "/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
+	auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
-	auto global_position_sub = nh.subscribe(mavros + "/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
+	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
-	auto battery_sub = nh.subscribe(mavros + "/battery", 1, &handleMessage<BatteryState, battery>);
+	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
-	auto statustext_sub = nh.subscribe(mavros + "/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
+	auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
-	auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
+	auto manual_control_sub = nh.subscribe("mavros/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
-	auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition);
+	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);
 
 	// Setpoint publishers
-	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
+	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
-	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
+	position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
-	attitude_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_attitude/attitude", 1);
+	attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
-	attitude_raw_pub = nh.advertise<AttitudeTarget>(mavros + "/setpoint_raw/attitude", 1);
+	attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
-	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
+	rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
-	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
+	thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);
 
 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);

clover/src/vpe_publisher.cpp

@@ -38,9 +38,9 @@ TransformStamped offset;
 
 void publishZero(const ros::TimerEvent& e)
 {
-	if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
+	if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
 
-	if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
+	if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
 		if (got_local_pos.isZero()) {
 			ROS_INFO("got local position");
 			got_local_pos = e.current_real;
@@ -109,7 +109,7 @@ void callback(const T& msg)
 	}
 }
 
-bool reset([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+bool reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	reset_flag = true;
 	res.success = true;

clover/test/basic.py

@@ -33,29 +33,3 @@ def test_web_video_server(node):
         # Python 3
         import urllib.request as urllib
     urllib.urlopen("http://localhost:8080").read()
-
-def test_blocks(node):
-    rospy.wait_for_service('clover_blocks/run', timeout=5)
-    rospy.wait_for_service('clover_blocks/stop', timeout=5)
-    rospy.wait_for_service('clover_blocks/load', timeout=5)
-    rospy.wait_for_service('clover_blocks/store', timeout=5)
-
-    from std_msgs.msg import String
-    from clover_blocks.srv import Run
-
-    def wait_print():
-        try:
-            wait_print.result = rospy.wait_for_message('clover_blocks/print', String, timeout=5).data
-        except Exception as e:
-            wait_print.result = str(e)
-
-    import threading
-    t = threading.Thread(target=wait_print)
-    t.start()
-    rospy.sleep(0.1)
-
-    run = rospy.ServiceProxy('clover_blocks/run', Run)
-    assert run(code='print("test")').success == True
-
-    t.join()
-    assert wait_print.result == 'test'

clover/test/basic.test

@@ -23,7 +23,10 @@
 
     <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped" required="true"/>
 
-    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
+    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen">
+        <param name="reference_frames/body" value="map"/>
+        <param name="reference_frames/base_link" value="map"/>
+    </node>
 
     <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
 
@@ -35,8 +38,6 @@
         <rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
     </node>
 
-    <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" required="true"/>
-
     <param name="test_module" value="$(find clover)/test/basic.py"/>
     <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
 </launch>

clover_description/urdf/clover/clover4_physics.xacro

@@ -35,7 +35,7 @@
   <xacro:property name="sqrt2" value="1.4142135623730951" />
   <xacro:property name="rotor_drag_coefficient" value="1.75e-04" />
   <xacro:property name="rolling_moment_coefficient" value="0.000001" />
-  <xacro:property name="color" value="DarkGrey" />
+  <xacro:property name="color" value="$(arg visual_material)" />
 
   <!-- Property Blocks -->
   <!-- Clover body inertia -->

clover_description/urdf/leds/led_strip.xacro

@@ -64,12 +64,6 @@
         <!-- <gazebo>
             <static>true</static>
         </gazebo> -->
-        <gazebo>
-            <plugin name="${name}_ros_controller" filename="libsim_leds_controller.so">
-                <robotNamespace></robotNamespace>
-                <ledCount>${led_count}</ledCount>
-            </plugin>
-        </gazebo>
     </xacro:macro>
 
 </robot>

clover_simulation/CMakeLists.txt

@@ -37,14 +37,6 @@ target_compile_options(sim_leds PRIVATE -std=c++11)
 
 add_dependencies(sim_leds ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-add_library(sim_leds_controller src/sim_leds_controller.cpp)
-
-target_include_directories(sim_leds_controller PRIVATE ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
-target_link_libraries(sim_leds_controller PRIVATE ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
-target_compile_options(sim_leds_controller PRIVATE -std=c++11)
-
-add_dependencies(sim_leds_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
 # Gazebo throttling camera plugin
 # for some reason, CMake does not support per-target link directories, and Gazebo does not put
 # CameraPlugin into ${GAZEBO_LIBRARIES}

clover_simulation/airframes/4500_clover

@@ -21,11 +21,11 @@ param set LPE_VIS_Z 0.1
 param set LPE_FUSION 86
 
 param set SENS_FLOW_ROT 0
-param set SENS_FLOW_MINHGT 0.0
+param set SENS_FLOW_MINHGT 0.01
 param set SENS_FLOW_MAXHGT 4.0
 param set SENS_FLOW_MAXR 10.0
 
-param set EKF2_AID_MASK 26 # flow + vis pos + vis yaw
+param set EKF2_AID_MASK 27 # gps + flow + vis pos + vis yaw
 param set EKF2_OF_DELAY 0
 param set EKF2_OF_QMIN 10
 param set EKF2_OF_N_MIN 0.05

clover_simulation/src/sim_leds.cpp

@@ -49,9 +49,14 @@ private:
 
 	std::unique_ptr<ros::NodeHandle> nh;
 
+	ros::ServiceServer setLedsSrv;
+	// Note: LED state should only be published by the /gazebo node
+	led_msgs::LEDStateArray ledState;
+	ros::Publisher statePublisher;
 	// LED state will be read from the topic to avoid creating more services
 	ros::Subscriber stateSubscriber;
 
+	bool setLeds(led_msgs::SetLEDs::Request& req, led_msgs::SetLEDs::Response& resp);
 	void handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds);
 
 public:
@@ -68,8 +73,16 @@ public:
 		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server");
 
 		nh.reset(new ros::NodeHandle(robotNamespace));
-
+		if (role == Role::Server)
-		stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
+		{
+			setLedsSrv = nh->advertiseService("led/set_leds", &LedController::setLeds, this);
+			statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
+		}
+		else
+		{
+			// LED state should be published to the "led/state" topic, so we grab our data there
+			stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
+		}
 	};
 
 	~LedController()
@@ -83,9 +96,13 @@ public:
 		std::lock_guard<std::mutex> lock(registryMutex);
 		if (totalLeds > 0) {
 			registeredLeds.resize(totalLeds);
+			ledState.leds.resize(totalLeds);
 		}
 		ROS_DEBUG_NAMED(("LedController_" + robotNamespace).c_str(), "Registering LED visual plugin to %s (LED id=%d)", (role == Role::Client) ? "client" : "server", ledIdx);
 		registeredLeds[ledIdx] = plugin;
+		ledState.leds[ledIdx].index = ledIdx;
+		if (role == Role::Server)
+			statePublisher.publish(ledState);
 	}
 
 	void unregisterPlugin(sim_led::LedVisualPlugin* plugin)
@@ -140,8 +157,7 @@ public:
 		{
 			auto indexStr = parentName.substr(lastDashPos + 1);
 			try {
-				if (indexStr == "visual") myIndex = 0; // the first visual doesn't have index
+				myIndex = std::stoi(indexStr);
-				else myIndex = std::stoi(indexStr);
 			} catch(const std::exception &e) {
 				gzwarn << "Failed to convert " << indexStr << " to integer: " << e.what() <<  ", assuming 0\n";
 				myIndex = 0;
@@ -179,6 +195,26 @@ public:
 };
 }
 
+// FIXME: These two functions basically do the same thing, maybe they can be merged?
+bool led_controller::LedController::setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
+{
+	std::lock_guard<std::mutex> lock(registryMutex);
+	for(const auto& led : req.leds)
+	{
+		if (led.index < registeredLeds.size()) {
+			auto color = GazeboColor(led.r / 255.0f, led.g / 255.0f, led.b / 255.0f);
+			auto ledPlugin = registeredLeds[led.index];
+			if (ledPlugin) ledPlugin->SetColor(color);
+			ledState.leds[led.index].r = led.r;
+			ledState.leds[led.index].g = led.g;
+			ledState.leds[led.index].b = led.b;
+		}
+	}
+	statePublisher.publish(ledState);
+	resp.success = true;
+	return true;
+}
+
 void led_controller::LedController::handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds)
 {
 	std::lock_guard<std::mutex> lock(registryMutex);

clover_simulation/src/sim_leds_controller.cpp

@@ -1,71 +0,0 @@
-#include <led_msgs/SetLEDs.h>
-#include <led_msgs/LEDStateArray.h>
-
-#include <ros/ros.h>
-
-#include <gazebo/gazebo.hh>
-#include <gazebo/physics/physics.hh>
-#include <gazebo/common/common.hh>
-
-class LedControllerPlugin : public gazebo::ModelPlugin {
-private:
-	std::unique_ptr<ros::NodeHandle> nh;
-	std::string ns;
-	ros::ServiceServer setLedsSrv;
-	led_msgs::LEDStateArray ledState;
-	ros::Publisher statePublisher;
-	std::mutex handleMutex;
-
-public:
-	bool setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
-	{
-		std::lock_guard<std::mutex> lock(handleMutex);
-		for(const auto& led : req.leds)
-		{
-			if (led.index < ledState.leds.size()) {
-				ledState.leds[led.index].r = led.r;
-				ledState.leds[led.index].g = led.g;
-				ledState.leds[led.index].b = led.b;
-			}
-		}
-		statePublisher.publish(ledState);
-		resp.success = true;
-		return true;
-	}
-
-	virtual void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
-	{
-		ROS_INFO("Initialize LED Controller");
-
-		// We need "libgazebo_ros_api.so" to be loaded
-		if (!ros::isInitialized())
-		{
-			ROS_FATAL_NAMED("LedController", "Tried to load ROS plugin when ROS Gazebo API is not loaded. Please use gazebo_ros node to"
-			                                 "launch Gazebo.");
-		}
-
-		ns = "";
-
-		if (sdf->HasElement("robotNamespace")) {
-			ns = sdf->Get<std::string>("robotNamespace");
-		}
-		if (!sdf->HasElement("ledCount")) {
-			gzerr << "ledCount is not set, but is required for the plugin to function correctly\n";
-			return;
-		}
-		int totalLeds = sdf->Get<int>("ledCount");
-		ledState.leds.resize(totalLeds);
-		for (int i = 0; i < totalLeds; i++) {
-			ledState.leds[i].index = i;
-		}
-
-		nh.reset(new ros::NodeHandle(ns));
-
-		setLedsSrv = nh->advertiseService("led/set_leds", &LedControllerPlugin::setLeds, this);
-		statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
-
-		statePublisher.publish(ledState);
-	}
-};
-
-GZ_REGISTER_MODEL_PLUGIN(LedControllerPlugin);

docs/assets/copter_cat/logo.svg

@@ -1,25 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
-<!-- Creator: CorelDRAW 2019 (64-Bit) -->
-<svg xmlns="http://www.w3.org/2000/svg" xml:space="preserve" width="255.858mm" height="78.1171mm" version="1.1" style="shape-rendering:geometricPrecision; text-rendering:geometricPrecision; image-rendering:optimizeQuality; fill-rule:evenodd; clip-rule:evenodd"
-viewBox="0 0 5794.02 1769"
- xmlns:xlink="http://www.w3.org/1999/xlink"
- xmlns:xodm="http://www.corel.com/coreldraw/odm/2003">
- <defs>
-  <style type="text/css">
-   <![CDATA[
-    .fil1 {fill:#1F1B20}
-    .fil0 {fill:#1F1B20;fill-rule:nonzero}
-   ]]>
-  </style>
- </defs>
- <g id="Слой_x0020_1">
-  <metadata id="CorelCorpID_0Corel-Layer"/>
-  <polygon class="fil0" points="3293.37,1768.99 2509.06,1768.97 2507.86,1661.12 3292.17,1660.65 "/>
-  <polygon class="fil0" points="3133.99,231.57 2664.24,231.57 2556.99,123.72 3237.05,123.23 "/>
-  <path class="fil0" d="M3647.51 123.72l1925.38 0 0 381.38 -107.85 -8.69 0 -264.84 -1809.74 0 -7.79 -107.85zm1925.38 676.77l0 968.51 -1653.42 0 -1.5 -108.34 1547.07 0.49 0 -850.17 107.85 -10.48z"/>
-  <path class="fil0" d="M2142.36 123.72l-1925.38 0 0 381.38 107.85 -8.69 0 -264.84 1809.74 0 7.79 -107.85zm-1925.38 676.77l0 968.51 1654.08 0 1.5 -108.34 -1547.73 0.49 0 -850.17 -107.85 -10.48z"/>
-  <path class="fil1" d="M2887.45 1229.18c-100.66,0 -189.34,-87.18 -244.46,-111.14 -55.12,-23.96 -215.7,-63.51 -215.7,-63.51 0,0 -88.67,-131.81 -110.24,-196.52 -21.57,-64.71 -38.35,-225.29 -38.35,-225.29 0,0 -28.76,-59.92 -38.35,-86.28 -9.59,-26.36 -19.17,-79.09 -19.17,-79.09l67.11 -59.92 -71.9 -21.57c0,0 -9.59,-131.82 -9.59,-184.54 0,-52.73 28.76,-201.32 28.76,-201.32 0,0 143.8,76.69 242.06,148.59 98.26,71.9 201.32,220.49 201.32,220.49 0,0 97.96,-32.96 220.19,-32.96 122.23,0 212.4,32.96 212.4,32.96 0,0 103.06,-148.59 201.32,-220.49 98.26,-71.9 242.06,-148.59 242.06,-148.59 29.86,154.29 35.16,239.28 25.77,366.69 -7.33,99.38 -20.22,164.6 -68.91,266.02 0,0 -16.78,146.2 -38.35,225.29 -21.57,79.09 -110.24,196.52 -110.24,196.52 0,0 -165.37,25.17 -221.69,53.92 -56.32,28.76 -153.38,120.73 -254.04,120.73zm-260.11 -471.35c0,0 -32.95,31.4 -32.95,71.54 0,40.15 29.36,76.45 29.36,76.45 0,0 30.97,-38.24 30.97,-73.59 0,-35.35 -27.38,-74.4 -27.38,-74.4zm-164.17 -37.15c0,0 84.84,-18.88 192.09,33.85 107.25,52.73 163.22,135.12 163.22,135.12 0,0 -122.28,71.58 -245.11,36.23 -122.83,-35.35 -110.2,-205.2 -110.2,-205.2zm687.07 37.15c0,0 32.95,31.4 32.95,71.54 0,40.15 -29.36,76.45 -29.36,76.45 0,0 -30.97,-38.24 -30.97,-73.59 0,-35.35 27.38,-74.4 27.38,-74.4zm164.17 -37.15c0,0 -84.84,-18.88 -192.09,33.85 -107.25,52.73 -163.22,135.12 -163.22,135.12 0,0 122.28,71.58 245.11,36.23 122.83,-35.35 110.2,-205.2 110.2,-205.2z"/>
-  <path class="fil1" d="M2895.05 1398.42c0,0 -216.07,-1.19 -337.7,-47.93 -121.63,-46.74 -147.39,-68.91 -147.39,-68.91l-149.64 380.47 134.66 0 1.2 105.45 -411.02 0 0 -105.45 154.43 0 105.6 -464.95c0,0 -226.48,-22.77 -424.21,-81.49 -197.72,-58.72 -354.7,-127.02 -492.51,-133.01 -137.81,-5.99 -114.44,-2.99 -198.32,-7.79 -83.88,-4.79 -142.6,-47.92 -142.6,-82.68 0,-34.75 1.2,-59.32 1.2,-97.66 0,-38.35 32.36,-46.74 32.36,-46.74l-1.2 -29.96 -22.77 0c0,-8.09 12.58,-46.74 -53.93,-46.74 -66.51,0 -175.67,58.19 -233.67,77.89 -58,19.71 -552.43,-15.58 -644.7,-41.94 -92.27,-26.36 -64.71,-77.89 -45.54,-94.67 19.17,-16.78 539.25,-63.51 645.9,-56.32 106.65,7.19 329.54,44.34 329.54,44.34l0 -26.36 25.76 0 0 -25.76 12.58 0c0,0 2.39,-94.67 2.39,-134.21 0,-39.54 37.23,-60.33 65.31,-59.9 28.08,0.43 63.2,25.75 63.2,61.1 0,35.35 2.7,128.82 2.7,128.82l7.49 -0.15 0 25.02 32.95 0 0 27.56c0,0 253.15,-39.85 361,-45.84 107.85,-5.99 594.37,37.15 617.14,58.72 22.77,21.57 35.95,77.89 -39.55,100.66 -75.5,22.77 -581.19,57.52 -639.91,46.74 -58.72,-10.79 -173.73,-77.7 -243.26,-76.69 -69.53,1.01 -56.62,35.95 -56.62,45.54l-22.78 -0.9 -0.16 18.87c0,0 83.07,-1.13 121.42,16.84 38.35,17.97 115.72,50.26 168.44,62.24 52.72,11.98 568.01,-14.38 636.31,-28.76 68.3,-14.38 115.48,-40.16 115.48,-40.16 0,0 16.34,115.66 47.5,189.95 31.16,74.3 86.28,147.39 109.05,164.17 22.77,16.78 156.98,45.54 196.52,59.92 39.55,14.38 110.24,58.72 149.79,86.28 39.55,27.56 112.97,44.34 172.89,44.34 59.92,0 126.57,-16.78 166.12,-44.34 39.55,-27.56 110.24,-71.9 149.79,-86.28 39.55,-14.38 173.76,-43.14 196.52,-59.92 22.77,-16.78 77.89,-89.88 109.05,-164.17 31.16,-74.3 47.5,-189.95 47.5,-189.95 0,0 47.17,25.78 115.48,40.16 68.3,14.38 583.59,40.74 636.31,28.76 52.72,-11.98 130.1,-44.27 168.44,-62.24 38.35,-17.97 121.42,-16.84 121.42,-16.84l-0.16 -18.87 -22.78 0.9c0,-9.59 12.91,-44.53 -56.62,-45.54 -69.53,-1.01 -184.54,65.91 -243.26,76.69 -58.72,10.79 -564.41,-23.97 -639.91,-46.74 -75.5,-22.77 -62.31,-79.09 -39.55,-100.66 22.77,-21.57 509.29,-64.71 617.14,-58.72 107.85,5.99 361,45.84 361,45.84l0 -27.56 32.95 0 0 -25.02 7.49 0.15c0,0 2.7,-93.47 2.7,-128.82 0,-35.35 35.12,-60.67 63.2,-61.1 28.08,-0.43 65.31,20.36 65.31,59.9 0,39.54 2.39,134.21 2.39,134.21l12.58 0 0 25.76 25.76 0 0 26.36c0,0 222.89,-37.15 329.54,-44.34 106.65,-7.19 626.73,39.54 645.9,56.32 19.17,16.78 46.74,68.3 -45.54,94.67 -92.27,26.36 -586.7,61.65 -644.7,41.94 -58,-19.71 -167.17,-77.89 -233.67,-77.89 -66.51,0 -53.93,38.65 -53.93,46.74l-22.77 0 -1.2 29.96c0,0 32.36,8.4 32.36,46.74 0,38.35 1.2,62.91 1.2,97.66 0,34.75 -58.72,77.88 -142.6,82.68 -83.88,4.79 -60.52,1.79 -198.32,7.79 -137.81,5.99 -294.79,74.3 -492.51,133.01 -197.72,58.72 -424.21,81.49 -424.21,81.49l105.6 464.95 154.43 0 0 105.45 -411.02 0 1.2 -105.45 134.66 0 -149.64 -380.47c0,0 -25.76,22.17 -147.39,68.91 -121.63,46.74 -341.62,47.93 -341.62,47.93z"/>
- </g>
-</svg>