robot_state_visualization.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
40 #include <QApplication>
41 
42 namespace moveit_rviz_plugin
43 {
45  const std::string& name, rviz::Property* parent_property)
46  : robot_(root_node, context, name, parent_property)
47  , octree_voxel_render_mode_(OCTOMAP_OCCUPIED_VOXELS)
48  , octree_voxel_color_mode_(OCTOMAP_Z_AXIS_COLOR)
49  , visible_(true)
50  , visual_visible_(true)
51  , collision_visible_(false)
52 {
57  render_shapes_.reset(new RenderShapes(context));
58 }
59 
60 void RobotStateVisualization::load(const urdf::ModelInterface& descr, bool visual, bool collision)
61 {
62  // clear previously loaded model
63  clear();
64 
65  robot_.load(descr, visual, collision);
69  QApplication::processEvents();
70 }
71 
73 {
74  render_shapes_->clear();
75  robot_.clear();
76 }
77 
78 void RobotStateVisualization::setDefaultAttachedObjectColor(const std_msgs::ColorRGBA& default_attached_object_color)
79 {
80  default_attached_object_color_ = default_attached_object_color;
81 }
82 
83 void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state)
84 {
86 }
87 
88 void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state,
89  const std_msgs::ColorRGBA& default_attached_object_color)
90 {
91  updateHelper(kinematic_state, default_attached_object_color, NULL);
92 }
93 
94 void RobotStateVisualization::update(const robot_state::RobotStateConstPtr& kinematic_state,
95  const std_msgs::ColorRGBA& default_attached_object_color,
96  const std::map<std::string, std_msgs::ColorRGBA>& color_map)
97 {
98  updateHelper(kinematic_state, default_attached_object_color, &color_map);
99 }
100 
101 void RobotStateVisualization::updateHelper(const robot_state::RobotStateConstPtr& kinematic_state,
102  const std_msgs::ColorRGBA& default_attached_object_color,
103  const std::map<std::string, std_msgs::ColorRGBA>* color_map)
104 {
105  robot_.update(PlanningLinkUpdater(kinematic_state));
106  render_shapes_->clear();
107 
108  std::vector<const robot_state::AttachedBody*> attached_bodies;
109  kinematic_state->getAttachedBodies(attached_bodies);
110  for (std::size_t i = 0; i < attached_bodies.size(); ++i)
111  {
112  std_msgs::ColorRGBA color = default_attached_object_color;
113  float alpha = robot_.getAlpha();
114  if (color_map)
115  {
116  std::map<std::string, std_msgs::ColorRGBA>::const_iterator it = color_map->find(attached_bodies[i]->getName());
117  if (it != color_map->end())
118  { // render attached bodies with a color that is a bit different
119  color.r = std::max(1.0f, it->second.r * 1.05f);
120  color.g = std::max(1.0f, it->second.g * 1.05f);
121  color.b = std::max(1.0f, it->second.b * 1.05f);
122  alpha = color.a = it->second.a;
123  }
124  }
125  rviz::Color rcolor(color.r, color.g, color.b);
126  const EigenSTL::vector_Affine3d& ab_t = attached_bodies[i]->getGlobalCollisionBodyTransforms();
127  const std::vector<shapes::ShapeConstPtr>& ab_shapes = attached_bodies[i]->getShapes();
128  for (std::size_t j = 0; j < ab_shapes.size(); ++j)
129  {
130  render_shapes_->renderShape(robot_.getVisualNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_,
131  octree_voxel_color_mode_, rcolor, alpha);
132  render_shapes_->renderShape(robot_.getCollisionNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_,
133  octree_voxel_color_mode_, rcolor, alpha);
134  }
135  }
139 }
140 
142 {
143  visible_ = visible;
144  robot_.setVisible(visible);
145 }
146 
148 {
149  visual_visible_ = visible;
150  robot_.setVisualVisible(visible);
151 }
152 
154 {
155  collision_visible_ = visible;
156  robot_.setCollisionVisible(visible);
157 }
158 
160 {
161  robot_.setAlpha(alpha);
162 }
163 }
virtual void update(const LinkUpdater &updater)
#define NULL
void setDefaultAttachedObjectColor(const std_msgs::ColorRGBA &default_attached_object_color)
virtual void clear()
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void setAlpha(float a)
void setVisualVisible(bool visible)
Set whether the visual meshes of the robot should be visible.
void setCollisionVisible(bool visible)
Set whether the collision meshes/primitives of the robot should be visible.
void load(const urdf::ModelInterface &descr, bool visual=true, bool collision=true)
f
std::string getName(void *handle)
virtual void load(const urdf::ModelInterface &urdf, bool visual=true, bool collision=true)
Ogre::SceneNode * getCollisionNode()
void setCollisionVisible(bool visible)
void setVisible(bool visible)
Set the robot as a whole to be visible or not.
float getAlpha()
Ogre::SceneNode * getVisualNode()
void update(const robot_state::RobotStateConstPtr &kinematic_state)
void updateHelper(const robot_state::RobotStateConstPtr &kinematic_state, const std_msgs::ColorRGBA &default_attached_object_color, const std::map< std::string, std_msgs::ColorRGBA > *color_map)
void setVisualVisible(bool visible)
RobotStateVisualization(Ogre::SceneNode *root_node, rviz::DisplayContext *context, const std::string &name, rviz::Property *parent_property)
virtual void setVisible(bool visible)
Update the links of an rviz::Robot using a robot_state::RobotState.


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:19:09