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 @@
+
+
+
@@ -20,4 +23,10 @@
+
+
+
+
+
+
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
+ *
+ * Licensed under the MIT License. See LICENSE.md in the project root for license information.
+ */
+
+#include
+#include
+#include
+#include
+
+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("frame_id", camera_frame, "main_camera_optical");
+ nh_priv.param("scale", markers_scale, 1.0);
+
+ ros::Publisher markers_pub = nh.advertise("camera_markers", 1, true);
+ markers_pub.publish(createMarkers());
+
+ ROS_INFO("Camera markers initialized");
+ ros::spin();
+}