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
00033
00034
00035 #include "bounding_box_array_display.h"
00036 #include <jsk_topic_tools/color_utils.h>
00037 namespace jsk_rviz_plugin
00038 {
00039 BoundingBoxArrayDisplay::BoundingBoxArrayDisplay()
00040 {
00041 color_property_ = new rviz::ColorProperty("color", QColor(25, 255, 0),
00042 "color to draw the bounding boxes",
00043 this, SLOT(updateColor()));
00044 alpha_property_ = new rviz::FloatProperty("alpha", 0.8,
00045 "alpha value to draw the bounding boxes",
00046 this, SLOT(updateAlpha()));
00047 only_edge_property_ = new rviz::BoolProperty("only edge", false,
00048 "show only the edges of the boxes",
00049 this, SLOT(updateOnlyEdge()));
00050 line_width_property_ = new rviz::FloatProperty("line width", 0.005,
00051 "line width of the edges",
00052 this, SLOT(updateLineWidth()));
00053 auto_color_property_ = new rviz::BoolProperty("auto color", false,
00054 "change the color of the boxes automatically",
00055 this, SLOT(updateAutoColor()));
00056 }
00057
00058 BoundingBoxArrayDisplay::~BoundingBoxArrayDisplay()
00059 {
00060 delete color_property_;
00061 delete alpha_property_;
00062 delete only_edge_property_;
00063 delete auto_color_property_;
00064 }
00065
00066 QColor BoundingBoxArrayDisplay::getColor(size_t index)
00067 {
00068 if (auto_color_) {
00069 std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(index);
00070 return QColor(ros_color.r * 255.0, ros_color.g * 255.0, ros_color.b * 255.0,
00071 ros_color.a * 255.0);
00072 }
00073 else {
00074 return color_;
00075 }
00076 }
00077
00078 void BoundingBoxArrayDisplay::onInitialize()
00079 {
00080 MFDClass::onInitialize();
00081 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00082
00083 updateColor();
00084 updateAlpha();
00085 updateOnlyEdge();
00086 updateAutoColor();
00087 updateLineWidth();
00088 }
00089
00090 void BoundingBoxArrayDisplay::updateLineWidth()
00091 {
00092 line_width_ = line_width_property_->getFloat();
00093 }
00094
00095 void BoundingBoxArrayDisplay::updateColor()
00096 {
00097 color_ = color_property_->getColor();
00098 }
00099
00100 void BoundingBoxArrayDisplay::updateAlpha()
00101 {
00102 alpha_ = alpha_property_->getFloat();
00103 }
00104
00105 void BoundingBoxArrayDisplay::updateOnlyEdge()
00106 {
00107 only_edge_ = only_edge_property_->getBool();
00108 }
00109
00110 void BoundingBoxArrayDisplay::updateAutoColor()
00111 {
00112 auto_color_ = auto_color_property_->getBool();
00113 }
00114
00115 void BoundingBoxArrayDisplay::reset()
00116 {
00117 MFDClass::reset();
00118 shapes_.clear();
00119 }
00120
00121 void BoundingBoxArrayDisplay::allocateShapes(int num)
00122 {
00123 if (num > shapes_.size()) {
00124 for (size_t i = shapes_.size(); i < num; i++) {
00125 ShapePtr shape (new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(),
00126 scene_node_));
00127 shapes_.push_back(shape);
00128 }
00129 }
00130 else if (num < shapes_.size())
00131 {
00132 shapes_.resize(num);
00133 }
00134 }
00135
00136 void BoundingBoxArrayDisplay::allocateBillboardLines(int num)
00137 {
00138 if (num > edges_.size()) {
00139 for (size_t i = edges_.size(); i < num; i++) {
00140 BillboardLinePtr line(new rviz::BillboardLine(context_->getSceneManager(), scene_node_));
00141 edges_.push_back(line);
00142 }
00143 }
00144 else if (num < edges_.size())
00145 {
00146 edges_.resize(num);
00147 }
00148 }
00149
00150 void BoundingBoxArrayDisplay::processMessage(const jsk_pcl_ros::BoundingBoxArray::ConstPtr& msg)
00151 {
00152 if (!only_edge_) {
00153 edges_.clear();
00154 allocateShapes(msg->boxes.size());
00155 for (size_t i = 0; i < msg->boxes.size(); i++) {
00156 jsk_pcl_ros::BoundingBox box = msg->boxes[i];
00157 ShapePtr shape = shapes_[i];
00158 Ogre::Vector3 position;
00159 Ogre::Quaternion quaternion;
00160 if(!context_->getFrameManager()->transform(box.header, box.pose,
00161 position,
00162 quaternion))
00163 {
00164 ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
00165 qPrintable( getName() ), box.header.frame_id.c_str(),
00166 qPrintable( fixed_frame_ ));
00167 return;
00168 }
00169 shape->setPosition(position);
00170 shape->setOrientation(quaternion);
00171 Ogre::Vector3 dimensions;
00172 dimensions[0] = box.dimensions.x;
00173 dimensions[1] = box.dimensions.y;
00174 dimensions[2] = box.dimensions.z;
00175 shape->setScale(dimensions);
00176 QColor color = getColor(i);
00177 shape->setColor(color.red() / 255.0,
00178 color.green() / 255.0,
00179 color.blue() / 255.0,
00180 alpha_);
00181 }
00182 }
00183 else {
00184 shapes_.clear();
00185 allocateBillboardLines(msg->boxes.size());
00186 for (size_t i = 0; i < msg->boxes.size(); i++) {
00187 jsk_pcl_ros::BoundingBox box = msg->boxes[i];
00188 geometry_msgs::Vector3 dimensions = box.dimensions;
00189
00190 BillboardLinePtr edge = edges_[i];
00191 edge->clear();
00192 Ogre::Vector3 position;
00193 Ogre::Quaternion quaternion;
00194 if(!context_->getFrameManager()->transform(box.header, box.pose,
00195 position,
00196 quaternion))
00197 {
00198 ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
00199 qPrintable( getName() ), box.header.frame_id.c_str(),
00200 qPrintable( fixed_frame_ ));
00201 return;
00202 }
00203 edge->setPosition(position);
00204 edge->setOrientation(quaternion);
00205
00206 edge->setMaxPointsPerLine(2);
00207 edge->setNumLines(12);
00208 edge->setLineWidth(line_width_);
00209 QColor color = getColor(i);
00210 edge->setColor(color.red() / 255.0,
00211 color.green() / 255.0,
00212 color.blue() / 255.0,
00213 alpha_);
00214
00215
00216
00217 Ogre::Vector3 A, B, C, D, E, F, G, H;
00218 A[0] = dimensions.x / 2.0;
00219 A[1] = dimensions.y / 2.0;
00220 A[2] = dimensions.z / 2.0;
00221 B[0] = - dimensions.x / 2.0;
00222 B[1] = dimensions.y / 2.0;
00223 B[2] = dimensions.z / 2.0;
00224 C[0] = - dimensions.x / 2.0;
00225 C[1] = - dimensions.y / 2.0;
00226 C[2] = dimensions.z / 2.0;
00227 D[0] = dimensions.x / 2.0;
00228 D[1] = - dimensions.y / 2.0;
00229 D[2] = dimensions.z / 2.0;
00230
00231 E[0] = dimensions.x / 2.0;
00232 E[1] = dimensions.y / 2.0;
00233 E[2] = - dimensions.z / 2.0;
00234 F[0] = - dimensions.x / 2.0;
00235 F[1] = dimensions.y / 2.0;
00236 F[2] = - dimensions.z / 2.0;
00237 G[0] = - dimensions.x / 2.0;
00238 G[1] = - dimensions.y / 2.0;
00239 G[2] = - dimensions.z / 2.0;
00240 H[0] = dimensions.x / 2.0;
00241 H[1] = - dimensions.y / 2.0;
00242 H[2] = - dimensions.z / 2.0;
00243
00244 edge->addPoint(A); edge->addPoint(B); edge->newLine();
00245 edge->addPoint(B); edge->addPoint(C); edge->newLine();
00246 edge->addPoint(C); edge->addPoint(D); edge->newLine();
00247 edge->addPoint(D); edge->addPoint(A); edge->newLine();
00248 edge->addPoint(E); edge->addPoint(F); edge->newLine();
00249 edge->addPoint(F); edge->addPoint(G); edge->newLine();
00250 edge->addPoint(G); edge->addPoint(H); edge->newLine();
00251 edge->addPoint(H); edge->addPoint(E); edge->newLine();
00252 edge->addPoint(A); edge->addPoint(E); edge->newLine();
00253 edge->addPoint(B); edge->addPoint(F); edge->newLine();
00254 edge->addPoint(C); edge->addPoint(G); edge->newLine();
00255 edge->addPoint(D); edge->addPoint(H);
00256 }
00257 }
00258 }
00259 }
00260
00261 #include <pluginlib/class_list_macros.h>
00262 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugin::BoundingBoxArrayDisplay, rviz::Display )