moveit2
The MoveIt Motion Planning Framework for ROS 2.
planning_scene_render.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 <rviz_common/display_context.hpp>
41 
42 #include <OgreSceneNode.h>
43 #include <OgreSceneManager.h>
44 
45 namespace moveit_rviz_plugin
46 {
47 PlanningSceneRender::PlanningSceneRender(Ogre::SceneNode* node, rviz_common::DisplayContext* context,
48  const RobotStateVisualizationPtr& robot)
49  : planning_scene_geometry_node_(node->createChildSceneNode()), context_(context), scene_robot_(robot)
50 {
51  render_shapes_ = std::make_shared<RenderShapes>(context);
52 }
53 
55 {
56  context_->getSceneManager()->destroySceneNode(planning_scene_geometry_node_);
57 }
58 
59 void PlanningSceneRender::updateRobotPosition(const planning_scene::PlanningSceneConstPtr& scene)
60 {
61  if (scene_robot_)
62  {
63  auto rs = std::make_shared<moveit::core::RobotState>(scene->getCurrentState());
64  rs->update();
65  scene_robot_->updateKinematicState(rs);
66  }
67 }
68 
70 {
71  render_shapes_->clear();
72 }
73 
74 void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningSceneConstPtr& scene,
75  const Ogre::ColourValue& default_env_color,
76  const Ogre::ColourValue& default_attached_color,
77  OctreeVoxelRenderMode octree_voxel_rendering,
78  OctreeVoxelColorMode octree_color_mode, float default_scene_alpha)
79 {
80  if (!scene)
81  return;
82 
83  clear();
84 
85  if (scene_robot_)
86  {
87  moveit::core::RobotState* rs = new moveit::core::RobotState(scene->getCurrentState());
88  rs->update();
89 
90  std_msgs::msg::ColorRGBA color;
91  color.r = default_attached_color.r;
92  color.g = default_attached_color.g;
93  color.b = default_attached_color.b;
94  color.a = 1.0f;
96  scene->getKnownObjectColors(color_map);
97  scene_robot_->update(moveit::core::RobotStateConstPtr(rs), color, color_map);
98  }
99 
100  const std::vector<std::string>& ids = scene->getWorld()->getObjectIds();
101  for (const std::string& id : ids)
102  {
103  collision_detection::CollisionEnv::ObjectConstPtr object = scene->getWorld()->getObject(id);
104  Ogre::ColourValue color = default_env_color;
105  float alpha = default_scene_alpha;
106  if (scene->hasObjectColor(id))
107  {
108  const std_msgs::msg::ColorRGBA& c = scene->getObjectColor(id);
109  color.r = c.r;
110  color.g = c.g;
111  color.b = c.b;
112  color.a = c.a;
113  alpha = c.a;
114  }
115  for (std::size_t j = 0; j < object->shapes_.size(); ++j)
116  {
117  render_shapes_->renderShape(planning_scene_geometry_node_, object->shapes_[j].get(),
118  object->global_shape_poses_[j], octree_voxel_rendering, octree_color_mode, color,
119  alpha);
120  }
121  }
122 }
123 } // namespace moveit_rviz_plugin
World::ObjectConstPtr ObjectConstPtr
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void update(bool force=false)
Update all transforms.
void renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene, const Ogre::ColourValue &default_scene_color, const Ogre::ColourValue &default_attached_color, OctreeVoxelRenderMode voxel_render_mode, OctreeVoxelColorMode voxel_color_mode, float default_scene_alpha)
PlanningSceneRender(Ogre::SceneNode *root_node, rviz_common::DisplayContext *context, const RobotStateVisualizationPtr &robot)
void updateRobotPosition(const planning_scene::PlanningSceneConstPtr &scene)
scene
Definition: pick.py:52
robot
Definition: pick.py:53
std::map< std::string, std_msgs::msg::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.