42 #include <OgreSceneNode.h>
43 #include <OgreSceneManager.h>
48 const RobotStateVisualizationPtr& robot)
49 : planning_scene_geometry_node_(node->createChildSceneNode()), context_(context), scene_robot_(robot)
51 render_shapes_ = std::make_shared<RenderShapes>(context);
63 auto rs = std::make_shared<moveit::core::RobotState>(scene->getCurrentState());
90 std_msgs::ColorRGBA color;
91 color.r = default_attached_color.
r_;
92 color.g = default_attached_color.
g_;
93 color.b = default_attached_color.
b_;
96 scene->getKnownObjectColors(color_map);
97 scene_robot_->update(moveit::core::RobotStateConstPtr(rs), color, color_map);
100 const std::vector<std::string>& ids = scene->getWorld()->getObjectIds();
101 for (
const std::string&
id : ids)
105 float alpha = default_scene_alpha;
106 if (scene->hasObjectColor(
id))
108 const std_msgs::ColorRGBA&
c = scene->getObjectColor(
id);
114 for (std::size_t j = 0; j <
object->shapes_.size(); ++j)
117 object->global_shape_poses_[j], octree_voxel_rendering, octree_color_mode, color,