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 #ifndef JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_COMMON_H_
00036 #define JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_COMMON_H_
00037
00038 #ifndef Q_MOC_RUN
00039 #include "bounding_box_display_common.h"
00040 #include <jsk_recognition_msgs/BoundingBoxArray.h>
00041 #include <jsk_topic_tools/color_utils.h>
00042 #include <rviz/properties/color_property.h>
00043 #include <rviz/properties/bool_property.h>
00044 #include <rviz/properties/float_property.h>
00045 #include <rviz/properties/enum_property.h>
00046 #include <rviz/message_filter_display.h>
00047 #include <rviz/ogre_helpers/shape.h>
00048 #include <rviz/ogre_helpers/billboard_line.h>
00049 #include <rviz/ogre_helpers/arrow.h>
00050 #include <OGRE/OgreSceneNode.h>
00051 #endif
00052
00053 namespace jsk_rviz_plugins
00054 {
00055
00056 template <class MessageType>
00057 class BoundingBoxDisplayCommon: public rviz::MessageFilterDisplay<MessageType>
00058 {
00059 public:
00060 BoundingBoxDisplayCommon() {};
00061 ~BoundingBoxDisplayCommon() {};
00062 typedef std::shared_ptr<rviz::Shape> ShapePtr;
00063 typedef std::shared_ptr<rviz::BillboardLine> BillboardLinePtr;
00064 typedef std::shared_ptr<rviz::Arrow> ArrowPtr;
00065
00066 protected:
00067 QColor color_;
00068 std::string coloring_method_;
00069 double alpha_;
00070 double line_width_;
00071
00072 std::vector<std::vector<ArrowPtr> > coords_objects_;
00073 std::vector<Ogre::SceneNode*> coords_nodes_;
00074 std::vector<BillboardLinePtr> edges_;
00075 std::vector<ShapePtr> shapes_;
00076
00077 QColor getColor(
00078 size_t index,
00079 const jsk_recognition_msgs::BoundingBox& box,
00080 double min_value,
00081 double max_value)
00082 {
00083 if (coloring_method_ == "auto") {
00084 std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(index);
00085 return QColor(ros_color.r * 255.0,
00086 ros_color.g * 255.0,
00087 ros_color.b * 255.0,
00088 ros_color.a * 255.0);
00089 }
00090 else if (coloring_method_ == "flat") {
00091 return color_;
00092 }
00093 else if (coloring_method_ == "label") {
00094 std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(box.label);
00095 return QColor(ros_color.r * 255.0,
00096 ros_color.g * 255.0,
00097 ros_color.b * 255.0,
00098 ros_color.a * 255.0);
00099 }
00100 else if (coloring_method_ == "value") {
00101 if (min_value != max_value) {
00102 std_msgs::ColorRGBA ros_color = jsk_topic_tools::heatColor((box.value - min_value) / (max_value - min_value));
00103 return QColor(ros_color.r * 255.0,
00104 ros_color.g * 255.0,
00105 ros_color.b * 255.0,
00106 ros_color.a * 255.0);
00107 }
00108 else {
00109 return QColor(255.0, 255.0, 255.0, 255.0);
00110 }
00111 }
00112 }
00113
00114 bool isValidBoundingBox(
00115 const jsk_recognition_msgs::BoundingBox box_msg)
00116 {
00117
00118 if (box_msg.dimensions.x < 1.0e-9 ||
00119 box_msg.dimensions.y < 1.0e-9 ||
00120 box_msg.dimensions.z < 1.0e-9 ||
00121 std::isnan(box_msg.dimensions.x) ||
00122 std::isnan(box_msg.dimensions.y) ||
00123 std::isnan(box_msg.dimensions.z)) {
00124 return false;
00125 }
00126 return true;
00127 }
00128
00129 void allocateShapes(int num)
00130 {
00131 if (num > shapes_.size()) {
00132 for (size_t i = shapes_.size(); i < num; i++) {
00133 ShapePtr shape (new rviz::Shape(
00134 rviz::Shape::Cube, this->context_->getSceneManager(),
00135 this->scene_node_));
00136 shapes_.push_back(shape);
00137 }
00138 }
00139 else if (num < shapes_.size())
00140 {
00141 shapes_.resize(num);
00142 }
00143 }
00144
00145 void allocateBillboardLines(int num)
00146 {
00147 if (num > edges_.size()) {
00148 for (size_t i = edges_.size(); i < num; i++) {
00149 BillboardLinePtr line(new rviz::BillboardLine(
00150 this->context_->getSceneManager(), this->scene_node_));
00151 edges_.push_back(line);
00152 }
00153 }
00154 else if (num < edges_.size())
00155 {
00156 edges_.resize(num);
00157 }
00158 }
00159
00160 void allocateCoords(int num)
00161 {
00162 if (num > coords_objects_.size()) {
00163 for (size_t i = coords_objects_.size(); i < num; i++) {
00164 Ogre::SceneNode* scene_node = this->scene_node_->createChildSceneNode();
00165 std::vector<ArrowPtr> coord;
00166 for (int i = 0; i < 3; i++) {
00167 ArrowPtr arrow (new rviz::Arrow(this->scene_manager_, scene_node));
00168 coord.push_back(arrow);
00169 }
00170 coords_nodes_.push_back(scene_node);
00171 coords_objects_.push_back(coord);
00172 }
00173 }
00174 else if (num < coords_objects_.size()) {
00175 for (size_t i = num; i < coords_objects_.size(); i++) {
00176
00177
00178
00179
00180 coords_nodes_[i]->setVisible(false);
00181 }
00182 coords_objects_.resize(num);
00183 coords_nodes_.resize(num);
00184 }
00185 }
00186
00187 void showBoxes(
00188 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg)
00189 {
00190 edges_.clear();
00191 allocateShapes(msg->boxes.size());
00192 float min_value = DBL_MAX;
00193 float max_value = -DBL_MAX;
00194 for (size_t i = 0; i < msg->boxes.size(); i++) {
00195 min_value = std::min(min_value, msg->boxes[i].value);
00196 max_value = std::max(max_value, msg->boxes[i].value);
00197 }
00198 for (size_t i = 0; i < msg->boxes.size(); i++) {
00199 jsk_recognition_msgs::BoundingBox box = msg->boxes[i];
00200 if (!isValidBoundingBox(box)) {
00201 ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]",
00202 box.dimensions.x, box.dimensions.y, box.dimensions.z);
00203 continue;
00204 }
00205
00206 ShapePtr shape = shapes_[i];
00207 Ogre::Vector3 position;
00208 Ogre::Quaternion orientation;
00209 if(!this->context_->getFrameManager()->transform(
00210 box.header, box.pose, position, orientation)) {
00211 std::ostringstream oss;
00212 oss << "Error transforming pose";
00213 oss << " from frame '" << box.header.frame_id << "'";
00214 oss << " to frame '" << qPrintable(this->fixed_frame_) << "'";
00215 ROS_ERROR_STREAM(oss.str());
00216 this->setStatus(rviz::StatusProperty::Error, "Transform", QString::fromStdString(oss.str()));
00217 return;
00218 }
00219
00220
00221
00222
00223
00224
00225
00226
00227 shape->setPosition(position);
00228 shape->setOrientation(orientation);
00229 Ogre::Vector3 dimensions;
00230 dimensions[0] = box.dimensions.x;
00231 dimensions[1] = box.dimensions.y;
00232 dimensions[2] = box.dimensions.z;
00233 shape->setScale(dimensions);
00234 QColor color = getColor(i, box, min_value, max_value);
00235 shape->setColor(color.red() / 255.0,
00236 color.green() / 255.0,
00237 color.blue() / 255.0,
00238 alpha_);
00239 }
00240 }
00241
00242 void showEdges(
00243 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg)
00244 {
00245 shapes_.clear();
00246 allocateBillboardLines(msg->boxes.size());
00247 float min_value = DBL_MAX;
00248 float max_value = -DBL_MAX;
00249 for (size_t i = 0; i < msg->boxes.size(); i++) {
00250 min_value = std::min(min_value, msg->boxes[i].value);
00251 max_value = std::max(max_value, msg->boxes[i].value);
00252 }
00253
00254 for (size_t i = 0; i < msg->boxes.size(); i++) {
00255 jsk_recognition_msgs::BoundingBox box = msg->boxes[i];
00256 if (!isValidBoundingBox(box)) {
00257 ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]",
00258 box.dimensions.x, box.dimensions.y, box.dimensions.z);
00259 continue;
00260 }
00261
00262 geometry_msgs::Vector3 dimensions = box.dimensions;
00263
00264 BillboardLinePtr edge = edges_[i];
00265 edge->clear();
00266 Ogre::Vector3 position;
00267 Ogre::Quaternion quaternion;
00268 if(!this->context_->getFrameManager()->transform(box.header, box.pose,
00269 position,
00270 quaternion)) {
00271 std::ostringstream oss;
00272 oss << "Error transforming pose";
00273 oss << " from frame '" << box.header.frame_id << "'";
00274 oss << " to frame '" << qPrintable(this->fixed_frame_) << "'";
00275 ROS_ERROR_STREAM(oss.str());
00276 this->setStatus(rviz::StatusProperty::Error, "Transform", QString::fromStdString(oss.str()));
00277 return;
00278 }
00279 edge->setPosition(position);
00280 edge->setOrientation(quaternion);
00281
00282 edge->setMaxPointsPerLine(2);
00283 edge->setNumLines(12);
00284 edge->setLineWidth(line_width_);
00285 QColor color = getColor(i, box, min_value, max_value);
00286 edge->setColor(color.red() / 255.0,
00287 color.green() / 255.0,
00288 color.blue() / 255.0,
00289 alpha_);
00290
00291 Ogre::Vector3 A, B, C, D, E, F, G, H;
00292 A[0] = dimensions.x / 2.0;
00293 A[1] = dimensions.y / 2.0;
00294 A[2] = dimensions.z / 2.0;
00295 B[0] = - dimensions.x / 2.0;
00296 B[1] = dimensions.y / 2.0;
00297 B[2] = dimensions.z / 2.0;
00298 C[0] = - dimensions.x / 2.0;
00299 C[1] = - dimensions.y / 2.0;
00300 C[2] = dimensions.z / 2.0;
00301 D[0] = dimensions.x / 2.0;
00302 D[1] = - dimensions.y / 2.0;
00303 D[2] = dimensions.z / 2.0;
00304
00305 E[0] = dimensions.x / 2.0;
00306 E[1] = dimensions.y / 2.0;
00307 E[2] = - dimensions.z / 2.0;
00308 F[0] = - dimensions.x / 2.0;
00309 F[1] = dimensions.y / 2.0;
00310 F[2] = - dimensions.z / 2.0;
00311 G[0] = - dimensions.x / 2.0;
00312 G[1] = - dimensions.y / 2.0;
00313 G[2] = - dimensions.z / 2.0;
00314 H[0] = dimensions.x / 2.0;
00315 H[1] = - dimensions.y / 2.0;
00316 H[2] = - dimensions.z / 2.0;
00317
00318 edge->addPoint(A); edge->addPoint(B); edge->newLine();
00319 edge->addPoint(B); edge->addPoint(C); edge->newLine();
00320 edge->addPoint(C); edge->addPoint(D); edge->newLine();
00321 edge->addPoint(D); edge->addPoint(A); edge->newLine();
00322 edge->addPoint(E); edge->addPoint(F); edge->newLine();
00323 edge->addPoint(F); edge->addPoint(G); edge->newLine();
00324 edge->addPoint(G); edge->addPoint(H); edge->newLine();
00325 edge->addPoint(H); edge->addPoint(E); edge->newLine();
00326 edge->addPoint(A); edge->addPoint(E); edge->newLine();
00327 edge->addPoint(B); edge->addPoint(F); edge->newLine();
00328 edge->addPoint(C); edge->addPoint(G); edge->newLine();
00329 edge->addPoint(D); edge->addPoint(H);
00330 }
00331 }
00332
00333 void showCoords(
00334 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg)
00335 {
00336 allocateCoords(msg->boxes.size());
00337 for (size_t i = 0; i < msg->boxes.size(); i++) {
00338 jsk_recognition_msgs::BoundingBox box = msg->boxes[i];
00339 std::vector<ArrowPtr> coord = coords_objects_[i];
00340
00341 Ogre::SceneNode* scene_node = coords_nodes_[i];
00342 scene_node->setVisible(true);
00343 Ogre::Vector3 position;
00344 Ogre::Quaternion orientation;
00345 if(!this->context_->getFrameManager()->getTransform(
00346 box.header, position, orientation)) {
00347 ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
00348 box.header.frame_id.c_str(), qPrintable(this->fixed_frame_));
00349 return;
00350 }
00351 scene_node->setPosition(position);
00352 scene_node->setOrientation(orientation);
00353
00354 float color[3][3] = {{1, 0, 0},
00355 {0, 1, 0},
00356 {0, 0, 1}};
00357 for (int j = 0; j < 3; j++) {
00358
00359 Ogre::Vector3 scale;
00360 if (color[j][0] == 1) {
00361 scale = Ogre::Vector3(
00362 box.dimensions.x,
00363 std::min(box.dimensions.y, box.dimensions.z),
00364 std::min(box.dimensions.y, box.dimensions.z));
00365 }
00366 if (color[j][1] == 1) {
00367 scale = Ogre::Vector3(
00368 box.dimensions.y,
00369 std::min(box.dimensions.x, box.dimensions.z),
00370 std::min(box.dimensions.x, box.dimensions.z));
00371 }
00372 if (color[j][2] == 1) {
00373 scale = Ogre::Vector3(
00374 box.dimensions.z,
00375 std::min(box.dimensions.x, box.dimensions.y),
00376 std::min(box.dimensions.x, box.dimensions.y));
00377 }
00378
00379 Ogre::Vector3 direction(color[j][0], color[j][1], color[j][2]);
00380 Ogre::Vector3 pos(box.pose.position.x,
00381 box.pose.position.y,
00382 box.pose.position.z);
00383 Ogre::Quaternion qua(box.pose.orientation.w,
00384 box.pose.orientation.x,
00385 box.pose.orientation.y,
00386 box.pose.orientation.z);
00387 direction = qua * direction;
00388 Ogre::ColourValue rgba;
00389 rgba.a = 1;
00390 rgba.r = color[j][0];
00391 rgba.g = color[j][1];
00392 rgba.b = color[j][2];
00393
00394 ArrowPtr arrow = coords_objects_[i][j];
00395 arrow->setPosition(pos);
00396 arrow->setDirection(direction);
00397 arrow->setScale(scale);
00398 arrow->setColor(rgba);
00399 }
00400 }
00401 }
00402
00403 void hideCoords()
00404 {
00405 for (size_t i = 0; i < coords_nodes_.size(); i++) {
00406 coords_nodes_[i]->setVisible(false);
00407 }
00408 }
00409
00410 };
00411
00412 }
00413
00414 #endif // JSK_RVIZ_PLUGINS_BOUDNING_BOX_DISPLAY_COMMON_H_