Go to the documentation of this file.00001 #include "MarkerPublisher.h"
00002
00003 using namespace ros;
00004 using namespace visualization_msgs;
00005 using namespace aruco;
00006
00007 void MarkerPublisher::publishMarker(std_msgs::Header imageHeader, aruco::Marker marker, btVector3 position, btQuaternion orientation)
00008 {
00009 visualization_msgs::Marker visualizationMarker;
00010
00011 visualizationMarker.header = imageHeader;
00012
00013 visualizationMarker.ns = "aruco_pose";
00014 visualizationMarker.id = marker.id;
00015 visualizationMarker.type = visualization_msgs::Marker::CUBE;
00016 visualizationMarker.action = visualization_msgs::Marker::ADD;
00017
00018 visualizationMarker.pose.position.x = position.x();
00019 visualizationMarker.pose.position.y = position.y();
00020 visualizationMarker.pose.position.z = position.z();
00021
00022 visualizationMarker.pose.orientation.x = orientation.x();
00023 visualizationMarker.pose.orientation.y = orientation.y();
00024 visualizationMarker.pose.orientation.z = orientation.z();
00025 visualizationMarker.pose.orientation.w = orientation.w();
00026
00027 visualizationMarker.scale.x = 1.00 * markerSize;
00028 visualizationMarker.scale.y = 1.00 * markerSize;
00029 visualizationMarker.scale.z = 0.25 * markerSize;
00030
00031 visualizationMarker.color.r = 0.0;
00032 visualizationMarker.color.g = 0.0;
00033 visualizationMarker.color.b = 1.0;
00034 visualizationMarker.color.a = 1.0;
00035
00036 visualizationMarker.lifetime = Duration(30.0);
00037
00038 visualizationPublisher.publish(visualizationMarker);
00039 }