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
00031 #ifndef JLO_DISPLAY_BASE_H
00032 #define JLO_DISPLAY_BASE_H
00033
00034 #include "rviz/display.h"
00035 #include "rviz/helpers/color.h"
00036 #include "rviz/properties/forwards.h"
00037 #include "rviz/visualization_manager.h"
00038 #include "rviz/frame_manager.h"
00039 #include "rviz/common.h"
00040
00041 #include <vision_msgs/cop_answer.h>
00042 #include <vision_srvs/srvjlo.h>
00043
00044 #include <tf/transform_listener.h>
00045
00046 #include <ogre_tools/axes.h>
00047 #include <ogre_tools/movable_text.h>
00048 #include <OGRE/OgreSceneNode.h>
00049 #include <OGRE/OgreSceneManager.h>
00050
00051 #include <list>
00052
00053 namespace Ogre
00054 {
00055 class SceneNode;
00056 }
00057
00058 namespace rviz_shows_cop
00059 {
00060
00065 class JloDisplayBase : public rviz::Display
00066 {
00067 public:
00068 struct JloDescription
00069 {
00070 unsigned long pose;
00071 std::string label;
00072 };
00073 JloDisplayBase(const std::string& name, rviz::VisualizationManager* manager);
00074
00075 virtual ~JloDisplayBase();
00076
00077 virtual void setTopic(const std::string& topic);
00078 const std::string& getTopic()
00079 {
00080 return (topic_);
00081 }
00082
00083 void setKeep(uint32_t keep);
00084 uint32_t getKeep() { return keep_; }
00085
00086 void setST( bool show_text );
00087 bool getST() { return show_text_; }
00088 void setSA( bool show_axis );
00089 bool getSA() { return show_axis_; }
00090 void setSC( bool show_cov);
00091 bool getSC() { return show_cov_; }
00092 void setAL(double axis_length);
00093 double getAL() { return axis_length_; }
00094 void setAT(double axis_thick);
00095 double getAT() { return axis_thickness_; }
00096 void setCL(double cov_length);
00097 double getCL() { return cov_length_; }
00098 void setCT(double cov_thick);
00099 double getCT() { return cov_thickness_; }
00100
00101 template<typename itT> std::list<std::pair<Ogre::SceneNode *, vision_msgs::partial_lo> >& displayJloSet(itT start, itT end, bool render);
00102
00103
00104 virtual void targetFrameChanged();
00105 virtual void fixedFrameChanged();
00106 virtual void createProperties();
00107 virtual void update(float wall_dt, float ros_dt);
00108 virtual void reset();
00109 virtual void processMessage()=0;
00110
00111 protected:
00112 void clear();
00113
00114 void popJloSet();
00115
00116
00117 virtual void onEnable();
00118 virtual void onDisable();
00119
00120 std::string topic_;
00121 uint32_t keep_;
00122 bool show_text_;
00123 bool show_axis_;
00124 bool show_cov_;
00125 double axis_length_;
00126 double axis_thickness_;
00127 double cov_length_;
00128 double cov_thickness_;
00129 bool m_binited;
00130 std::list< std::list< std::pair< Ogre::SceneNode *, vision_msgs::partial_lo> > > jlo_nodes_;
00131 Ogre::SceneNode* scene_node_;
00132
00133 rviz::ROSTopicStringPropertyWPtr topic_property_;
00134 rviz::IntPropertyWPtr keep_property_;
00135 rviz::CategoryPropertyWPtr extended_props_p_;
00136 rviz::BoolPropertyWPtr show_cov_p_;
00137 rviz::BoolPropertyWPtr show_axis_p_;
00138 rviz::BoolPropertyWPtr show_text_p_;
00139 rviz::FloatPropertyWPtr axis_thickness_p_;
00140 rviz::FloatPropertyWPtr axis_length_p_;
00141 rviz::FloatPropertyWPtr cov_length_p_;
00142 rviz::FloatPropertyWPtr cov_thickness_p_;
00143
00144 bool inited_jlo;
00145 ros::ServiceClient jlo_client;
00146 bool GetJlo(unsigned long id, unsigned long parent_id, vision_msgs::partial_lo &lo);
00147 unsigned long NameQueryJlo(std::string name);
00148
00149 template<typename T> void setSceneNodePose(Ogre::SceneNode* scene_node, T mat, Ogre::Quaternion &orientation);
00150 };
00151
00152
00153 template<typename itT>
00154 std::list<std::pair<Ogre::SceneNode *, vision_msgs::partial_lo> >& JloDisplayBase::displayJloSet(itT start, itT end, bool render=true)
00155 {
00156 if(m_binited)
00157 {
00158 Ogre::Quaternion orientation;
00159 const double matid[] = {1, 0, 0, 0,
00160 0, 1, 0, 0,
00161 0, 0, 1, 0,
00162 0, 0, 0, 1};
00163
00164 setSceneNodePose(scene_node_, matid, orientation);
00165
00166 jlo_nodes_.push_back(std::list<std::pair<Ogre::SceneNode *, vision_msgs::partial_lo> >());
00167 while(jlo_nodes_.size() > keep_)
00168 popJloSet();
00169
00170 for(itT it=start; it!=end; it++)
00171 {
00172 Ogre::SceneNode* node = scene_node_->createChildSceneNode();
00173
00174 vision_msgs::partial_lo lo;
00175 vision_msgs::partial_lo::_pose_type mat;
00176 vision_msgs::partial_lo::_cov_type cov;
00177
00178 if(it->pose == 1)
00179 {
00180 cov[ 0] = 0.0;
00181 cov[ 7] = 0.0;
00182 cov[14] = 0.0;
00183 cov[21] = 0.0;
00184 cov[28] = 0.0;
00185 cov[35] = 0.0;
00186 std::copy(&matid[0], &matid[16], mat.begin());
00187 lo.pose = mat;
00188 }
00189 else
00190 {
00191 unsigned long reference_frame_id = 1;
00192 reference_frame_id = NameQueryJlo(fixed_frame_);
00193 printf("NAmeQuery %s resolved to id: %ld\n", fixed_frame_.c_str(), reference_frame_id);
00194 if(!GetJlo(it->pose, reference_frame_id, lo))
00195 continue;
00196 mat = lo.pose;
00197 cov = lo.cov;
00198 }
00199 jlo_nodes_.back().push_back(std::pair<Ogre::SceneNode *, vision_msgs::partial_lo>(node, lo));
00200 setSceneNodePose(node , mat, orientation);
00201 if(show_axis_)
00202 {
00203 ogre_tools::Axes* axes_ = new ogre_tools::Axes( scene_manager_, node, axis_length_, axis_thickness_);
00204 axes_->getSceneNode()->setVisible( true);
00205 axes_->setOrientation(orientation);
00206 }
00207 Ogre::Matrix3 rotMat;
00208 orientation.ToRotationMatrix(rotMat);
00209 if(show_cov_)
00210 {
00211 for(int j = 0; j< 12; j++)
00212 {
00213 ogre_tools::Axes* axes_cov = new ogre_tools::Axes( scene_manager_, node, cov_length_, cov_thickness_ );
00214 Ogre::SceneNode* axis_tmp = axes_cov->getSceneNode();
00215
00216 Ogre::Vector3 vec( cov[0] * (j%6 == 0 ? 1 : 0) * (j>5?-1:1),
00217 cov[7] * (j%6 == 1 ? 1 : 0) * (j>5?-1:1),
00218 cov[14] * (j%6 == 2 ? 1 : 0) * (j>5?-1:1));
00219
00221 Ogre::Matrix3 rot (mat[0],mat[1],mat[2],
00222 mat[4],mat[5],mat[6],
00223 mat[8],mat[9],mat[10]);
00224
00225 Ogre::Vector3 vec_trans = rot * vec;
00226 axis_tmp->setPosition(Ogre::Vector3(-vec_trans.y, vec_trans.z, -vec_trans.x));
00227 Ogre::Matrix3 temp;
00228 temp.FromEulerAnglesZYX(Ogre::Radian(cov[21] * (j%6 == 3 ? 1 : 0) * (j>5?-1:1)),
00229 Ogre::Radian(cov[28] * (j%6 == 4 ? 1 : 0) * (j>5?-1:1)),
00230 Ogre::Radian(cov[35] * (j%6 == 5 ? 1 : 0) * (j>5?-1:1)));
00231 Ogre::Quaternion temp_quat;
00232 temp = rotMat * temp;
00233 temp_quat.FromRotationMatrix(temp);
00234 axes_cov->setOrientation(temp_quat);
00235 axis_tmp->setVisible( true);
00236 }
00237 }
00238 if(show_text_)
00239 {
00240 ogre_tools::MovableText* text = new ogre_tools::MovableText(it->label, "Arial",0.1, Ogre::ColourValue::White);
00241 Ogre::SceneNode *text_node= node->createChildSceneNode();
00242 text_node->attachObject(text);
00243 text_node->setVisible( true);
00244 }
00245 node->setVisible(true);
00246 }
00247 if(render)
00248 causeRender();
00249 }
00250 return jlo_nodes_.back();
00251 }
00252
00253 template<typename T>
00254 void JloDisplayBase::setSceneNodePose(Ogre::SceneNode* scene_node, T mat, Ogre::Quaternion &orientation)
00255 {
00256 if(m_binited)
00257 {
00258 std::string fxFrame = fixed_frame_;
00259 if(fixed_frame_.length() < 2)
00260 fxFrame = "/map";
00261 try
00262 {
00263 tf::Stamped<tf::Pose> pose_w(btTransform(btMatrix3x3(mat[0],mat[1],mat[2],
00264 mat[4],mat[5],mat[6],
00265 mat[8],mat[9],mat[10]),
00266 btVector3(mat[3], mat[7], mat[11])), ros::Time(),
00267 "/map");
00268
00269 Ogre::Vector3 position;
00270 if(!vis_manager_->getFrameManager()->getTransform(fxFrame, ros::Time(), position, orientation, false))
00271 {
00272 ROS_ERROR("Error getting transforming frame '%s'",
00273 fxFrame.c_str());
00274 return;
00275 }
00276
00277 Ogre::Vector3 position_w = Ogre::Vector3(pose_w.getOrigin().x(),
00278 pose_w.getOrigin().y(), pose_w.getOrigin().z());
00279 rviz::robotToOgre(position_w);
00280 position_w.x += position.x;
00281 position_w.y += position.y;
00282 position_w.z += position.z;
00283 Ogre::Matrix3 rot, rot2;
00284 orientation.ToRotationMatrix(rot);
00285
00286 rot2 = rot * Ogre::Matrix3(mat[0],mat[1],mat[2],mat[4],mat[5],mat[6],mat[8],mat[9],mat[10]);
00287 orientation.FromRotationMatrix(rot2);
00288 btScalar yaw_w, pitch_w, roll_w;
00289 pose_w.getBasis().getEulerYPR(yaw_w, pitch_w, roll_w);
00290
00291 Ogre::Matrix3 orientation_w(1,0,0,0,1,0,0,0,1);
00292 scene_node->setPosition(position_w);
00293 }
00294 catch(...)
00295 {
00296 return;
00297 }
00298 }
00299 }
00300
00301 }
00302
00303
00304 #endif