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 <algorithm>
00031 #include <vector>
00032
00033 #include <boost/bind.hpp>
00034
00035 #include <OgreBillboardSet.h>
00036 #include <OgreManualObject.h>
00037 #include <OgreMatrix4.h>
00038 #include <OgreSceneManager.h>
00039 #include <OgreSceneNode.h>
00040
00041 #include <rviz/display_context.h>
00042 #include <rviz/frame_manager.h>
00043 #include <rviz/ogre_helpers/billboard_line.h>
00044 #include <rviz/properties/color_property.h>
00045 #include <rviz/properties/enum_property.h>
00046 #include <rviz/properties/float_property.h>
00047 #include <rviz/properties/int_property.h>
00048 #include <rviz/properties/vector_property.h>
00049
00050 #include <trajectory_tracker_msgs/PathWithVelocity.h>
00051 #include <trajectory_tracker_msgs/PoseStampedWithVelocity.h>
00052 #include <trajectory_tracker_rviz_plugins/path_with_velocity_display.h>
00053 #include <trajectory_tracker_rviz_plugins/validate_floats.h>
00054
00055 #include <rviz/validate_floats.h>
00056 #ifdef HAVE_VALIDATE_QUATERNION_H
00057 #include <rviz/validate_quaternions.h>
00058 #endif
00059
00060 namespace trajectory_tracker_rviz_plugins
00061 {
00062 PathWithVelocityDisplay::PathWithVelocityDisplay()
00063 {
00064 style_property_ = new rviz::EnumProperty(
00065 "Line Style", "Lines",
00066 "The rendering operation to use to draw the grid lines.",
00067 this, SLOT(updateStyle()));
00068
00069 style_property_->addOption("Lines", LINES);
00070 style_property_->addOption("Billboards", BILLBOARDS);
00071
00072 line_width_property_ = new rviz::FloatProperty(
00073 "Line Width", 0.03,
00074 "The width, in meters, of each path line."
00075 "Only works with the 'Billboards' style.",
00076 this, SLOT(updateLineWidth()), this);
00077 line_width_property_->setMin(0.001);
00078 line_width_property_->hide();
00079
00080 color_property_ = new rviz::ColorProperty(
00081 "Color", QColor(25, 255, 0),
00082 "Color to draw the path.", this);
00083
00084 alpha_property_ = new rviz::FloatProperty(
00085 "Alpha", 1.0,
00086 "Amount of transparency to apply to the path.", this);
00087
00088 buffer_length_property_ = new rviz::IntProperty(
00089 "Buffer Length", 1,
00090 "Number of paths to display.",
00091 this, SLOT(updateBufferLength()));
00092 buffer_length_property_->setMin(1);
00093
00094 offset_property_ = new rviz::VectorProperty(
00095 "Offset", Ogre::Vector3::ZERO,
00096 "Allows you to offset the path from the origin of the reference frame. In meters.",
00097 this, SLOT(updateOffset()));
00098
00099 pose_style_property_ = new rviz::EnumProperty(
00100 "Pose Style", "None", "Shape to display the pose as.",
00101 this, SLOT(updatePoseStyle()));
00102 pose_style_property_->addOption("None", NONE);
00103 pose_style_property_->addOption("Axes", AXES);
00104 pose_style_property_->addOption("Arrows", ARROWS);
00105
00106 pose_axes_length_property_ = new rviz::FloatProperty(
00107 "Length", 0.3,
00108 "Length of the axes.",
00109 this, SLOT(updatePoseAxisGeometry()));
00110 pose_axes_radius_property_ = new rviz::FloatProperty(
00111 "Radius", 0.03,
00112 "Radius of the axes.",
00113 this, SLOT(updatePoseAxisGeometry()));
00114
00115 pose_arrow_color_property_ = new rviz::ColorProperty(
00116 "Pose Color",
00117 QColor(255, 85, 255),
00118 "Color to draw the poses.",
00119 this, SLOT(updatePoseArrowColor()));
00120 pose_arrow_shaft_length_property_ = new rviz::FloatProperty(
00121 "Shaft Length", 0.1,
00122 "Length of the arrow shaft.",
00123 this,
00124 SLOT(updatePoseArrowGeometry()));
00125 pose_arrow_head_length_property_ = new rviz::FloatProperty(
00126 "Head Length", 0.2,
00127 "Length of the arrow head.",
00128 this,
00129 SLOT(updatePoseArrowGeometry()));
00130 pose_arrow_shaft_diameter_property_ = new rviz::FloatProperty(
00131 "Shaft Diameter", 0.1,
00132 "Diameter of the arrow shaft.",
00133 this,
00134 SLOT(updatePoseArrowGeometry()));
00135 pose_arrow_head_diameter_property_ = new rviz::FloatProperty(
00136 "Head Diameter", 0.3,
00137 "Diameter of the arrow head.",
00138 this,
00139 SLOT(updatePoseArrowGeometry()));
00140 pose_axes_length_property_->hide();
00141 pose_axes_radius_property_->hide();
00142 pose_arrow_color_property_->hide();
00143 pose_arrow_shaft_length_property_->hide();
00144 pose_arrow_head_length_property_->hide();
00145 pose_arrow_shaft_diameter_property_->hide();
00146 pose_arrow_head_diameter_property_->hide();
00147 }
00148
00149 PathWithVelocityDisplay::~PathWithVelocityDisplay()
00150 {
00151 destroyObjects();
00152 destroyPoseAxesChain();
00153 destroyPoseArrowChain();
00154 }
00155
00156 void PathWithVelocityDisplay::onInitialize()
00157 {
00158 MFDClass::onInitialize();
00159 updateBufferLength();
00160 }
00161
00162 void PathWithVelocityDisplay::reset()
00163 {
00164 MFDClass::reset();
00165 updateBufferLength();
00166 }
00167
00168 void PathWithVelocityDisplay::allocateAxesVector(std::vector<rviz::Axes*>& axes_vect, size_t num)
00169 {
00170 if (num > axes_vect.size())
00171 {
00172 for (size_t i = axes_vect.size(); i < num; i++)
00173 {
00174 rviz::Axes* axes = new rviz::Axes(scene_manager_, scene_node_,
00175 pose_axes_length_property_->getFloat(),
00176 pose_axes_radius_property_->getFloat());
00177 axes_vect.push_back(axes);
00178 }
00179 }
00180 else if (num < axes_vect.size())
00181 {
00182 for (int i = static_cast<int>(axes_vect.size()) - 1; static_cast<int>(num) <= i; i--)
00183 {
00184 delete axes_vect[i];
00185 }
00186 axes_vect.resize(num);
00187 }
00188 }
00189
00190 void PathWithVelocityDisplay::allocateArrowVector(std::vector<rviz::Arrow*>& arrow_vect, size_t num)
00191 {
00192 if (num > arrow_vect.size())
00193 {
00194 for (size_t i = arrow_vect.size(); i < num; i++)
00195 {
00196 rviz::Arrow* arrow = new rviz::Arrow(scene_manager_, scene_node_);
00197 arrow_vect.push_back(arrow);
00198 }
00199 }
00200 else if (num < arrow_vect.size())
00201 {
00202 for (int i = static_cast<int>(arrow_vect.size()) - 1; static_cast<int>(num) <= i; i--)
00203 {
00204 delete arrow_vect[i];
00205 }
00206 arrow_vect.resize(num);
00207 }
00208 }
00209
00210 void PathWithVelocityDisplay::destroyPoseAxesChain()
00211 {
00212 for (size_t i = 0; i < axes_chain_.size(); i++)
00213 {
00214 allocateAxesVector(axes_chain_[i], 0);
00215 }
00216 axes_chain_.resize(0);
00217 }
00218
00219 void PathWithVelocityDisplay::destroyPoseArrowChain()
00220 {
00221 for (size_t i = 0; i < arrow_chain_.size(); i++)
00222 {
00223 allocateArrowVector(arrow_chain_[i], 0);
00224 }
00225 arrow_chain_.resize(0);
00226 }
00227
00228 void PathWithVelocityDisplay::updateStyle()
00229 {
00230 LineStyle style = (LineStyle)style_property_->getOptionInt();
00231
00232 switch (style)
00233 {
00234 case LINES:
00235 default:
00236 line_width_property_->hide();
00237 break;
00238
00239 case BILLBOARDS:
00240 line_width_property_->show();
00241 break;
00242 }
00243
00244 updateBufferLength();
00245 }
00246
00247 void PathWithVelocityDisplay::updateLineWidth()
00248 {
00249 LineStyle style = (LineStyle)style_property_->getOptionInt();
00250 float line_width = line_width_property_->getFloat();
00251
00252 if (style == BILLBOARDS)
00253 {
00254 for (size_t i = 0; i < billboard_lines_.size(); i++)
00255 {
00256 rviz::BillboardLine* billboard_line = billboard_lines_[i];
00257 if (billboard_line)
00258 billboard_line->setLineWidth(line_width);
00259 }
00260 }
00261 context_->queueRender();
00262 }
00263
00264 void PathWithVelocityDisplay::updateOffset()
00265 {
00266 scene_node_->setPosition(offset_property_->getVector());
00267 context_->queueRender();
00268 }
00269
00270 void PathWithVelocityDisplay::updatePoseStyle()
00271 {
00272 PoseStyle pose_style = (PoseStyle)pose_style_property_->getOptionInt();
00273 switch (pose_style)
00274 {
00275 case AXES:
00276 pose_axes_length_property_->show();
00277 pose_axes_radius_property_->show();
00278 pose_arrow_color_property_->hide();
00279 pose_arrow_shaft_length_property_->hide();
00280 pose_arrow_head_length_property_->hide();
00281 pose_arrow_shaft_diameter_property_->hide();
00282 pose_arrow_head_diameter_property_->hide();
00283 break;
00284 case ARROWS:
00285 pose_axes_length_property_->hide();
00286 pose_axes_radius_property_->hide();
00287 pose_arrow_color_property_->show();
00288 pose_arrow_shaft_length_property_->show();
00289 pose_arrow_head_length_property_->show();
00290 pose_arrow_shaft_diameter_property_->show();
00291 pose_arrow_head_diameter_property_->show();
00292 break;
00293 default:
00294 pose_axes_length_property_->hide();
00295 pose_axes_radius_property_->hide();
00296 pose_arrow_color_property_->hide();
00297 pose_arrow_shaft_length_property_->hide();
00298 pose_arrow_head_length_property_->hide();
00299 pose_arrow_shaft_diameter_property_->hide();
00300 pose_arrow_head_diameter_property_->hide();
00301 }
00302 updateBufferLength();
00303 }
00304
00305 void PathWithVelocityDisplay::updatePoseAxisGeometry()
00306 {
00307 for (size_t i = 0; i < axes_chain_.size(); i++)
00308 {
00309 std::vector<rviz::Axes*>& axes_vect = axes_chain_[i];
00310 for (size_t j = 0; j < axes_vect.size(); j++)
00311 {
00312 axes_vect[j]->set(pose_axes_length_property_->getFloat(),
00313 pose_axes_radius_property_->getFloat());
00314 }
00315 }
00316 context_->queueRender();
00317 }
00318
00319 void PathWithVelocityDisplay::updatePoseArrowColor()
00320 {
00321 QColor color = pose_arrow_color_property_->getColor();
00322
00323 for (size_t i = 0; i < arrow_chain_.size(); i++)
00324 {
00325 std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
00326 for (size_t j = 0; j < arrow_vect.size(); j++)
00327 {
00328 arrow_vect[j]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f);
00329 }
00330 }
00331 context_->queueRender();
00332 }
00333
00334 void PathWithVelocityDisplay::updatePoseArrowGeometry()
00335 {
00336 for (size_t i = 0; i < arrow_chain_.size(); i++)
00337 {
00338 std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[i];
00339 for (size_t j = 0; j < arrow_vect.size(); j++)
00340 {
00341 arrow_vect[j]->set(pose_arrow_shaft_length_property_->getFloat(),
00342 pose_arrow_shaft_diameter_property_->getFloat(),
00343 pose_arrow_head_length_property_->getFloat(),
00344 pose_arrow_head_diameter_property_->getFloat());
00345 }
00346 }
00347 context_->queueRender();
00348 }
00349
00350 void PathWithVelocityDisplay::destroyObjects()
00351 {
00352
00353 for (size_t i = 0; i < manual_objects_.size(); i++)
00354 {
00355 Ogre::ManualObject*& manual_object = manual_objects_[i];
00356 if (manual_object)
00357 {
00358 manual_object->clear();
00359 scene_manager_->destroyManualObject(manual_object);
00360 manual_object = NULL;
00361 }
00362 }
00363
00364
00365 for (size_t i = 0; i < billboard_lines_.size(); i++)
00366 {
00367 rviz::BillboardLine*& billboard_line = billboard_lines_[i];
00368 if (billboard_line)
00369 {
00370 delete billboard_line;
00371 billboard_line = NULL;
00372 }
00373 }
00374 }
00375
00376 void PathWithVelocityDisplay::updateBufferLength()
00377 {
00378
00379 destroyObjects();
00380
00381
00382 destroyPoseAxesChain();
00383 destroyPoseArrowChain();
00384
00385
00386 int buffer_length = buffer_length_property_->getInt();
00387 LineStyle style = (LineStyle)style_property_->getOptionInt();
00388
00389
00390 switch (style)
00391 {
00392 case LINES:
00393 manual_objects_.resize(buffer_length);
00394 for (size_t i = 0; i < manual_objects_.size(); i++)
00395 {
00396 Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
00397 manual_object->setDynamic(true);
00398 scene_node_->attachObject(manual_object);
00399
00400 manual_objects_[i] = manual_object;
00401 }
00402 break;
00403
00404 case BILLBOARDS:
00405 billboard_lines_.resize(buffer_length);
00406 for (size_t i = 0; i < billboard_lines_.size(); i++)
00407 {
00408 rviz::BillboardLine* billboard_line = new rviz::BillboardLine(scene_manager_, scene_node_);
00409 billboard_lines_[i] = billboard_line;
00410 }
00411 break;
00412 }
00413 axes_chain_.resize(buffer_length);
00414 arrow_chain_.resize(buffer_length);
00415 }
00416
00417 void PathWithVelocityDisplay::processMessage(const trajectory_tracker_msgs::PathWithVelocity::ConstPtr& msg)
00418 {
00419
00420 size_t bufferIndex = messages_received_ % buffer_length_property_->getInt();
00421
00422 LineStyle style = (LineStyle)style_property_->getOptionInt();
00423 Ogre::ManualObject* manual_object = NULL;
00424 rviz::BillboardLine* billboard_line = NULL;
00425
00426
00427 switch (style)
00428 {
00429 case LINES:
00430 manual_object = manual_objects_[bufferIndex];
00431 manual_object->clear();
00432 break;
00433
00434 case BILLBOARDS:
00435 billboard_line = billboard_lines_[bufferIndex];
00436 billboard_line->clear();
00437 break;
00438 }
00439
00440
00441 if (!trajectory_tracker_rviz_plugins::validateFloats(*msg))
00442 {
00443 setStatus(rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)");
00444 return;
00445 }
00446
00447 #ifdef HAVE_VALIDATE_QUATERNION_H
00448 if (!trajectory_tracker_rviz_plugins::validateQuaternions(msg->poses))
00449 {
00450 ROS_WARN_ONCE_NAMED(
00451 "quaternions", "Path '%s' contains unnormalized quaternions. "
00452 "This warning will only be output once but may be true for others; "
00453 "enable DEBUG messages for ros.rviz.quaternions to see more details.",
00454 qPrintable(getName()));
00455 ROS_DEBUG_NAMED(
00456 "quaternions", "Path '%s' contains unnormalized quaternions.",
00457 qPrintable(getName()));
00458 }
00459 #endif
00460
00461
00462 Ogre::Vector3 position;
00463 Ogre::Quaternion orientation;
00464 if (!context_->getFrameManager()->getTransform(msg->header, position, orientation))
00465 {
00466 ROS_DEBUG(
00467 "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
00468 }
00469
00470 Ogre::Matrix4 transform(orientation);
00471 transform.setTrans(position);
00472
00473
00474
00475
00476 Ogre::ColourValue color = color_property_->getOgreColor();
00477 color.a = alpha_property_->getFloat();
00478
00479 uint32_t num_points = msg->poses.size();
00480 float line_width = line_width_property_->getFloat();
00481
00482 switch (style)
00483 {
00484 case LINES:
00485 manual_object->estimateVertexCount(num_points);
00486 manual_object->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
00487 for (uint32_t i = 0; i < num_points; ++i)
00488 {
00489 const geometry_msgs::Point& pos = msg->poses[i].pose.position;
00490 Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
00491 manual_object->position(xpos.x, xpos.y, xpos.z);
00492 manual_object->colour(color);
00493 }
00494
00495 manual_object->end();
00496 break;
00497
00498 case BILLBOARDS:
00499 billboard_line->setNumLines(1);
00500 billboard_line->setMaxPointsPerLine(num_points);
00501 billboard_line->setLineWidth(line_width);
00502
00503 for (uint32_t i = 0; i < num_points; ++i)
00504 {
00505 const geometry_msgs::Point& pos = msg->poses[i].pose.position;
00506 Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
00507 billboard_line->addPoint(xpos, color);
00508 }
00509
00510 break;
00511 }
00512
00513
00514 PoseStyle pose_style = (PoseStyle)pose_style_property_->getOptionInt();
00515 std::vector<rviz::Arrow*>& arrow_vect = arrow_chain_[bufferIndex];
00516 std::vector<rviz::Axes*>& axes_vect = axes_chain_[bufferIndex];
00517
00518 switch (pose_style)
00519 {
00520 case AXES:
00521 allocateAxesVector(axes_vect, num_points);
00522 for (uint32_t i = 0; i < num_points; ++i)
00523 {
00524 const geometry_msgs::Point& pos = msg->poses[i].pose.position;
00525 Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
00526 axes_vect[i]->setPosition(xpos);
00527 Ogre::Quaternion orientation(msg->poses[i].pose.orientation.w,
00528 msg->poses[i].pose.orientation.x,
00529 msg->poses[i].pose.orientation.y,
00530 msg->poses[i].pose.orientation.z);
00531 axes_vect[i]->setOrientation(orientation);
00532 }
00533 break;
00534
00535 case ARROWS:
00536 allocateArrowVector(arrow_vect, num_points);
00537 for (uint32_t i = 0; i < num_points; ++i)
00538 {
00539 const geometry_msgs::Point& pos = msg->poses[i].pose.position;
00540 Ogre::Vector3 xpos = transform * Ogre::Vector3(pos.x, pos.y, pos.z);
00541
00542 QColor color = pose_arrow_color_property_->getColor();
00543 arrow_vect[i]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f);
00544
00545 arrow_vect[i]->set(pose_arrow_shaft_length_property_->getFloat(),
00546 pose_arrow_shaft_diameter_property_->getFloat(),
00547 pose_arrow_head_length_property_->getFloat(),
00548 pose_arrow_head_diameter_property_->getFloat());
00549 arrow_vect[i]->setPosition(xpos);
00550 Ogre::Quaternion orientation(msg->poses[i].pose.orientation.w,
00551 msg->poses[i].pose.orientation.x,
00552 msg->poses[i].pose.orientation.y,
00553 msg->poses[i].pose.orientation.z);
00554
00555 Ogre::Vector3 dir(1, 0, 0);
00556 dir = orientation * dir;
00557 arrow_vect[i]->setDirection(dir);
00558 }
00559 break;
00560
00561 default:
00562 break;
00563 }
00564 context_->queueRender();
00565 }
00566
00567 }
00568
00569 #include <pluginlib/class_list_macros.hpp>
00570 PLUGINLIB_EXPORT_CLASS(trajectory_tracker_rviz_plugins::PathWithVelocityDisplay, rviz::Display)