From 4647ee64f65a16954eb8d8b7b10228a512f45af5 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Mon, 21 Oct 2019 20:15:38 +0300
Subject: [PATCH] Update clever-config

---
 .gitignore                                    |  2 +-
 builder/clever-config/launch/aruco.launch     | 41 ++++++++++
 builder/clever-config/launch/clever.launch    | 74 +++++++++++++++++++
 .../clever-config/launch/main_camera.launch   | 38 ++++++++++
 builder/clever-config/map/animation_map.txt   | 10 +++
 5 files changed, 164 insertions(+), 1 deletion(-)
 create mode 100644 builder/clever-config/launch/aruco.launch
 create mode 100644 builder/clever-config/launch/clever.launch
 create mode 100644 builder/clever-config/launch/main_camera.launch
 create mode 100644 builder/clever-config/map/animation_map.txt

diff --git a/.gitignore b/.gitignore
index 8761dcb..addfab1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -111,7 +111,7 @@ Drone/client_logs
 images/
 .vscode/
 \.idea/
-builder/clever-config
+# builder/clever-config
 Drone/_copter_client_old_\.py
 
 Drone/test_cl\.py
diff --git a/builder/clever-config/launch/aruco.launch b/builder/clever-config/launch/aruco.launch
new file mode 100644
index 0000000..efb6a36
--- /dev/null
+++ b/builder/clever-config/launch/aruco.launch
@@ -0,0 +1,41 @@
+<launch>
+    <arg name="aruco_detect" default="true"/>
+    <arg name="aruco_map" default="true"/>
+    <arg name="aruco_vpe" default="true"/>
+
+    <!-- For additional help go to https://clever.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">
+        <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/markers" if="$(arg aruco_map)"/>
+        <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+        <param name="known_tilt" value="map_flipped"/>
+        <param name="length" value="0.36"/>
+    </node>
+
+    <!-- aruco_map: estimate aruco map pose -->
+    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="map" value="$(find aruco_pose)/map/animation_map.txt"/>
+        <param name="known_tilt" value="map_flipped"/>
+        <param name="image_axis" value="true"/>
+        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
+        <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
+        <param name="markers/frame_id" value="aruco_map"/>
+        <param name="markers/child_frame_id_prefix" value="aruco_"/>
+    </node>
+
+    <!-- vpe publisher from aruco markers -->
+    <node name="vpe_publisher" pkg="clever" 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"/>
+        <param name="publish_zero" value="true"/>
+        <param name="offset_frame_id" value="aruco_map"/>
+    </node>
+</launch>
diff --git a/builder/clever-config/launch/clever.launch b/builder/clever-config/launch/clever.launch
new file mode 100644
index 0000000..101f15d
--- /dev/null
+++ b/builder/clever-config/launch/clever.launch
@@ -0,0 +1,74 @@
+<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="false"/>
+    <arg name="aruco" default="true"/>
+    <arg name="rc" default="false"/>
+    <arg name="rangefinder_vl53l1x" default="true"/>
+    <arg name="led" default="false"/>
+
+    <!-- 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"/>
+        <param name="nav_from_sp" value="false"/>
+    </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
new file mode 100644
index 0000000..256f8f7
--- /dev/null
+++ b/builder/clever-config/launch/main_camera.launch
@@ -0,0 +1,38 @@
+<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="20"/> <!-- 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="640"/>
+        <param name="image_height" value="480"/>
+    </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
new file mode 100644
index 0000000..83b7a17
--- /dev/null
+++ b/builder/clever-config/map/animation_map.txt
@@ -0,0 +1,10 @@
+130 0.36 0 0 0 0 0 0
+127 0.36 0 2.60 0 0 0 0
+112 0.36 1.24 0 0 0 0 0
+42 0.36 1.24 2.60 0 0 0 0
+52 0.36 2.48 0 0 0 0 0
+80 0.36 2.48 2.60 0 0 0 0
+48 0.36 3.72 0 -0.24 0 0 0
+87 0.36 3.72 2.60 -0.24 0 0 0
+113 0.36 4.96 0 -0.24 0 0 0
+69 0.36 4.96 2.60 -0.24 0 0 0