cop_rviz_plugin: cop_rviz_plugin: CopAnswerDisplay.cpp Source File

00001 /*
00002  * Copyright (c) 2008, U. Klank
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *
00030  */
00031 
00032 #include "CopAnswerDisplay.h"
00033 #include "rviz/common.h"
00034 #include "rviz/properties/property.h"
00035 #include "rviz/properties/property_manager.h"
00036 #include <ogre_tools/shape.h>
00037 #include <ogre_tools/billboard_line.h>
00038 
00039 #include "Line3D.h"
00040 
00041 #include <algorithm>
00042 #include <vector>
00043 #include <sstream>
00044 
00045 using namespace vision_srvs;
00046 namespace rviz_shows_cop
00047 {
00048 
00049 CopAnswerDisplay::CopAnswerDisplay(const std::string & name,
00050                                          rviz::VisualizationManager * manager)
00051   : JloDisplayBase(name, manager)
00052 {
00053   m_binited = false;
00054   setSGP(false);
00055   setTH(0.72);
00056   setOffset(0.0);
00057   /*m_manualObject = scene_manager_->createManualObject( "Fingerlines");
00058   m_manualObject->setDynamic( true );
00059   scene_node_->attachObject(m_manualObject);*/
00060 }
00061 
00062 CopAnswerDisplay::~CopAnswerDisplay()
00063 {
00064   unsubscribe();
00065   /*scene_manager_->destroyManualObject( m_manualObject );*/
00066 
00067 }
00068 
00069 void CopAnswerDisplay::setTopic(const std::string & topic)
00070 {
00071   if(m_binited)
00072     unsubscribe();
00073 
00074   JloDisplayBase::setTopic(topic);
00075   subscribe();
00076 }
00077 
00078 void CopAnswerDisplay::setSGP(bool sgp)
00079 {
00080   m_sgp = sgp;
00081   propertyChanged(m_sgp_property);
00082   processMessage(m_currentMessage);
00083 }
00084 
00085 void CopAnswerDisplay::setHand(bool hand)
00086 {
00087   m_hand = hand;
00088   propertyChanged(m_hand_property);
00089   processMessage(m_currentMessage);
00090 }
00091 
00092 void CopAnswerDisplay::setAuto(bool hand)
00093 {
00094   m_auto = hand;
00095   propertyChanged(m_auto_prop);
00096   processMessage(m_currentMessage);
00097 }
00098 
00099 
00100 void CopAnswerDisplay::setSGPColor(rviz::Color col)
00101 {
00102   m_sgpColor = col;
00103   propertyChanged(m_sgpColor_prop);
00104   processMessage(m_currentMessage);
00105 }
00106 
00107 void CopAnswerDisplay::setTH(float table_height)
00108 {
00109   m_th = table_height;
00110   propertyChanged(m_th_property);
00111   processMessage(m_currentMessage);
00112 }
00113 
00114 void CopAnswerDisplay::setOffset(float offset)
00115 {
00116   m_offset = offset;
00117   propertyChanged(m_offset_property);
00118   processMessage(m_currentMessage);
00119 }
00120 void CopAnswerDisplay::setOfftop(float offset)
00121 {
00122   m_offtop = offset;
00123   propertyChanged(m_offset_property);
00124   processMessage(m_currentMessage);
00125 }
00126 
00127 void CopAnswerDisplay::setA(float table_height)
00128 {
00129   alpha = table_height;
00130   m_manConf.alpha = alpha;
00131   propertyChanged(m_alpha_prop);
00132   processMessage(m_currentMessage);
00133 }
00134 void CopAnswerDisplay::setB(float table_height)
00135 {
00136   beta = table_height;
00137   m_manConf.beta = beta;
00138   propertyChanged(m_beta_prop);
00139   processMessage(m_currentMessage);
00140 }
00141 void CopAnswerDisplay::setD(float table_height)
00142 {
00143   delta = table_height;
00144   m_manConf.delta_max=delta;
00145   propertyChanged(m_delta_prop);
00146   processMessage(m_currentMessage);
00147 }
00148 
00149 
00150 void CopAnswerDisplay::subscribe()
00151 {
00152   if (getTopic().length() < 2 || !isEnabled())
00153   {
00154     m_binited = false;
00155     return;
00156   }
00157   m_binited = true;
00158   printf("subscribe to %s\n", getTopic().c_str());
00159   cop_subscriber = update_nh_.subscribe<vision_msgs::cop_answer>(getTopic(),1, boost::bind(&CopAnswerDisplay::incomingMessage, this, _1));
00160 }
00161 
00162 void CopAnswerDisplay::unsubscribe()
00163 {
00164    if(m_binited)
00165      cop_subscriber.shutdown();
00166 }
00167 
00168 double CopAnswerDisplay::GetOffsetBaseLinkRotZ(Ogre::Quaternion quat)
00169 {
00170   double rot = quat.getRoll().valueRadians();
00171   printf("Roll: %f Pitch %f  Yaw %f\n", quat.getRoll().valueRadians(), quat.getPitch().valueRadians(),quat.getYaw().valueRadians());
00172   return rot; 
00173 }
00174 
00175 
00176 #define NUM_HAND_POINTS 10
00177 #define MAX_OBSTACLES 100
00178 void CopAnswerDisplay::AttachSGPPoints(Ogre::SceneNode* object, std::vector<vision_msgs::partial_lo::_pose_type > matrices,
00179                                         std::vector<vision_msgs::partial_lo::_cov_type> covs, size_t index)
00180 {
00181   Point3D points[NUM_HAND_POINTS];
00182   Point3D ptemps[NUM_HAND_POINTS];
00183 
00184 
00185   Point3D obstacles[MAX_OBSTACLES];
00186   CovariancePoint obstacle_covs[MAX_OBSTACLES];
00187   size_t num_obstacles = matrices.size() < MAX_OBSTACLES ? matrices.size() : MAX_OBSTACLES;
00188   int param_num = num_obstacles;
00190   if(num_obstacles == 0)
00191     return;
00192   Ogre::Matrix3 rot (matrices[index][0],matrices[index][1],matrices[index][2],
00193                       matrices[index][4],matrices[index][5],matrices[index][6],
00194                       matrices[index][8],matrices[index][9],matrices[index][10]);
00195   Ogre::Quaternion quat = object->getOrientation();
00196   Ogre::Vector3 vec = object->getPosition();
00197   Ogre::Quaternion quat2;
00198   quat2.FromRotationMatrix(rot);
00199   double offset_rot_z = m_offset;/*GetOffsetBaseLinkRotZ(quat2);*/
00200   int handness = m_hand ? -1 : 1;
00201 
00203   points[0].x = 0.00*handness;  points[0].y = 0.000;   points[0].z =-0.03;
00204   points[1].x = 0.06*handness;  points[1].y = 0.025;   points[1].z = 0.11;
00205   points[2].x = 0.05*handness;  points[2].y = 0.025;   points[2].z = 0.0;
00206   points[3].x =-0.06*handness;  points[3].y = 0.010;   points[3].z = 0.11;
00207   points[4].x =-0.05*handness;  points[4].y = 0.010;   points[4].z = 0.0;
00208   points[5].x = 0.06*handness;  points[5].y = -0.025;  points[5].z = 0.11;
00209   points[6].x = 0.05*handness;  points[6].y = -0.025;  points[6].z = 0.0;
00210   points[7].x = 0.06*handness;  points[7].y = -0.085;  points[7].z = 0.11;
00211   points[8].x = 0.05*handness;  points[8].y = -0.085;  points[8].z = 0.00;
00212   points[9].x = 0.00*handness;  points[9].y = -0.010;  points[9].z =-0.03;
00213 
00214 
00215 
00216 
00218   size_t done = 0;
00219   for(size_t i = 0; i < num_obstacles; i++)
00220   {
00221     size_t ind_temp = i + 1 - done;
00222     if(i == index)
00223     {
00224       ind_temp = 0;
00225       done = 1;
00226     }
00227     if(matrices[i].size() != 16)
00228     {
00229       ROS_INFO(" \nWrong Matrix legnth at mat i=%ld. its not 16 but %ld\n ", i, matrices[i].size());
00230       throw "Error !!\n";
00231     }
00232     if(covs[i].size() != 36)
00233     {
00234       ROS_INFO(" \nWrong cov legnth at mat i=%ld. its not 36 but %ld\n ", i, matrices[i].size());
00235       throw "Error !!\n";
00236     }
00237 
00238     obstacles[ind_temp].x = matrices[i][3];
00239     obstacles[ind_temp].y = matrices[i][7];
00240     obstacles[ind_temp].z = matrices[i][11];
00241     obstacle_covs[ind_temp].sx  = covs[i][0];
00242     obstacle_covs[ind_temp].sxy = covs[i][1];
00243     obstacle_covs[ind_temp].sxz = covs[i][2];
00244     obstacle_covs[ind_temp].syx = covs[i][6];
00245     obstacle_covs[ind_temp].sy  = covs[i][7];
00246     obstacle_covs[ind_temp].syz = covs[i][8];
00247     obstacle_covs[ind_temp].szx = covs[i][12];
00248     obstacle_covs[ind_temp].szy = covs[i][13];
00249     obstacle_covs[ind_temp].sz  = covs[i][14];
00250   }
00251   HandConfig conf;
00252   if(m_auto)
00253   {
00255     InitSGP(points, NUM_HAND_POINTS, m_th);
00256     /*printf("%ld elements with diag of covx = %f, %f, %f\n", num_obstacles, obstacle_covs[0].sx, obstacle_covs[0].sy, obstacle_covs[0].sz);*/
00258     conf = GetGraspLM(obstacles, obstacle_covs, param_num, offset_rot_z, m_offtop);
00259   }
00260   else
00261   {
00262     conf = m_manConf;
00263   }
00264   printf("Estimated hand configuration: a: %f b: %f d: %f\n", conf.alpha, conf.beta, conf.delta_max);
00265 
00266   for(size_t j = 0; j< NUM_HAND_POINTS; j++)
00267   {
00268     ptemps[j] = TransformPoint(points[j], obstacles[0], conf.alpha, conf.beta, conf.delta_max);
00269   }
00270   /*Ogre::SceneNode* handref;*/
00271   Ogre::SceneNode* tmpnode;
00272 
00273 
00274   ogre_tools::BillboardLine  *lines = new ogre_tools::BillboardLine(vis_manager_->getSceneManager(), object);
00275 
00276   Ogre::Vector3 pos, scale;
00277   Ogre::Quaternion orient;
00278 
00279   /*lines->setPosition(pos);
00280   lines->setOrientation(orient);*/
00281   /*lines->setScale(scale);*/
00282   lines->clear();
00283   lines->setLineWidth( 0.005 );
00284   lines->setMaxPointsPerLine(2);
00285   lines->setNumLines(NUM_HAND_POINTS + 1);
00286   lines->setColor(m_sgpColor.r_, m_sgpColor.g_, m_sgpColor.b_, 1.0);
00287 
00288   /*m_manualObject->estimateVertexCount(2* NUM_HAND_POINTS );
00289   m_manualObject->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );*/
00290 
00291   for(size_t j = 0; j< NUM_HAND_POINTS; j++)
00292   {
00293 
00294     Ogre::Matrix3 rot (matrices[index][0],matrices[index][1],matrices[index][2],
00295                        matrices[index][4],matrices[index][5],matrices[index][6],
00296                        matrices[index][8],matrices[index][9],matrices[index][10]);
00297 
00299     Point3D ptemp = ptemps[j];
00300     printf("Point %ld: %f %f %f  (relobj: %f, %f, %f)\n", j, ptemp.x, ptemp.y, ptemp.z, ptemp.x - obstacles[0].x, ptemp.y - obstacles[0].y, ptemp.z - obstacles[0].z);
00301 
00302     /*ogre_tools::Axes* shape= new ogre_tools::Axes( scene_manager_, object, 0.005, 0.005 );*/
00303     ogre_tools::Shape* shape = new ogre_tools::Shape (ogre_tools::Shape::Sphere, scene_manager_, object);
00304     shape->setColor(m_sgpColor.r_, m_sgpColor.g_, m_sgpColor.b_, 1.0);
00305     tmpnode = shape->getRootNode();
00306 
00307     lines->newLine();
00308     if(j > 0 && j % 2 == 0 && j < 9)
00309     {
00310       //m_manualObject->position();
00311       Ogre::Vector3 vec1 ((ptemps[0].x - obstacles[0].x), (ptemps[0].y - obstacles[0].y), ptemps[0].z - obstacles[0].z);
00312       Ogre::Matrix3 rotinv =  rot.Inverse();
00313       Ogre::Vector3 vec_trans1 = rot * vec1;
00314       lines->addPoint(Ogre::Vector3(-vec_trans1.y,vec_trans1.z,-vec_trans1.x));
00315 
00316       Ogre::Vector3 vec((ptemps[j].x - obstacles[0].x), (ptemps[j].y - obstacles[0].y), ptemps[j].z - obstacles[0].z);
00317       Ogre::Vector3 vec_trans = rot * vec;
00318       lines->addPoint(Ogre::Vector3(-vec_trans.y,vec_trans.z,-vec_trans.x));
00319       //m_manualObject->colour( color );
00320     }
00321     else if(j > 0 && j % 2 == 1 && j < 9)
00322     {
00323       Ogre::Vector3 vec1 ((ptemps[j].x - obstacles[0].x), (ptemps[j].y - obstacles[0].y), ptemps[j].z - obstacles[0].z);
00324       Ogre::Matrix3 rotinv =  rot.Inverse();
00325       Ogre::Vector3 vec_trans1 = rot * vec1;
00326       lines->addPoint(Ogre::Vector3(-vec_trans1.y,vec_trans1.z,-vec_trans1.x));
00327 
00328       Ogre::Vector3 vec((ptemps[j+1].x - obstacles[0].x), (ptemps[j+1].y - obstacles[0].y), ptemps[j+1].z - obstacles[0].z);
00329       Ogre::Vector3 vec_trans = rot * vec;
00330       lines->addPoint(Ogre::Vector3(-vec_trans.y,vec_trans.z,-vec_trans.x));
00331     }
00332     else if (j == 0)
00333     {
00334       Ogre::Vector3 vec1((ptemps[j].x - obstacles[0].x), (ptemps[j].y - obstacles[0].y), ptemps[j].z - obstacles[0].z);
00335       Ogre::Matrix3 rotinv =  rot.Inverse();
00336       Ogre::Vector3 vec_trans1 = rot * vec1;
00337       lines->addPoint(Ogre::Vector3(-vec_trans1.y,vec_trans1.z,-vec_trans1.x));
00338       Ogre::Vector3 vec((ptemps[j].x - obstacles[0].x), (ptemps[j].y - obstacles[0].y), ptemps[j].z - obstacles[0].z);
00339       Ogre::Vector3 vec_trans = rot * vec;
00340       lines->addPoint(Ogre::Vector3(-vec_trans.y,vec_trans.z,-vec_trans.x));
00341     }
00342     else
00343     {
00344       Ogre::Vector3 vec1((ptemps[j].x - obstacles[0].x), (ptemps[j].y - obstacles[0].y), ptemps[j].z - obstacles[0].z);
00345       Ogre::Matrix3 rotinv =  rot.Inverse();
00346       Ogre::Vector3 vec_trans1 = rot * vec1;
00347       lines->addPoint(Ogre::Vector3(-vec_trans1.y,vec_trans1.z,-vec_trans1.x));
00348       Ogre::Vector3 vec((ptemps[j].x - obstacles[0].x), (ptemps[j].y - obstacles[0].y), ptemps[j].z - obstacles[0].z);
00349       Ogre::Vector3 vec_trans = rot * vec;
00350       lines->addPoint(Ogre::Vector3(-vec_trans.y,vec_trans.z,-vec_trans.x));
00351     }
00352     //m_manualObject->end();
00353     /*tmpnode  = shape->getSceneNode();*/
00354     shape->setScale(Ogre::Vector3(0.02,0.02,0.02));
00355 
00356     /*if(j == 0)
00357        handref = tmpnode = axis->getSceneNode();
00358     else
00359     {
00360        tmpnode = axis->getSceneNode();
00361        ogre_tools::Arrow* arrow = new ogre_tools::Arrow( scene_manager_, tmpnode, 1.0f, 0.01, 1.0f, 0.08 );
00362        arrow->setPosition(handref->getPosition());
00363        arrow->getSceneNode()->setVisible(true);
00364     }*/
00365     /*Ogre::Quaternion temp_quat;
00366     temp_quat.FromRotationMatrix(rotMat);*/
00367 
00368     /*tmpnode->setOrientation(quat);*/
00369     Ogre::Vector3 vec((ptemp.x - obstacles[0].x), (ptemp.y - obstacles[0].y), ptemp.z - obstacles[0].z);
00370     Ogre::Matrix3 rotinv =  rot.Inverse();
00371     Ogre::Vector3 vec_trans =  rotinv * vec;
00372     /* printf("vec_trans (rotinv): %f %f %f \n", vec_trans.x, vec_trans.y, vec_trans.z);*/
00373 
00374     vec_trans = rot * vec;
00375     /*printf("vec_trans (rot): %f %f %f \n", vec_trans.x, vec_trans.y, vec_trans.z);*/
00376     tmpnode->setPosition(Ogre::Vector3(-vec_trans.y, vec_trans.z, -vec_trans.x));
00377     tmpnode->setVisible(true);
00378   }
00379 
00380 
00381  /* lines->setVisible(true);*/
00382 }
00383 
00384 void CopAnswerDisplay::processMessage (vision_msgs::cop_answer& msg)
00385 {
00386   if(m_binited)
00387   {
00388     std::vector<vision_msgs::partial_lo::_pose_type > matrices;
00389     std::vector<vision_msgs::partial_lo::_cov_type> covs;
00390 
00391     if (msg.error.length() > 0)
00392     {
00393       return;
00394     }
00395 
00396     std::vector<JloDisplayBase::JloDescription> jloSet(msg.found_poses.size());
00397 
00398     for(unsigned int i = 0; i < msg.found_poses.size(); i++)
00399     {
00400       std::stringstream strm;
00401 
00402       jloSet[i].pose = msg.found_poses[i].position;
00403 
00404       if(msg.found_poses[i].models.size() > 0)
00405       {
00406         strm << msg.found_poses[i].models[0].sem_class << " ";
00407       }
00408       strm << "(" << msg.found_poses[i].objectId << " at " << msg.found_poses[i].position << ")";
00409       jloSet[i].label = strm.str();
00410     }
00411     std::list<std::pair<Ogre::SceneNode*, vision_msgs::partial_lo> > &list =  displayJloSet(jloSet.begin(), jloSet.end(), false);
00412     std::list<std::pair<Ogre::SceneNode*, vision_msgs::partial_lo> >::iterator iter = list.begin();
00413     for(; iter != list.end(); iter++)
00414     {
00415       matrices.push_back((*iter).second.pose);
00416       covs.push_back((*iter).second.cov);
00417     }
00418     iter = list.begin();
00419     if(m_sgp)
00420     {
00421       for(size_t i = 0; i < msg.found_poses.size(); i++)
00422       {
00423         AttachSGPPoints((*iter).first, matrices, covs, i);
00424         iter++;
00425       }
00426     }
00427     causeRender();
00428   }
00429 }
00430 
00431 void CopAnswerDisplay::incomingMessage(const vision_msgs::cop_answer::ConstPtr& msg)
00432 {
00433   m_currentMessage = *(msg.get());
00434   printf("Got message: %ld elems\n", m_currentMessage.found_poses.size());
00435   try
00436   {
00437    processMessage(m_currentMessage);
00438   }
00439   catch(...)
00440   {
00441     ROS_INFO("tf sucks, please start a state publisher or similar stuff\n");
00442   }
00443 
00444 }
00445 
00446 void CopAnswerDisplay::createProperties()
00447 {
00448   /*lor_property_ = property_manager_->createProperty<rviz::ColorProperty> ("Color", property_prefix_, boost::bind(&CopAnswerDisplay::getColor, this),
00449                                                                       boost::bind(&CopAnswerDisplay::setColor, this, _1), parent_category_, this);*/
00450   JloDisplayBase::createProperties();
00451 
00452   topic_property_ = property_manager_->createProperty<rviz::ROSTopicStringProperty> ("Topic", property_prefix_, boost::bind(&JloDisplayBase::getTopic, this),
00453                                                                                boost::bind(&JloDisplayBase::setTopic, this, _1), parent_category_, this);
00454   rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00455   topic_prop->setMessageType(vision_msgs::cop_answer::__s_getDataType());
00456 
00457 
00458   m_sgp_property = property_manager_->createProperty<rviz::BoolProperty> ("GraspPoints", property_prefix_, boost::bind(&CopAnswerDisplay::getSGP, this),
00459                                                                       boost::bind(&CopAnswerDisplay::setSGP, this, _1), parent_category_,this);
00460   m_hand_property = property_manager_->createProperty<rviz::BoolProperty> ("Right Hand", property_prefix_, boost::bind(&CopAnswerDisplay::getHand, this),
00461                                                                       boost::bind(&CopAnswerDisplay::setHand, this, _1), parent_category_,this);
00462   m_sgpColor_prop = property_manager_->createProperty<rviz::ColorProperty> ("Hand Color", property_prefix_, boost::bind(&CopAnswerDisplay::getSGPColor, this),
00463                                                                       boost::bind(&CopAnswerDisplay::setSGPColor, this, _1), parent_category_,this);
00464   m_th_property = property_manager_->createProperty<rviz::FloatProperty> ("Table Height", property_prefix_, boost::bind(&CopAnswerDisplay::getTH, this),
00465                                                                       boost::bind(&CopAnswerDisplay::setTH, this, _1), parent_category_,this);
00466 
00467   m_offset_property = property_manager_->createProperty<rviz::FloatProperty> ("Offset Z", property_prefix_, boost::bind(&CopAnswerDisplay::getOffset, this),
00468                                                                       boost::bind(&CopAnswerDisplay::setOffset, this, _1), parent_category_,this);
00469 
00470   m_offset_property = property_manager_->createProperty<rviz::FloatProperty> ("Offset Z top", property_prefix_, boost::bind(&CopAnswerDisplay::getOfftop, this),
00471                                                                       boost::bind(&CopAnswerDisplay::setOfftop, this, _1), parent_category_,this);
00472 
00473   m_auto_prop = property_manager_->createProperty<rviz::BoolProperty> ("Automatic grasp point", property_prefix_, boost::bind(&CopAnswerDisplay::getAuto, this),
00474                                                                       boost::bind(&CopAnswerDisplay::setAuto, this, _1), parent_category_,this);
00475   m_alpha_prop = property_manager_->createProperty<rviz::FloatProperty> ("Alpha", property_prefix_, boost::bind(&CopAnswerDisplay::getA, this),
00476                                                                       boost::bind(&CopAnswerDisplay::setA, this, _1), parent_category_,this);
00477   m_beta_prop = property_manager_->createProperty<rviz::FloatProperty> ("Beta", property_prefix_, boost::bind(&CopAnswerDisplay::getB, this),
00478                                                                       boost::bind(&CopAnswerDisplay::setB, this, _1), parent_category_,this);
00479   m_delta_prop = property_manager_->createProperty<rviz::FloatProperty> ("Theta", property_prefix_, boost::bind(&CopAnswerDisplay::getD, this),
00480                                                                       boost::bind(&CopAnswerDisplay::setD, this, _1), parent_category_,this);
00481 
00482 }
00483 
00484 } // namespace