Go to the documentation of this file.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
00032 #include <GL/glew.h>
00033 #include <GL3/gl3.h>
00034 #include <GL/glu.h>
00035 #include <GL/glx.h>
00036 #undef Success // <---- Screw Xlib for this
00037
00038 #include <ros/node_handle.h>
00039
00040 #include <realtime_urdf_filter/urdf_renderer.h>
00041
00042 namespace realtime_urdf_filter
00043 {
00044 URDFRenderer::URDFRenderer (std::string model_description,
00045 std::string tf_prefix,
00046 std::string cam_frame,
00047 std::string fixed_frame,
00048 tf::TransformListener &tf)
00049 : model_description_(model_description)
00050 , tf_prefix_(tf_prefix)
00051 , camera_frame_ (cam_frame)
00052 , fixed_frame_(fixed_frame)
00053 , tf_(tf)
00054 {
00055 initURDFModel ();
00056 tf_.setExtrapolationLimit (ros::Duration (5.0));
00057 }
00058
00060
00061 void
00062 URDFRenderer::initURDFModel ()
00063 {
00064 urdf::Model model;
00065 if (!model.initString(model_description_))
00066 {
00067 ROS_ERROR ("URDF failed Model parse");
00068 return;
00069 }
00070
00071 ROS_INFO ("URDF parsed OK");
00072 loadURDFModel (model);
00073 ROS_INFO ("URDF loaded OK");
00074 }
00075
00076
00079 void URDFRenderer::loadURDFModel
00080 (urdf::Model &model)
00081 {
00082 typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
00083 V_Link links;
00084 model.getLinks(links);
00085
00086 V_Link::iterator it = links.begin();
00087 V_Link::iterator end = links.end();
00088
00089 for (; it != end; ++it)
00090 process_link (*it);
00091 }
00092
00094
00095 void URDFRenderer::process_link (boost::shared_ptr<urdf::Link> link)
00096 {
00097 if (link->visual.get() == NULL || link->visual->geometry.get() == NULL)
00098 return;
00099
00100 boost::shared_ptr<Renderable> r;
00101 if (link->visual->geometry->type == urdf::Geometry::BOX)
00102 {
00103 boost::shared_ptr<urdf::Box> box = boost::dynamic_pointer_cast<urdf::Box> (link->visual->geometry);
00104 r.reset (new RenderableBox (box->dim.x, box->dim.y, box->dim.z));
00105 }
00106 else if (link->visual->geometry->type == urdf::Geometry::CYLINDER)
00107 {
00108 boost::shared_ptr<urdf::Cylinder> cylinder = boost::dynamic_pointer_cast<urdf::Cylinder> (link->visual->geometry);
00109 r.reset (new RenderableCylinder (cylinder->radius, cylinder->length));
00110 }
00111 else if (link->visual->geometry->type == urdf::Geometry::SPHERE)
00112 {
00113 boost::shared_ptr<urdf::Sphere> sphere = boost::dynamic_pointer_cast<urdf::Sphere> (link->visual->geometry);
00114 r.reset (new RenderableSphere (sphere->radius));
00115 }
00116 else if (link->visual->geometry->type == urdf::Geometry::MESH)
00117 {
00118 boost::shared_ptr<urdf::Mesh> mesh = boost::dynamic_pointer_cast<urdf::Mesh> (link->visual->geometry);
00119 std::string meshname (mesh->filename);
00120 RenderableMesh* rm = new RenderableMesh (meshname);
00121 rm->setScale (mesh->scale.x, mesh->scale.y, mesh->scale.z);
00122 r.reset (rm);
00123 }
00124 r->setLinkName (tf_prefix_+ "/" + link->name);
00125 urdf::Vector3 origin = link->visual->origin.position;
00126 urdf::Rotation rotation = link->visual->origin.rotation;
00127 r->link_offset = tf::Transform (
00128 tf::Quaternion (rotation.x, rotation.y, rotation.z, rotation.w).normalize (),
00129 tf::Vector3 (origin.x, origin.y, origin.z));
00130 if (link->visual &&
00131 (link->visual->material))
00132 r->color = link->visual->material->color;
00133 renderables_.push_back (r);
00134 }
00135
00137
00138 void URDFRenderer::update_link_transforms ()
00139 {
00140 tf::StampedTransform t;
00141
00142 std::vector<boost::shared_ptr<Renderable> >::const_iterator it = renderables_.begin ();
00143 for (; it != renderables_.end (); it++)
00144 {
00145 try
00146 {
00147 tf_.lookupTransform (fixed_frame_, (*it)->name, ros::Time (), t);
00148 }
00149 catch (tf::TransformException ex)
00150 {
00151 ROS_ERROR("%s",ex.what());
00152 }
00153 (*it)->link_to_fixed = tf::Transform (t.getRotation (), t.getOrigin ());
00154 }
00155 }
00156
00158
00159 void URDFRenderer::render ()
00160 {
00161 update_link_transforms ();
00162
00163 std::vector<boost::shared_ptr<Renderable> >::const_iterator it = renderables_.begin ();
00164 for (; it != renderables_.end (); it++)
00165 (*it)->render ();
00166 }
00167
00168 }
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247