40 #include <rviz_common/display_context.hpp>
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);
56 context_->getSceneManager()->destroySceneNode(planning_scene_geometry_node_);
63 auto rs = std::make_shared<moveit::core::RobotState>(
scene->getCurrentState());
65 scene_robot_->updateKinematicState(rs);
71 render_shapes_->clear();
75 const Ogre::ColourValue& default_env_color,
76 const Ogre::ColourValue& default_attached_color,
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;
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)
104 Ogre::ColourValue color = default_env_color;
105 float alpha = default_scene_alpha;
106 if (
scene->hasObjectColor(
id))
108 const std_msgs::msg::ColorRGBA&
c =
scene->getObjectColor(
id);
115 for (std::size_t j = 0; j <
object->shapes_.size(); ++j)
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,
World::ObjectConstPtr ObjectConstPtr
Representation of a robot's state. This includes position, velocity, acceleration and effort.
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)
std::map< std::string, std_msgs::msg::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.