00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "shape_marker.h"
00031 #include "marker_selection_handler.h"
00032 #include "rviz/default_plugin/marker_display.h"
00033
00034 #include "rviz/display_context.h"
00035 #include "rviz/selection/selection_manager.h"
00036
00037 #include <rviz/ogre_helpers/shape.h>
00038
00039 #include <OGRE/OgreSceneNode.h>
00040 #include <OGRE/OgreMatrix3.h>
00041
00042 namespace rviz
00043 {
00044
00045 ShapeMarker::ShapeMarker( MarkerDisplay* owner,
00046 DisplayContext* context,
00047 Ogre::SceneNode* parent_node )
00048 : MarkerBase( owner, context, parent_node )
00049 , shape_( 0 )
00050 {
00051 }
00052
00053 ShapeMarker::~ShapeMarker()
00054 {
00055 delete shape_;
00056 }
00057
00058 void ShapeMarker::onNewMessage( const MarkerConstPtr& old_message,
00059 const MarkerConstPtr& new_message )
00060 {
00061 if (!shape_ || old_message->type != new_message->type)
00062 {
00063 delete shape_;
00064 shape_ = 0;
00065
00066 switch (new_message->type)
00067 {
00068 case visualization_msgs::Marker::CUBE:
00069 {
00070 shape_ = new Shape(Shape::Cube,
00071 context_->getSceneManager(), scene_node_);
00072 }
00073 break;
00074
00075 case visualization_msgs::Marker::CYLINDER:
00076 {
00077 shape_ = new Shape(Shape::Cylinder,
00078 context_->getSceneManager(), scene_node_);
00079 }
00080 break;
00081
00082 case visualization_msgs::Marker::SPHERE:
00083 {
00084 shape_ = new Shape(Shape::Sphere,
00085 context_->getSceneManager(), scene_node_);
00086 }
00087 break;
00088
00089 default:
00090 ROS_BREAK();
00091 break;
00092 }
00093
00094 context_->getSelectionManager()->removeObject(coll_);
00095 coll_ = context_->getSelectionManager()->createCollisionForObject(
00096 shape_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID(
00097 new_message->ns, new_message->id))), coll_);
00098 }
00099
00100 Ogre::Vector3 pos, scale, scale_correct;
00101 Ogre::Quaternion orient;
00102 transform(new_message, pos, orient, scale);
00103
00104 if (owner_ && (new_message->scale.x * new_message->scale.y
00105 * new_message->scale.z == 0.0f))
00106 {
00107 owner_->setMarkerStatus(getID(), StatusProperty::Warn,
00108 "Scale of 0 in one of x/y/z");
00109 }
00110
00111 setPosition(pos);
00112 setOrientation( orient * Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) );
00113
00114 scale_correct = Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) * scale;
00115
00116 shape_->setScale(scale_correct);
00117
00118 shape_->setColor(new_message->color.r, new_message->color.g,
00119 new_message->color.b, new_message->color.a);
00120 }
00121
00122 S_MaterialPtr ShapeMarker::getMaterials()
00123 {
00124 S_MaterialPtr materials;
00125 extractMaterials(shape_->getEntity(), materials);
00126 return materials;
00127 }
00128
00129 }