CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-27 05:29: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

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