wrench_visual.cpp
Go to the documentation of this file.
00001 #include <OGRE/OgreVector3.h>
00002 #include <OGRE/OgreSceneNode.h>
00003 #include <OGRE/OgreSceneManager.h>
00004 
00005 #include <rviz/ogre_helpers/arrow.h>
00006 #include <rviz/ogre_helpers/billboard_line.h>
00007 
00008 #include <ros/ros.h>
00009 
00010 #include "wrench_visual.h"
00011 
00012 namespace jsk_rviz_plugin
00013 {
00014 
00015     WrenchStampedVisual::WrenchStampedVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node )
00016     {
00017         scene_manager_ = scene_manager;
00018 
00019         // Ogre::SceneNode s form a tree, with each node storing the
00020         // transform (position and orientation) of itself relative to its
00021         // parent.  Ogre does the math of combining those transforms when it
00022         // is time to render.
00023         //
00024         // Here we create a node to store the pose of the WrenchStamped's header frame
00025         // relative to the RViz fixed frame.
00026         frame_node_ = parent_node->createChildSceneNode();
00027 
00028         // We create the arrow object within the frame node so that we can
00029         // set its position and direction relative to its header frame.
00030         arrow_force_ = new rviz::Arrow( scene_manager_, frame_node_ );
00031         arrow_torque_ = new rviz::Arrow( scene_manager_, frame_node_ );
00032         circle_torque_ = new rviz::BillboardLine( scene_manager_, frame_node_ );
00033         circle_arrow_torque_ = new rviz::Arrow( scene_manager_, frame_node_ );
00034     }
00035 
00036     WrenchStampedVisual::~WrenchStampedVisual()
00037     {
00038         // Delete the arrow to make it disappear.
00039         delete arrow_force_;
00040         delete arrow_torque_;
00041         delete circle_torque_;
00042         delete circle_arrow_torque_;
00043 
00044         // Destroy the frame node since we don't need it anymore.
00045         scene_manager_->destroySceneNode( frame_node_ );
00046     }
00047 
00048 
00049     void WrenchStampedVisual::setMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg )
00050     {
00051         Ogre::Vector3 force(msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z);
00052         Ogre::Vector3 torque(msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z);
00053         double force_length = force.length() * 0.001 * scale_;
00054         double torque_length = torque.length() * 0.001 * scale_;
00055         arrow_force_->setScale(Ogre::Vector3(width_, width_, force_length)); 
00056         arrow_torque_->setScale(Ogre::Vector3(width_, width_, torque_length));
00057 
00058         arrow_force_->setDirection(force);
00059         arrow_torque_->setDirection(torque);
00060         Ogre::Vector3 axis_z(0,0,1);
00061         Ogre::Quaternion orientation(axis_z.angleBetween(torque), axis_z.crossProduct(torque.normalisedCopy()));
00062         if ( std::isnan(orientation.x) ||
00063              std::isnan(orientation.y) ||
00064              std::isnan(orientation.z) ) orientation = Ogre::Quaternion::IDENTITY;
00065         //circle_arrow_torque_->setScale(Ogre::Vector3(width_, width_, 0.05));
00066         circle_arrow_torque_->set(0, width_*0.1, width_*0.1*1.0, width_*0.1*2.0);
00067         circle_arrow_torque_->setDirection(orientation * Ogre::Vector3(0,1,0));
00068         circle_arrow_torque_->setPosition(orientation * Ogre::Vector3(0.05+torque_length/4, 0, torque_length/2));
00069         circle_torque_->clear();
00070         circle_torque_->setLineWidth(width_*0.05);
00071         for (int i = 4; i <= 32; i++) {
00072             Ogre::Vector3 point = Ogre::Vector3((0.05+torque_length/4)*cos(i*2*M_PI/32),
00073                                                 (0.05+torque_length/4)*sin(i*2*M_PI/32),
00074                                                 torque_length/2);
00075             circle_torque_->addPoint(orientation * point);
00076         }
00077     }
00078 
00079     // Position and orientation are passed through to the SceneNode.
00080     void WrenchStampedVisual::setFramePosition( const Ogre::Vector3& position )
00081     {
00082         frame_node_->setPosition( position );
00083     }
00084 
00085     void WrenchStampedVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
00086     {
00087         frame_node_->setOrientation( orientation );
00088     }
00089 
00090     // Color is passed through to the rviz object.
00091     void WrenchStampedVisual::setForceColor( float r, float g, float b, float a )
00092     {
00093         arrow_force_->setColor( r, g, b, a );
00094     }
00095     // Color is passed through to the rviz object.
00096     void WrenchStampedVisual::setTorqueColor( float r, float g, float b, float a )
00097     {
00098         arrow_torque_->setColor( r, g, b, a );
00099         circle_torque_->setColor( r, g, b, a );
00100         circle_arrow_torque_->setColor( r, g, b, a );
00101     }
00102 
00103     void  WrenchStampedVisual::setScale( float s ) {
00104       scale_ = s;
00105     }
00106     void  WrenchStampedVisual::setWidth( float w ) {
00107       width_ = w;
00108     }
00109 
00110 } // end namespace jsk_rviz_plugin
00111 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


jsk_rviz_plugins
Author(s): Kei Okada
autogenerated on Sat Mar 23 2013 20:30:29