34 #include <OGRE/OgreVector3.h> 35 #include <OGRE/OgreMatrix3.h> 36 #include <OGRE/OgreSceneNode.h> 37 #include <OGRE/OgreSceneManager.h> 59 _markers.resize(msg->markers.size());
61 for (
size_t i = 0; i < msg->markers.size(); i++) {
62 marker_msgs::MarkerWithCovariance marker_cov = msg->markers[i];
63 marker_msgs::Marker
marker = marker_cov.marker;
65 double p_x = marker.pose.position.x;
66 double p_y = marker.pose.position.y;
67 double p_z = marker.pose.position.z;
68 double o_x = marker.pose.orientation.x;
69 double o_y = marker.pose.orientation.y;
70 double o_z = marker.pose.orientation.z;
71 double o_w = marker.pose.orientation.w;
74 if (marker.ids.size() > 0)
79 m->setOrientation(Ogre::Quaternion(o_w, o_x, o_y, o_z));
84 m->setCovarianceMatrix(msg->markers[i].covariance);
98 for (
size_t i = 0; i <
_markers.size(); i++) {
106 for (
size_t i = 0; i <
_markers.size(); i++) {
107 _markers[i]->setShowMarker(showMarker);
114 for (
size_t i = 0; i <
_markers.size(); i++) {
115 _markers[i]->setShowLabel(showLabel);
122 for (
size_t i = 0; i <
_markers.size(); i++) {
123 _markers[i]->setScale(Ogre::Vector3(scale, scale, scale));
void setFramePosition(const Ogre::Vector3 &position)
void setShowLabel(bool showLabel)
Ogre::SceneNode * frame_node_
void setFrameOrientation(const Ogre::Quaternion &orientation)
void setShowAxes(bool showAxes)
std::vector< boost::shared_ptr< Marker > > _markers
void setShowMarker(bool showMarker)
void setScale(float scale)
Ogre::SceneManager * scene_manager_
virtual ~MarkerWithCovarianceArrayVisual()
MarkerWithCovarianceArrayVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node)
virtual void setPosition(const Ogre::Vector3 &position)
void setMessage(const marker_msgs::MarkerWithCovarianceArray::ConstPtr &msg)