From 4a25fed9d5faa81ed3cb7a9db31dfe3fbf35bb47 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 7 Sep 2018 22:21:26 +0300
Subject: [PATCH] Add camera visualisation markers node for aligning frame with
 rviz

---
 .editorconfig                    |  2 +-
 clever/CMakeLists.txt            |  4 ++
 clever/launch/main_camera.launch |  9 +++
 clever/src/camera_markers.cpp    | 95 ++++++++++++++++++++++++++++++++
 4 files changed, 109 insertions(+), 1 deletion(-)
 create mode 100644 clever/src/camera_markers.cpp

diff --git a/.editorconfig b/.editorconfig
index da7471f2..f55e3aa5 100644
--- a/.editorconfig
+++ b/.editorconfig
@@ -5,7 +5,7 @@ end_of_line = lf
 insert_final_newline = true
 charset = utf-8
 
-[*.{py,cpp,h,swift,launch}]
+[*.{py,swift,launch}]
 indent_style = space
 indent_size = 4
 
diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt
index 1f1223cd..b49fcbfa 100644
--- a/clever/CMakeLists.txt
+++ b/clever/CMakeLists.txt
@@ -156,8 +156,12 @@ add_library(aruco_vpe
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(rc src/rc.cpp)
 
+add_executable(camera_markers src/camera_markers.cpp)
+
 target_link_libraries(rc ${catkin_LIBRARIES})
 
+target_link_libraries(camera_markers ${catkin_LIBRARIES})
+
 ## 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
diff --git a/clever/launch/main_camera.launch b/clever/launch/main_camera.launch
index 480ba38f..19a865f1 100644
--- a/clever/launch/main_camera.launch
+++ b/clever/launch/main_camera.launch
@@ -1,4 +1,7 @@
 <launch>
+    <!-- Camera position and orientation are represented by fcu -> main_camera_optical transform -->
+    <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
+
     <!-- clever 2 -->
     <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0 0 -0.07 -1.5707963 0 3.1415926 fcu main_camera_optical"/>-->
 
@@ -20,4 +23,10 @@
         <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="frame_id" value="main_camera_optical"/>
+        <param name="scale" value="3.0"/>
+    </node>
 </launch>
diff --git a/clever/src/camera_markers.cpp b/clever/src/camera_markers.cpp
new file mode 100644
index 00000000..a55b36a2
--- /dev/null
+++ b/clever/src/camera_markers.cpp
@@ -0,0 +1,95 @@
+/*
+ * Visualization marker for camera alignment
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Licensed under the MIT License. See LICENSE.md in the project root for license information.
+ */
+
+#include <string>
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+using namespace visualization_msgs;
+
+double markers_scale;
+std::string camera_frame;
+
+MarkerArray createMarkers() {
+	MarkerArray markers;
+
+	Marker lens;
+	lens.header.frame_id = camera_frame;
+	lens.ns = "camera_markers";
+	lens.id = 0;
+	lens.action = Marker::ADD;
+	lens.type = visualization_msgs::Marker::CYLINDER;
+	lens.frame_locked = true;
+	lens.scale.x = 0.013 * markers_scale;
+	lens.scale.y = 0.013 * markers_scale;
+	lens.scale.z = 0.015 * markers_scale;
+	lens.color.r = 0.3;
+	lens.color.g = 0.3;
+	lens.color.b = 0.3;
+	lens.color.a = 0.9;
+	lens.pose.position.z = 0.0075 * markers_scale;
+	lens.pose.orientation.w = 1;
+
+	Marker board;
+	board.header.frame_id = camera_frame;
+	board.ns = "camera_markers";
+	board.id = 1;
+	board.action = Marker::ADD;
+	board.type = Marker::CUBE;
+	board.frame_locked = true;
+	board.scale.x = 0.024 * markers_scale;
+	board.scale.y = 0.024 * markers_scale;
+	board.scale.z = 0.001 * markers_scale;
+	board.color.r = 0.0;
+	board.color.g = 0.8;
+	board.color.b = 0.0;
+	board.color.a = 0.9;
+	board.pose.orientation.w = 1;
+
+	Marker wire;
+	wire.header.frame_id = camera_frame;
+	wire.ns = "camera_markers";
+	wire.id = 2;
+	wire.action = Marker::ADD;
+	wire.type = Marker::CUBE;
+	wire.frame_locked = true;
+	wire.scale.x = 0.014 * markers_scale;
+	wire.scale.y = 0.04 * markers_scale;
+	wire.scale.z = 0.001 * markers_scale;
+	wire.color.r = 0.9;
+	wire.color.g = 0.9;
+	wire.color.b = 1.0;
+	wire.color.a = 0.8;
+	wire.pose.position.x = 0;
+	wire.pose.position.y = (0.01 + 0.02) * markers_scale;
+	wire.pose.position.z = 0.002 * markers_scale;
+	wire.pose.orientation.w = 1;
+
+	markers.markers.push_back(lens);
+	markers.markers.push_back(board);
+	markers.markers.push_back(wire);
+
+	return markers;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "camera_markers");
+	ros::NodeHandle nh, nh_priv("~");
+
+	nh_priv.param<std::string>("frame_id", camera_frame, "main_camera_optical");
+	nh_priv.param("scale", markers_scale, 1.0);
+
+	ros::Publisher markers_pub = nh.advertise<visualization_msgs::MarkerArray>("camera_markers", 1, true);
+	markers_pub.publish(createMarkers());
+
+	ROS_INFO("Camera markers initialized");
+	ros::spin();
+}