From 9b8f8eeabfc3a8387228d155ee06ea497c0bf523 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Tue, 9 Jun 2020 21:37:13 +0300
Subject: [PATCH] builder: Update clover configuration

---
 builder/clever-config/launch/clever.launch    |  73 -------------
 .../clever-config/launch/main_camera.launch   |  38 -------
 builder/clever-config/map/animation_map.txt   | 100 ------------------
 .../camera_info/calibration.yaml              |  22 ++--
 .../launch/aruco.launch                       |  11 +-
 builder/clover-config/launch/led.launch       |  38 +++++++
 .../clover-config/launch/main_camera.launch   |  37 +++++++
 builder/clover-config/map/animation_map.txt   |   5 +
 8 files changed, 98 insertions(+), 226 deletions(-)
 delete mode 100644 builder/clever-config/launch/clever.launch
 delete mode 100644 builder/clever-config/launch/main_camera.launch
 delete mode 100644 builder/clever-config/map/animation_map.txt
 rename builder/{clever-config => clover-config}/camera_info/calibration.yaml (71%)
 rename builder/{clever-config => clover-config}/launch/aruco.launch (81%)
 create mode 100644 builder/clover-config/launch/led.launch
 create mode 100644 builder/clover-config/launch/main_camera.launch
 create mode 100644 builder/clover-config/map/animation_map.txt

diff --git a/builder/clever-config/launch/clever.launch b/builder/clever-config/launch/clever.launch
deleted file mode 100644
index 97ad8de..0000000
--- a/builder/clever-config/launch/clever.launch
+++ /dev/null
@@ -1,73 +0,0 @@
-<launch>
-    <arg name="fcu_conn" default="usb"/>
-    <arg name="fcu_ip" default="127.0.0.1"/>
-    <arg name="gcs_bridge" default="tcp"/>
-    <arg name="web_video_server" default="true"/>
-    <arg name="rosbridge" default="true"/>
-    <arg name="main_camera" default="true"/>
-    <arg name="optical_flow" default="true"/>
-    <arg name="aruco" default="false"/>
-    <arg name="rangefinder_vl53l1x" default="true"/>
-    <arg name="led" default="false"/>
-    <arg name="rc" default="true"/>
-
-    <!-- log formatting -->
-    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
-
-    <!-- mavros -->
-    <include file="$(find clever)/launch/mavros.launch">
-        <arg name="fcu_conn" value="$(arg fcu_conn)"/>
-        <arg name="fcu_ip" value="$(arg fcu_ip)"/>
-        <arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
-    </include>
-
-    <!-- web video server -->
-    <node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5">
-        <param name="default_stream_type" value="ros_compressed"/>
-        <param name="publish_rate" value="1.0"/>
-    </node>
-
-    <!-- aruco markers -->
-    <include file="$(find clever)/launch/aruco.launch" if="$(arg aruco)"/>
-
-    <!-- optical flow -->
-    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clever/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
-        <remap from="image_raw" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <param name="calc_flow_gyro" value="true"/>
-    </node>
-
-    <!-- main nodelet manager -->
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
-        <param name="num_worker_threads" value="2"/>
-    </node>
-
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
-
-    <!-- simplified offboard control -->
-    <node name="simple_offboard" pkg="clever" type="simple_offboard" output="screen" clear_params="true">
-        <param name="reference_frames/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-        <param name="reference_frames/navigate_target" value="map"/>
-    </node>
-
-    <!-- main camera -->
-    <include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>
-
-    <!-- rosbridge -->
-    <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
-
-    <!-- tf2 republisher for web visualization -->
-    <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" output="screen" if="$(arg rosbridge)"/>
-
-    <!-- vl53l1x ToF rangefinder -->
-    <node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
-        <param name="frame_id" value="rangefinder"/>
-    </node>
-
-    <!-- led strip -->
-    <include file="$(find clever)/launch/led.launch" if="$(arg led)"/>
-
-    <!-- rc backend -->
-    <node name="rc" pkg="clever" type="rc" output="screen" if="$(arg rc)"/>
-</launch>
diff --git a/builder/clever-config/launch/main_camera.launch b/builder/clever-config/launch/main_camera.launch
deleted file mode 100644
index 76ab4bd..0000000
--- a/builder/clever-config/launch/main_camera.launch
+++ /dev/null
@@ -1,38 +0,0 @@
-<launch>
-    <!-- Camera position and orientation are represented by base_link -> main_camera_optical transform -->
-    <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
-
-    <!-- article about camera setup: https://clever.coex.tech/camera_frame -->
-
-    <!-- camera is oriented downward, camera cable goes backward [option 1] -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
-
-    <!-- camera is oriented downward, camera cable goes forward  [option 2] -->
-    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 base_link main_camera_optical"/>-->
-
-    <!-- camera is oriented upward, camera cable goes backward   [option 3] -->
-    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>-->
-
-    <!-- camera is oriented upward, camera cable goes forward    [option 4] -->
-    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 base_link main_camera_optical"/>-->
-
-    <!-- camera node -->
-    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true">
-        <param name="frame_id" value="main_camera_optical"/>
-        <param name="camera_info_url" value="file://$(find clever)/camera_info/calibration.yaml"/>
-
-        <param name="rate" value="100"/> <!-- poll rate -->
-        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
-        <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
-        <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
-
-        <!-- camera resolution, NOTE: camera_info file should match it -->
-        <param name="image_width" value="320"/>
-        <param name="image_height" value="240"/>
-    </node>
-
-    <!-- camera visualization markers -->
-    <node pkg="clever" type="camera_markers" ns="main_camera" name="main_camera_markers">
-        <param name="scale" value="3.0"/>
-    </node>
-</launch>
diff --git a/builder/clever-config/map/animation_map.txt b/builder/clever-config/map/animation_map.txt
deleted file mode 100644
index dfe273d..0000000
--- a/builder/clever-config/map/animation_map.txt
+++ /dev/null
@@ -1,100 +0,0 @@
-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	
-3	0.33	3.0	9.0	0	0	0	0	
-4	0.33	4.0	9.0	0	0	0	0	
-5	0.33	5.0	9.0	0	0	0	0	
-6	0.33	6.0	9.0	0	0	0	0	
-7	0.33	7.0	9.0	0	0	0	0	
-8	0.33	8.0	9.0	0	0	0	0	
-9	0.33	9.0	9.0	0	0	0	0	
-10	0.33	0.0	8.0	0	0	0	0	
-11	0.33	1.0	8.0	0	0	0	0	
-12	0.33	2.0	8.0	0	0	0	0	
-13	0.33	3.0	8.0	0	0	0	0	
-14	0.33	4.0	8.0	0	0	0	0	
-15	0.33	5.0	8.0	0	0	0	0	
-16	0.33	6.0	8.0	0	0	0	0	
-#17	0.33	7.0	8.0	0	0	0	0	
-18	0.33	8.0	8.0	0	0	0	0	
-19	0.33	9.0	8.0	0	0	0	0	
-20	0.33	0.0	7.0	0	0	0	0	
-21	0.33	1.0	7.0	0	0	0	0	
-22	0.33	2.0	7.0	0	0	0	0	
-23	0.33	3.0	7.0	0	0	0	0	
-24	0.33	4.0	7.0	0	0	0	0	
-25	0.33	5.0	7.0	0	0	0	0	
-26	0.33	6.0	7.0	0	0	0	0	
-27	0.33	7.0	7.0	0	0	0	0	
-28	0.33	8.0	7.0	0	0	0	0	
-29	0.33	9.0	7.0	0	0	0	0	
-30	0.33	0.0	6.0	0	0	0	0	
-31	0.33	1.0	6.0	0	0	0	0	
-32	0.33	2.0	6.0	0	0	0	0	
-33	0.33	3.0	6.0	0	0	0	0	
-34	0.33	4.0	6.0	0	0	0	0	
-35	0.33	5.0	6.0	0	0	0	0	
-36	0.33	6.0	6.0	0	0	0	0	
-37	0.33	7.0	6.0	0	0	0	0	
-38	0.33	8.0	6.0	0	0	0	0	
-39	0.33	9.0	6.0	0	0	0	0	
-40	0.33	0.0	5.0	0	0	0	0	
-41	0.33	1.0	5.0	0	0	0	0	
-42	0.33	2.0	5.0	0	0	0	0	
-43	0.33	3.0	5.0	0	0	0	0	
-44	0.33	4.0	5.0	0	0	0	0	
-45	0.33	5.0	5.0	0	0	0	0	
-46	0.33	6.0	5.0	0	0	0	0	
-47	0.33	7.0	5.0	0	0	0	0	
-48	0.33	8.0	5.0	0	0	0	0	
-49	0.33	9.0	5.0	0	0	0	0	
-50	0.33	0.0	4.0	0	0	0	0	
-51	0.33	1.0	4.0	0	0	0	0	
-52	0.33	2.0	4.0	0	0	0	0	
-53	0.33	3.0	4.0	0	0	0	0	
-54	0.33	4.0	4.0	0	0	0	0	
-55	0.33	5.0	4.0	0	0	0	0	
-56	0.33	6.0	4.0	0	0	0	0	
-57	0.33	7.0	4.0	0	0	0	0	
-58	0.33	8.0	4.0	0	0	0	0	
-59	0.33	9.0	4.0	0	0	0	0	
-60	0.33	0.0	3.0	0	0	0	0	
-61	0.33	1.0	3.0	0	0	0	0	
-62	0.33	2.0	3.0	0	0	0	0	
-63	0.33	3.0	3.0	0	0	0	0	
-64	0.33	4.0	3.0	0	0	0	0	
-65	0.33	5.0	3.0	0	0	0	0	
-66	0.33	6.0	3.0	0	0	0	0	
-67	0.33	7.0	3.0	0	0	0	0	
-68	0.33	8.0	3.0	0	0	0	0	
-69	0.33	9.0	3.0	0	0	0	0	
-70	0.33	0.0	2.0	0	0	0	0	
-71	0.33	1.0	2.0	0	0	0	0	
-72	0.33	2.0	2.0	0	0	0	0	
-73	0.33	3.0	2.0	0	0	0	0	
-74	0.33	4.0	2.0	0	0	0	0	
-75	0.33	5.0	2.0	0	0	0	0	
-76	0.33	6.0	2.0	0	0	0	0	
-77	0.33	7.0	2.0	0	0	0	0	
-78	0.33	8.0	2.0	0	0	0	0	
-79	0.33	9.0	2.0	0	0	0	0	
-80	0.33	0.0	1.0	0	0	0	0	
-81	0.33	1.0	1.0	0	0	0	0	
-82	0.33	2.0	1.0	0	0	0	0	
-83	0.33	3.0	1.0	0	0	0	0	
-84	0.33	4.0	1.0	0	0	0	0	
-85	0.33	5.0	1.0	0	0	0	0	
-86	0.33	6.0	1.0	0	0	0	0	
-87	0.33	7.0	1.0	0	0	0	0	
-88	0.33	8.0	1.0	0	0	0	0	
-89	0.33	9.0	1.0	0	0	0	0	
-90	0.33	0.0	0.0	0	0	0	0	
-91	0.33	1.0	0.0	0	0	0	0	
-92	0.33	2.0	0.0	0	0	0	0	
-93	0.33	3.0	0.0	0	0	0	0	
-94	0.33	4.0	0.0	0	0	0	0	
-95	0.33	5.0	0.0	0	0	0	0	
-96	0.33	6.0	0.0	0	0	0	0	
-97	0.33	7.0	0.0	0	0	0	0	
-98	0.33	8.0	0.0	0	0	0	0	
-99	0.33	9.0	0.0	0	0	0	0	
diff --git a/builder/clever-config/camera_info/calibration.yaml b/builder/clover-config/camera_info/calibration.yaml
similarity index 71%
rename from builder/clever-config/camera_info/calibration.yaml
rename to builder/clover-config/camera_info/calibration.yaml
index 76fcd8c..4d22e59 100644
--- a/builder/clever-config/camera_info/calibration.yaml
+++ b/builder/clover-config/camera_info/calibration.yaml
@@ -1,17 +1,17 @@
-image_width: 320
-image_height: 240
+image_width: 640
+image_height: 480
 distortion_model: plumb_bob
-camera_name: raspicam
+camera_name: main_camera_optical
 camera_matrix:
   rows: 3
   cols: 3
   data:
-  - 166.23942373073172
+  - 332.47884746146343
   - 0.
-  - 162.19011246829268
+  - 320.0
   - 0.
-  - 166.5880923974026
-  - 109.82227735714285
+  - 333.1761847948052
+  - 240.0
   - 0.
   - 0.
   - 1.
@@ -31,13 +31,13 @@ projection_matrix:
   rows: 3
   cols: 4
   data:
-  - 166.23942373073172
+  - 332.47884746146343
   - 0.
-  - 162.19011246829268
+  - 324.38022493658536
   - 0.
   - 0.
-  - 166.5880923974026
-  - 109.82227735714285
+  - 333.1761847948052
+  - 219.6445547142857
   - 0.
   - 0.
   - 0.
diff --git a/builder/clever-config/launch/aruco.launch b/builder/clover-config/launch/aruco.launch
similarity index 81%
rename from builder/clever-config/launch/aruco.launch
rename to builder/clover-config/launch/aruco.launch
index d1c6e05..6fde1bc 100644
--- a/builder/clever-config/launch/aruco.launch
+++ b/builder/clover-config/launch/aruco.launch
@@ -1,9 +1,9 @@
 <launch>
     <arg name="aruco_detect" default="true"/>
-    <arg name="aruco_map" default="true"/>
-    <arg name="aruco_vpe" default="true"/>
+    <arg name="aruco_map" default="false"/>
+    <arg name="aruco_vpe" default="false"/>
 
-    <!-- For additional help go to https://clever.coex.tech/aruco -->
+    <!-- For additional help go to https://clover.coex.tech/aruco -->
 
     <!-- aruco_detect: detect aruco markers, estimate poses -->
     <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
@@ -14,6 +14,9 @@
         <param name="send_tf" value="true"/>
         <param name="known_tilt" value="map"/>
         <param name="length" value="0.33"/>
+        <!-- aruco detector parameters -->
+        <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
+        <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
     </node>
 
     <!-- aruco_map: estimate aruco map pose -->
@@ -31,7 +34,7 @@
     </node>
 
     <!-- vpe publisher from aruco markers -->
-    <node name="vpe_publisher" pkg="clever" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
+    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
         <remap from="~pose_cov" to="aruco_map/pose"/>
         <remap from="~vpe" to="mavros/vision_pose/pose"/>
         <param name="frame_id" value="aruco_map_detected"/>
diff --git a/builder/clover-config/launch/led.launch b/builder/clover-config/launch/led.launch
new file mode 100644
index 0000000..17fc859
--- /dev/null
+++ b/builder/clover-config/launch/led.launch
@@ -0,0 +1,38 @@
+<launch>
+    <arg name="ws281x" default="true"/>
+    <arg name="led_effect" default="true"/>
+    <arg name="led_notify" default="false"/>
+
+    <!-- For additional help go to https://clover.coex.tech/led -->
+
+    <!-- ws281x led strip driver -->
+    <node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(arg ws281x)">
+        <param name="led_count" value="58"/>
+        <param name="gpio_pin" value="21"/>
+        <param name="brightness" value="64"/>
+        <param name="strip_type" value="WS2811_STRIP_GRB"/>
+        <param name="target_frequency" value="800000"/>
+        <param name="dma" value="10"/>
+        <param name="invert" value="false"/>
+    </node>
+
+    <!-- high level led effects control, events notification with leds -->
+    <node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" if="$(arg led_effect)">
+        <param name="blink_rate" value="2"/>
+        <param name="fade_period" value="0.5"/>
+        <param name="rainbow_period" value="5"/>
+        <!-- events effects table -->
+        <rosparam param="notify" if="$(arg led_notify)">
+            startup: { r: 255, g: 255, b: 255 }
+            connected: { effect: rainbow }
+            disconnected: { effect: blink, r: 255, g: 50, b: 50 }
+            acro: { r: 245, g: 155, b: 0 }
+            stabilized: { r: 30, g: 180, b: 50 }
+            altctl: { r: 255, g: 255, b: 40 }
+            posctl: { r: 50, g: 100, b: 220 }
+            offboard: { r: 220, g: 20, b: 250 }
+            low_battery: { threshold: 3.7, effect: blink_fast, r: 255, g: 0, b: 0 }
+            error: { effect: flash, r: 255, g: 0, b: 0 }
+        </rosparam>
+    </node>
+</launch>
diff --git a/builder/clover-config/launch/main_camera.launch b/builder/clover-config/launch/main_camera.launch
new file mode 100644
index 0000000..0d54454
--- /dev/null
+++ b/builder/clover-config/launch/main_camera.launch
@@ -0,0 +1,37 @@
+<launch>
+    <!-- article about camera setup: https://clover.coex.tech/camera_setup -->
+
+    <arg name="direction_z" default="down"/> <!-- direction the camera points: down, up -->
+    <arg name="direction_y" default="backward"/> <!-- direction the camera cable points: backward, forward -->
+
+    <node if="$(eval direction_z == 'down' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
+    <node if="$(eval direction_z == 'down' and direction_y == 'forward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 base_link main_camera_optical"/>
+    <node if="$(eval direction_z == 'up' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>
+    <node if="$(eval direction_z == 'up' and direction_y == 'forward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 base_link main_camera_optical"/>
+
+    <!-- Template for custom camera orientation -->
+    <!-- Camera position and orientation are represented by base_link -> main_camera_optical transform -->
+    <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
+    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/> -->
+
+    <!-- camera node -->
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true">
+        <param name="device_path" value="/dev/video0"/> <!-- v4l2 device -->
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="camera_info_url" value="file://$(find clover)/camera_info/calibration.yaml"/>
+
+        <param name="rate" value="100"/> <!-- poll rate -->
+        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
+        <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
+        <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
+
+        <!-- camera resolution -->
+        <param name="image_width" value="320"/>
+        <param name="image_height" value="240"/>
+    </node>
+
+    <!-- camera visualization markers -->
+    <node pkg="clover" type="camera_markers" ns="main_camera" name="main_camera_markers">
+        <param name="scale" value="3.0"/>
+    </node>
+</launch>
diff --git a/builder/clover-config/map/animation_map.txt b/builder/clover-config/map/animation_map.txt
new file mode 100644
index 0000000..970a77d
--- /dev/null
+++ b/builder/clover-config/map/animation_map.txt
@@ -0,0 +1,5 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
+1	0.33	0	0	0	0	0	0
+2	0.33	1	0	0	0	0	0
+3	0.33	0	1	0	0	0	0
+4	0.33	1	1	0	0	0	0