30 #include <OgreManualObject.h> 31 #include <OgreSceneManager.h> 32 #include <OgreSceneNode.h> 49 : manual_object_(
NULL )
52 length_property_ =
new rviz::FloatProperty(
"Arrow Length", 0.3,
"Length of the arrows.",
this );
53 axes_length_property_ =
new rviz::FloatProperty(
"Axes Length", 1,
"Length of each axis, in meters.",
54 this, SLOT( updateAxisGeometry() ));
55 axes_radius_property_ =
new rviz::FloatProperty(
"Axes Radius", 0.1,
"Radius of each axis, in meters.",
56 this, SLOT( updateAxisGeometry() ));
57 shape_property_ =
new rviz::EnumProperty(
"Shape",
"Arrow",
"Shape to display the pose as.",
58 this, SLOT( updateShapeChoice() ));
59 shape_property_->addOption(
"Arrow", Arrow );
60 shape_property_->addOption(
"Axes", Axes );
73 #if ROS_VERSION_MINIMUM(1,12,0) 74 ROS_WARN(
"jsk_rviz_plugins/PoseArrayDisplay is deprecated. Please use rviz default PoseArrayDisplay plugin instead.");
78 manual_object_->setDynamic(
true );
82 updateShapeVisibility();
88 bool use_arrow = ( shape_property_->getOptionInt() == Arrow );
91 length_property_->setHidden( !use_arrow );
93 axes_length_property_->setHidden( use_arrow );
94 axes_radius_property_->setHidden( use_arrow );
96 updateShapeVisibility();
101 void PoseArrayDisplay::updateShapeVisibility()
106 manual_object_->setVisible(
false);
107 for (
int i = 0; i < coords_nodes_.size() ; i++)
112 bool use_arrow = (shape_property_->getOptionInt() ==
Arrow);
113 for (
int i = 0; i < coords_nodes_.size() ; i++)
116 manual_object_->setVisible(use_arrow);
120 void PoseArrayDisplay::updateAxisGeometry()
122 for(
size_t i = 0; i < coords_objects_.size() ; i++)
123 coords_objects_[i]->
set( axes_length_property_->getFloat(),
124 axes_radius_property_->getFloat() );
128 void PoseArrayDisplay::allocateCoords(
int num)
130 if (num > coords_objects_.size()) {
131 for (
size_t i = coords_objects_.size(); i < num; i++) {
132 Ogre::SceneNode* scene_node =
scene_node_->createChildSceneNode();
134 axes_length_property_->getFloat(),
135 axes_radius_property_->getFloat());
136 coords_nodes_.push_back(scene_node);
137 coords_objects_.push_back(axes);
140 else if (num < coords_objects_.size()) {
141 for (
int i = coords_objects_.size() - 1; num <= i; i--) {
142 delete coords_objects_[i];
145 coords_objects_.resize(num);
146 coords_nodes_.resize(num);
158 #if ROS_VERSION_MINIMUM(1,12,0) 159 ROS_WARN_THROTTLE(1.0,
"jsk_rviz_plugins/PoseArrayDisplay is deprecated. Please use rviz default PoseArrayDisplay plugin instead.");
167 manual_object_->clear();
169 Ogre::Vector3 position;
170 Ogre::Quaternion orientation;
173 ROS_DEBUG(
"Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable(
fixed_frame_ ));
177 updateShapeVisibility();
182 manual_object_->clear();
184 if(shape_property_->getOptionInt() ==
Arrow ) {
185 for (
int i = 0; i < coords_nodes_.size() ; i++)
188 float length = length_property_->getFloat();
189 size_t num_poses = msg->poses.size();
190 manual_object_->estimateVertexCount( num_poses * 6 );
191 manual_object_->begin(
"BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
192 for(
size_t i=0; i < num_poses; ++i )
194 Ogre::Vector3
pos( msg->poses[i].position.x,
195 msg->poses[i].position.y,
196 msg->poses[i].position.z );
197 Ogre::Quaternion orient( msg->poses[i].orientation.w,
198 msg->poses[i].orientation.x,
199 msg->poses[i].orientation.y,
200 msg->poses[i].orientation.z );
204 Ogre::Vector3 vertices[6];
206 vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 );
207 vertices[2] = vertices[ 1 ];
208 vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 );
209 vertices[4] = vertices[ 1 ];
210 vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 );
212 for(
int i = 0; i < 6; ++i )
214 manual_object_->position( vertices[i] );
215 manual_object_->colour( color );
218 manual_object_->end();
221 allocateCoords(msg->poses.size());
222 for (
int i = 0; i < msg->poses.size() ; i++){
223 geometry_msgs::Pose
pose = msg->poses[i];
224 Ogre::SceneNode* scene_node = coords_nodes_[i];
225 scene_node->setVisible(
true);
227 Ogre::Vector3 position( msg->poses[i].position.x,
228 msg->poses[i].position.y,
229 msg->poses[i].position.z );
230 Ogre::Quaternion orientation( msg->poses[i].orientation.w,
231 msg->poses[i].orientation.x,
232 msg->poses[i].orientation.y,
233 msg->poses[i].orientation.z );
234 scene_node->setPosition(position);
235 scene_node->setOrientation(orientation);
247 manual_object_->clear();
249 if ( coords_objects_.size() > 0 ) {
rviz::ColorProperty * color_property_
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
Ogre::ColourValue getOgreColor() const
Ogre::SceneNode * scene_node_
void setVisible(PanelDockWidget *widget, bool visible)
void onInitialize() override
#define ROS_WARN_THROTTLE(period,...)
bool validateFloats(const sensor_msgs::CameraInfo &msg)
~PoseArrayDisplay() override
void processMessage(const geometry_msgs::PoseArray::ConstPtr &msg) override
virtual FrameManager * getFrameManager() const=0
bool validateFloats(const visualization_msgs::InteractiveMarker &msg)
Ogre::SceneManager * scene_manager_
virtual void setHidden(bool hidden)
virtual void queueRender()=0
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void onInitialize() override
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)