From c16ba87d10453bd967c347ea3373b79fb0d564b2 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Mon, 8 Jul 2019 17:03:08 +0300
Subject: [PATCH] builder: Add ability to move custom files from clever-config
 directory to clever image

---
 builder/assets/animation_map.txt              | 25 -------
 builder/clever-config/launch/aruco.launch     | 37 ++++++++++
 builder/clever-config/launch/clever.launch    | 71 +++++++++++++++++++
 .../clever-config/launch/main_camera.launch   | 37 ++++++++++
 builder/clever-config/map/animation_map.txt   |  8 +++
 builder/image-build.sh                        |  7 +-
 builder/image-configure.sh                    | 10 +--
 7 files changed, 160 insertions(+), 35 deletions(-)
 delete mode 100644 builder/assets/animation_map.txt
 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/builder/assets/animation_map.txt b/builder/assets/animation_map.txt
deleted file mode 100644
index 8e04a74..0000000
--- a/builder/assets/animation_map.txt
+++ /dev/null
@@ -1,25 +0,0 @@
-0	0.3375	0.0	4.6	0	0	0	0	
-1	0.3375	1.15	4.6	0	0	0	0	
-2	0.3375	2.3	4.6	0	0	0	0	
-3	0.3375	3.45	4.6	0	0	0	0	
-4	0.3375	4.6	4.6	0	0	0	0	
-5	0.3375	0.0	3.45	0	0	0	0	
-6	0.3375	1.15	3.45	0	0	0	0	
-7	0.3375	2.3	3.45	0	0	0	0	
-8	0.3375	3.45	3.45	0	0	0	0	
-9	0.3375	4.6	3.45	0	0	0	0	
-10	0.3375	0.0	2.3	0	0	0	0	
-11	0.3375	1.15	2.3	0	0	0	0	
-12	0.3375	2.3	2.3	0	0	0	0	
-13	0.3375	3.45	2.3	0	0	0	0	
-14	0.3375	4.6	2.3	0	0	0	0	
-15	0.3375	0.0	1.15	0	0	0	0	
-16	0.3375	1.15	1.15	0	0	0	0	
-17	0.3375	2.3	1.15	0	0	0	0	
-18	0.3375	3.45	1.15	0	0	0	0	
-19	0.3375	4.6	1.15	0	0	0	0	
-20	0.3375	0.0	0.0	0	0	0	0	
-21	0.3375	1.15	0.0	0	0	0	0	
-22	0.3375	2.3	0.0	0	0	0	0	
-23	0.3375	3.45	0.0	0	0	0	0	
-24	0.3375	4.6	0.0	0	0	0	0	
diff --git a/builder/clever-config/launch/aruco.launch b/builder/clever-config/launch/aruco.launch
new file mode 100644
index 0000000..d7754d9
--- /dev/null
+++ b/builder/clever-config/launch/aruco.launch
@@ -0,0 +1,37 @@
+<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.copterexpress.com/aruco.html -->
+
+    <!-- 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"/>
+        <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+        <param name="known_tilt" value="map"/>
+        <param name="length" value="0.33"/>
+    </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"/>
+        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
+        <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
+    </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..1020ea2
--- /dev/null
+++ b/builder/clever-config/launch/clever.launch
@@ -0,0 +1,71 @@
+<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="true"/>
+    <arg name="rc" default="true"/>
+    <arg name="rangefinder_vl53l1x" default="true"/>
+
+    <!-- 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"/>
+    </node>
+
+    <!-- Auxiliary frames -->
+    <node name="frames" pkg="clever" type="frames" output="screen">
+        <param name="body/frame_id" value="body"/>
+    </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="vl53l1x" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
+        <param name="frame_id" value="rangefinder"/>
+        <remap from="~range" to="mavros/distance_sensor/rangefinder_sub"/> <!-- redirect data to FCU -->
+    </node>
+
+    <!-- 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..92bec22
--- /dev/null
+++ b/builder/clever-config/launch/main_camera.launch
@@ -0,0 +1,37 @@
+<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.copterexpress.com/camera_frame.html -->
+
+    <!-- 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/fisheye_cam_320.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 -->
+
+        <!-- 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
new file mode 100644
index 0000000..9a54274
--- /dev/null
+++ b/builder/clever-config/map/animation_map.txt
@@ -0,0 +1,8 @@
+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
+104	0.33	0.77	0.77	0	0	0	0
+103	0.33	0	1.54	0	0	0	0
+102	0.33	0.77	1.54	0	0	0	0
+101	0.33	0	2.31	0	0	0	0
+100	0.33	0.77	2.31	0	0	0	0
diff --git a/builder/image-build.sh b/builder/image-build.sh
index 5432a5f..e2f70d5 100755
--- a/builder/image-build.sh
+++ b/builder/image-build.sh
@@ -30,10 +30,12 @@ echo_stamp() {
 
 REPO_DIR="/mnt"
 SCRIPTS_DIR="${REPO_DIR}/builder"
+CONFIG_DIR="${SCRIPTS_DIR}/clever-config"
 IMAGES_DIR="${REPO_DIR}/images"
 
 [[ ! -d ${SCRIPTS_DIR} ]] && (echo_stamp "Directory ${SCRIPTS_DIR} doesn't exist" "ERROR"; exit 1)
 [[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo_stamp "Directory ${IMAGES_DIR} was created successful" "SUCCESS"
+[[ ! -d ${CONFIG_DIR} ]] && mkdir ${CONFIG_DIR} && echo_stamp "Directory ${CONFIG_DIR} was created successful" "SUCCESS"
 
 if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="$(cd ${REPO_DIR}; git log --format=%h -1)"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
 # IMAGE_VERSION="${TRAVIS_TAG:=$(cd ${REPO_DIR}; git log --format=%h -1)}"
@@ -105,7 +107,10 @@ git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
 
 # Copy service file for clever show client
 img-chroot ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever-show.service' '/lib/systemd/system/'
-img-chroot ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/animation_map.txt' '/home/pi/catkin_ws/src/clever/aruco_pose/map/'
+
+# Copy config files for clever
+if [[ -d "${CONFIG_DIR}/launch" ]]; then img-chroot ${IMAGE_PATH} copy ${CONFIG_DIR}'/launch' '/home/pi/catkin_ws/src/clever/clever'; fi
+if [[ -d "${CONFIG_DIR}/map" ]]; then img-chroot ${IMAGE_PATH} copy ${CONFIG_DIR}'/map' '/home/pi/catkin_ws/src/clever/aruco_pose'; fi
 
 # Shrink image
 img-resize ${IMAGE_PATH}
diff --git a/builder/image-configure.sh b/builder/image-configure.sh
index 3ad8cf5..f7e0885 100755
--- a/builder/image-configure.sh
+++ b/builder/image-configure.sh
@@ -23,7 +23,7 @@ echo_stamp() {
 }
 
 # rename wifi ssid
-sed -i "s/NEW_SSID='CLEVER/NEW_SSID='CleverShow/" /root/init_rpi.sh
+sed -i "s/NEW_SSID='CLEVER/NEW_SSID='CLEVERSHOW/" /root/init_rpi.sh
 
 # add sudoers variables to make sudo works with ros (for led strip)
 grep -qxF 'Defaults        env_keep += "ROS_LOG_DIR"' /etc/sudoers || cat << EOT >> /etc/sudoers
@@ -38,13 +38,5 @@ Defaults        env_keep += "ROS_HOME"
 Defaults        env_keep += "ROS_LOG_DIR"
 EOT
 
-# configure aruco.launch and clever.launch (for positioning with aruco map)
-sed -i '/<arg name="aruco_map"/c \    <arg name="aruco_map" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/aruco.launch
-sed -i '/<arg name="aruco_vpe"/c \    <arg name="aruco_vpe" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/aruco.launch
-sed -i '/<param name="map"/c \        <param name="map" value="\$\(find aruco_pose\)/map/animation_map.txt"/>' /home/pi/catkin_ws/src/clever/clever/launch/aruco.launch
-sed -i '/<arg name="aruco"/c \    <arg name="aruco" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
-sed -i '/<arg name="rangefinder_vl53l1x"/c \    <arg name="rangefinder_vl53l1x" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
-sed -i '/<arg name="optical_flow"/c \    <arg name="optical_flow" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
-
 echo_stamp "Image was configured!" "SUCCESS"