CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-31 06:59:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add camera visualisation markers node for aligning frame with rviz

Browse Source
This commit is contained in:
Oleg Kalachev
2018-09-07 22:21:26 +03:00
parent 6fb4d43500
commit 4a25fed9d5
4 changed files with 109 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
clever/CMakeLists.txt
View File

@@ -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

9
clever/launch/main_camera.launch
View File

@@ -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>

95
clever/src/camera_markers.cpp Normal file
View File

@@ -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();
}