people_position_measurement_array_display.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "people_position_measurement_array_display.h"
00037 #include <rviz/uniform_string_stream.h>
00038 #include <rviz/view_manager.h>
00039 #include <rviz/render_panel.h>
00040 #include <OGRE/OgreCamera.h>
00041 #include <QPainter>
00042 #include <rviz/ogre_helpers/render_system.h>
00043 #include <OGRE/OgreRenderSystem.h>
00044 
00045 #include <algorithm>
00046 #include <boost/lambda/lambda.hpp>
00047 
00048 namespace jsk_rviz_plugins
00049 {
00050   PeoplePositionMeasurementArrayDisplay::PeoplePositionMeasurementArrayDisplay()
00051   {
00052     size_property_ = new rviz::FloatProperty("size", 0.3,
00053                                              "size of the visualizer", this,
00054                                              SLOT(updateSize()));
00055     timeout_property_ = new rviz::FloatProperty(
00056       "timeout", 10.0, "timeout seconds", this, SLOT(updateTimeout()));
00057     anonymous_property_ = new rviz::BoolProperty(
00058       "anonymous", false,
00059       "anonymous",
00060       this, SLOT(updateAnonymous()));
00061     text_property_ = new rviz::StringProperty(
00062       "text", "person found here person found here",
00063       "text to rotate",
00064       this, SLOT(updateText()));
00065   }
00066 
00067   
00068   PeoplePositionMeasurementArrayDisplay::~PeoplePositionMeasurementArrayDisplay()
00069   {
00070     delete size_property_;
00071   }
00072 
00073   void PeoplePositionMeasurementArrayDisplay::onInitialize()
00074   {
00075     MFDClass::onInitialize();
00076     scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00077     updateSize();
00078     updateTimeout();
00079     updateAnonymous();
00080     updateText();
00081   }
00082 
00083   void PeoplePositionMeasurementArrayDisplay::clearObjects()
00084   {
00085     faces_.clear();
00086     visualizers_.clear();
00087   }
00088   
00089   void PeoplePositionMeasurementArrayDisplay::reset()
00090   {
00091     MFDClass::reset();
00092     clearObjects();
00093   }
00094 
00095   void PeoplePositionMeasurementArrayDisplay::processMessage(
00096     const people_msgs::PositionMeasurementArray::ConstPtr& msg)
00097   {
00098     boost::mutex::scoped_lock lock(mutex_);
00099     static int count = 0;
00100     static int square_count = 0;
00101     faces_ = msg->people;
00102     // check texture is ready or not
00103     if (faces_.size() > visualizers_.size()) {
00104       for (size_t i = visualizers_.size(); i < faces_.size(); i++) {
00105         visualizers_.push_back(GISCircleVisualizer::Ptr(new GISCircleVisualizer(
00106                                                           scene_manager_,
00107                                                           scene_node_,
00108                                                           size_,
00109                                                           text_)));
00110         visualizers_[visualizers_.size() - 1]->setAnonymous(anonymous_);
00111         visualizers_[visualizers_.size() - 1]->update(0, 0);
00112         QColor color(25.0, 255.0, 240.0);
00113         visualizers_[visualizers_.size() - 1]->setColor(color);
00114       }
00115     }
00116     else {
00117       visualizers_.resize(faces_.size());
00118     }
00119     // move scene_node
00120     for (size_t i = 0; i < faces_.size(); i++) {
00121       Ogre::Quaternion orientation;
00122       Ogre::Vector3 position;
00123       geometry_msgs::Pose pose;
00124       pose.position = faces_[i].pos;
00125       pose.orientation.w = 1.0;
00126       if(!context_->getFrameManager()->transform(msg->header,
00127                                                  pose,
00128                                                  position, orientation))
00129       {
00130         ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
00131                    msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
00132         
00133       }
00134       else {
00135         visualizers_[i]->setPosition(position);
00136       }
00137     }
00138     latest_time_ = msg->header.stamp;
00139   }
00140 
00141   void PeoplePositionMeasurementArrayDisplay::update(
00142     float wall_dt, float ros_dt)
00143   {
00144     boost::mutex::scoped_lock lock(mutex_);
00145     if (faces_.size() == 0) {
00146       return;
00147     }
00148     if ((ros::Time::now() - latest_time_).toSec() > timeout_) {
00149       ROS_WARN("timeout face recognition result");
00150       clearObjects();
00151       return;
00152     }
00153     for (size_t i = 0; i < visualizers_.size(); i++) {
00154       visualizers_[i]->setOrientation(context_);
00155     }
00156     for (size_t i = 0; i < visualizers_.size(); i++) {
00157       visualizers_[i]->update(wall_dt, ros_dt);
00158     }
00159   }
00160 
00161   void PeoplePositionMeasurementArrayDisplay::updateTimeout()
00162   {
00163     boost::mutex::scoped_lock lock(mutex_);
00164     timeout_ = timeout_property_->getFloat();
00165   }
00166   
00167   void PeoplePositionMeasurementArrayDisplay::updateSize()
00168   {
00169     boost::mutex::scoped_lock lock(mutex_);
00170     size_ = size_property_->getFloat();
00171     visualizers_.clear();
00172   }
00173 
00174   void PeoplePositionMeasurementArrayDisplay::updateAnonymous()
00175   {
00176     boost::mutex::scoped_lock lock(mutex_);
00177     anonymous_ = anonymous_property_->getBool();
00178     for (size_t i = 0; i < visualizers_.size(); i++) {
00179       visualizers_[i]->setAnonymous(anonymous_);
00180     }
00181   }
00182 
00183   void PeoplePositionMeasurementArrayDisplay::updateText()
00184   {
00185     boost::mutex::scoped_lock lock(mutex_);
00186     text_ = text_property_->getStdString();
00187     for (size_t i = 0; i < visualizers_.size(); i++) {
00188       visualizers_[i]->setText(text_);
00189     }
00190   }
00191   
00192 }
00193 
00194 
00195 #include <pluginlib/class_list_macros.h>
00196 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::PeoplePositionMeasurementArrayDisplay, rviz::Display )
00197 


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03