segment_array_display.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ryohei Ueda and JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "segment_array_display.h"
00036 #include <jsk_topic_tools/color_utils.h>
00037 namespace jsk_rviz_plugins
00038 {
00039   SegmentArrayDisplay::SegmentArrayDisplay()
00040   {
00041     coloring_property_ = new rviz::EnumProperty(
00042       "coloring", "Auto",
00043       "coloring method",
00044       this, SLOT(updateColoring()));
00045     coloring_property_->addOption("Auto", 0);
00046     coloring_property_->addOption("Flat color", 1);
00047 
00048     color_property_ = new rviz::ColorProperty(
00049       "color", QColor(25, 255, 0),
00050       "color to draw the edges",
00051       this, SLOT(updateColor()));
00052     alpha_property_ = new rviz::FloatProperty(
00053       "alpha", 0.8,
00054       "alpha value to draw the edges",
00055       this, SLOT(updateAlpha()));
00056     line_width_property_ = new rviz::FloatProperty(
00057       "line width", 0.005,
00058       "line width of the edges",
00059       this, SLOT(updateLineWidth()));
00060   }
00061 
00062   SegmentArrayDisplay::~SegmentArrayDisplay()
00063   {
00064     delete color_property_;
00065     delete alpha_property_;
00066     delete coloring_property_;
00067   }
00068 
00069   QColor SegmentArrayDisplay::getColor(size_t index)
00070   {
00071     if (coloring_method_ == "auto") {
00072       std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(index);
00073       return QColor(ros_color.r * 255.0,
00074                     ros_color.g * 255.0,
00075                     ros_color.b * 255.0,
00076                     ros_color.a * 255.0);
00077     }
00078     else if (coloring_method_ == "flat") {
00079       return color_;
00080     }
00081   }
00082 
00083   void SegmentArrayDisplay::onInitialize()
00084   {
00085     MFDClass::onInitialize();
00086     scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00087 
00088     updateColor();
00089     updateAlpha();
00090     updateColoring();
00091     updateLineWidth();
00092   }
00093 
00094   void SegmentArrayDisplay::updateLineWidth()
00095   {
00096     line_width_ = line_width_property_->getFloat();
00097     if (latest_msg_) {
00098       processMessage(latest_msg_);
00099     }
00100   }
00101 
00102   void SegmentArrayDisplay::updateColor()
00103   {
00104     color_ = color_property_->getColor();
00105     if (latest_msg_) {
00106       processMessage(latest_msg_);
00107     }
00108   }
00109 
00110   void SegmentArrayDisplay::updateAlpha()
00111   {
00112     alpha_ = alpha_property_->getFloat();
00113     if (latest_msg_) {
00114       processMessage(latest_msg_);
00115     }
00116   }
00117 
00118   void SegmentArrayDisplay::updateColoring()
00119   {
00120     if (coloring_property_->getOptionInt() == 0) {
00121       coloring_method_ = "auto";
00122       color_property_->hide();
00123     }
00124     else if (coloring_property_->getOptionInt() == 1) {
00125       coloring_method_ = "flat";
00126       color_property_->show();
00127     }
00128 
00129     if (latest_msg_) {
00130       processMessage(latest_msg_);
00131     }
00132   }
00133 
00134   void SegmentArrayDisplay::reset()
00135   {
00136     MFDClass::reset();
00137     edges_.clear();
00138     latest_msg_.reset();
00139   }
00140 
00141   void SegmentArrayDisplay::allocateBillboardLines(int num)
00142   {
00143     if (num > edges_.size()) {
00144       for (size_t i = edges_.size(); i < num; i++) {
00145         BillboardLinePtr line(new rviz::BillboardLine(
00146                                                       this->context_->getSceneManager(), this->scene_node_));
00147         edges_.push_back(line);
00148       }
00149     }
00150     else if (num < edges_.size())
00151       {
00152         edges_.resize(num);       // ok??
00153       }
00154   }
00155 
00156   void SegmentArrayDisplay::showEdges(
00157     const jsk_recognition_msgs::SegmentArray::ConstPtr& msg)
00158   {
00159     allocateBillboardLines(msg->segments.size());
00160     for (size_t i = 0; i < msg->segments.size(); i++) {
00161       jsk_recognition_msgs::Segment edge_msg = msg->segments[i];
00162 
00163       BillboardLinePtr edge = edges_[i];
00164       edge->clear();
00165 
00166       geometry_msgs::Pose start_pose_local;
00167       geometry_msgs::Pose end_pose_local;
00168       start_pose_local.position = edge_msg.start_point;
00169       start_pose_local.orientation.w = 1.0;
00170       end_pose_local.position = edge_msg.end_point;
00171       end_pose_local.orientation.w = 1.0;
00172 
00173       Ogre::Vector3 start_point;
00174       Ogre::Vector3 end_point;
00175       Ogre::Quaternion quaternion; // not used to visualize
00176       bool transform_ret;
00177       transform_ret =
00178         context_->getFrameManager()->transform(msg->header, start_pose_local, start_point, quaternion)
00179         && context_->getFrameManager()->transform(msg->header, end_pose_local, end_point, quaternion);
00180       if(!transform_ret) {
00181         ROS_ERROR( "Error transforming pose"
00182                    "'%s' from frame '%s' to frame '%s'",
00183                    qPrintable( getName() ), msg->header.frame_id.c_str(),
00184                    qPrintable( fixed_frame_ ));
00185         return;                 // return?
00186       }
00187       edge->addPoint(start_point);
00188       edge->addPoint(end_point);
00189       edge->setLineWidth(line_width_);
00190       QColor color = getColor(i);
00191       edge->setColor(color.red() / 255.0,
00192                      color.green() / 255.0,
00193                      color.blue() / 255.0,
00194                      alpha_);
00195     }
00196   }
00197 
00198   void SegmentArrayDisplay::processMessage(
00199     const jsk_recognition_msgs::SegmentArray::ConstPtr& msg)
00200   {
00201     // Store latest message
00202     latest_msg_ = msg;
00203 
00204     showEdges(msg);
00205   }
00206 }
00207 
00208 #include <pluginlib/class_list_macros.h>
00209 PLUGINLIB_EXPORT_CLASS(
00210   jsk_rviz_plugins::SegmentArrayDisplay, rviz::Display)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Wed May 1 2019 02:40:22