simple_occupancy_grid_array_display.cpp
Go to the documentation of this file.
1 // -*- mode: c++; -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 #define BOOST_PARAMETER_MAX_ARITY 7
37 #include <Eigen/Geometry>
40 #include "rviz_util.h"
42 
43 namespace jsk_rviz_plugins
44 {
46  {
48  "Auto Color", true,
49  "Auto coloring",
50  this, SLOT(updateAutoColor()));
51 
53  "Alpha", 1.0,
54  "Amount of transparency to apply to the polygon.",
55  this, SLOT(updateAlpha()));
56 
57  alpha_property_->setMin(0.0);
58  alpha_property_->setMax(1.0);
59 
60 
61  }
62 
64  {
65  delete alpha_property_;
67  }
68 
70  {
72  updateAlpha();
74  }
75 
77  {
78  if (num > clouds_.size()) { // need to allocate new node and clouds
79  for (size_t i = clouds_.size(); i < num; i++) {
80  Ogre::SceneNode* node = scene_node_->createChildSceneNode();
81  rviz::PointCloud* cloud = new rviz::PointCloud();
83  cloud->setCommonDirection( Ogre::Vector3::UNIT_Z );
84  cloud->setCommonUpVector( Ogre::Vector3::UNIT_Y );
85  node->attachObject(cloud);
86  clouds_.push_back(cloud);
87  nodes_.push_back(node);
88  }
89  }
90  else if (num < clouds_.size()) // need to destroy
91  {
92  for (size_t i = num; i < clouds_.size(); i++) {
93  nodes_[i]->detachObject(clouds_[i]);
94  delete clouds_[i];
95  scene_manager_->destroySceneNode(nodes_[i]);
96  }
97  clouds_.resize(num);
98  nodes_.resize(num);
99  }
100  }
101 
103  {
104  MFDClass::reset();
106  }
107 
109  const jsk_recognition_msgs::SimpleOccupancyGridArray::ConstPtr& msg)
110  {
111  Ogre::ColourValue white(1, 1, 1, 1);
112  allocateCloudsAndNodes(msg->grids.size()); // not enough
113  for (size_t i = 0; i < msg->grids.size(); i++) {
114  Ogre::SceneNode* node = nodes_[i];
115  rviz::PointCloud* cloud = clouds_[i];
116  const jsk_recognition_msgs::SimpleOccupancyGrid grid = msg->grids[i];
117  Ogre::Vector3 position;
118  Ogre::Quaternion quaternion;
119 
120  // coefficients
121  geometry_msgs::Pose plane_pose;
123  Eigen::Affine3f plane_pose_eigen = plane->coordinates();
124  tf::poseEigenToMsg(plane_pose_eigen, plane_pose);
125  if(!context_->getFrameManager()->transform(grid.header, plane_pose,
126  position,
127  quaternion)) {
128  std::ostringstream oss;
129  oss << "Error transforming pose";
130  oss << " from frame '" << grid.header.frame_id << "'";
131  oss << " to frame '" << qPrintable(fixed_frame_) << "'";
132  ROS_ERROR_STREAM(oss.str());
133  setStatus(rviz::StatusProperty::Error, "Transform", QString::fromStdString(oss.str()));
134  return;
135  }
136  node->setPosition(position);
137  node->setOrientation(quaternion);
138  cloud->setDimensions(grid.resolution, grid.resolution, 0.0);
139  std::vector<rviz::PointCloud::Point> points;
140  for (size_t ci = 0; ci < grid.cells.size(); ci++) {
141  const geometry_msgs::Point p = grid.cells[ci];
143  if (auto_color_) {
145  }
146  else {
147  point.color = white;
148  }
149  point.position.x = p.x;
150  point.position.y = p.y;
151  point.position.z = p.z;
152  points.push_back(point);
153  }
154  cloud->clear();
155  cloud->setAlpha(alpha_);
156  if (!points.empty()) {
157  cloud->addPoints(&points.front(), points.size());
158  }
159  }
161  }
162 
164  {
166  }
167 
169  {
171  }
172 
173 }
174 
177 
void addPoints(Point *points, uint32_t num_points)
void setMin(float min)
void setMax(float max)
void setRenderMode(RenderMode mode)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
Ogre::SceneNode * scene_node_
Ogre::ColourValue colorMsgToOgre(const std_msgs::ColorRGBA &c)
Definition: rviz_util.h:46
QString fixed_frame_
void setCommonUpVector(const Ogre::Vector3 &vec)
void setDimensions(float width, float height, float depth)
Ogre::Vector3 position
bool transform(const Header &header, const geometry_msgs::Pose &pose, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
virtual FrameManager * getFrameManager() const=0
void setAlpha(float alpha, bool per_point_alpha=false)
Ogre::SceneManager * scene_manager_
void processMessage(const jsk_recognition_msgs::SimpleOccupancyGridArray::ConstPtr &msg)
virtual void queueRender()=0
std_msgs::ColorRGBA colorCategory20(int i)
Ogre::ColourValue color
virtual float getFloat() const
void setCommonDirection(const Ogre::Vector3 &vec)
#define ROS_ERROR_STREAM(args)
virtual bool getBool() const
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58