MarkerPublisher.cpp
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 }