40#include <rviz_common/properties/parse_color.hpp>
41#include <rviz_default_plugins/robot/robot_link.hpp>
42#include <QApplication>
51 return moveit::getLogger(
"moveit.ros.rviz_plugin_render_tools.robot_state_visualization");
56 const std::string& name,
57 rviz_common::properties::Property* parent_property)
58 : robot_(root_node, context, name, parent_property)
62 , visual_visible_(true)
63 , collision_visible_(false)
65 default_attached_object_color_.r = 0.0f;
66 default_attached_object_color_.g = 0.7f;
67 default_attached_object_color_.b = 0.0f;
68 default_attached_object_color_.a = 1.0f;
69 render_shapes_ = std::make_shared<RenderShapes>(context);
77 robot_.load(descr, visual, collision);
78 robot_.setVisualVisible(visual_visible_);
79 robot_.setCollisionVisible(collision_visible_);
80 robot_.setVisible(visible_);
85 render_shapes_->clear();
91 default_attached_object_color_ = default_attached_object_color;
96 render_shapes_->updateShapeColors(attached_object_color.r, attached_object_color.g, attached_object_color.b,
102 updateHelper(robot_state, default_attached_object_color_,
nullptr);
106 const std_msgs::msg::ColorRGBA& default_attached_object_color)
108 updateHelper(robot_state, default_attached_object_color,
nullptr);
112 const std_msgs::msg::ColorRGBA& default_attached_object_color,
113 const std::map<std::string, std_msgs::msg::ColorRGBA>& color_map)
115 updateHelper(robot_state, default_attached_object_color, &color_map);
118void RobotStateVisualization::updateHelper(
const moveit::core::RobotStateConstPtr& robot_state,
119 const std_msgs::msg::ColorRGBA& default_attached_object_color,
120 const std::map<std::string, std_msgs::msg::ColorRGBA>* color_map)
123 render_shapes_->clear();
125 std::vector<const moveit::core::AttachedBody*> attached_bodies;
126 robot_state->getAttachedBodies(attached_bodies);
129 std_msgs::msg::ColorRGBA color = default_attached_object_color;
130 double alpha = robot_.getAlpha();
133 std::map<std::string, std_msgs::msg::ColorRGBA>::const_iterator it = color_map->find(attached_body->getName());
134 if (it != color_map->end())
136 color.r = std::max(1.0f, it->second.r * 1.05f);
137 color.g = std::max(1.0f, it->second.g * 1.05f);
138 color.b = std::max(1.0f, it->second.b * 1.05f);
139 alpha = color.a = it->second.a;
142 rviz_default_plugins::robot::RobotLink* link = robot_.getLink(attached_body->getAttachedLinkName());
145 RCLCPP_ERROR_STREAM(
getLogger(),
"Link " << attached_body->getAttachedLinkName() <<
" not found in rviz::Robot");
148 Ogre::ColourValue rcolor(color.r, color.g, color.b);
149 const EigenSTL::vector_Isometry3d& ab_t = attached_body->getShapePosesInLinkFrame();
150 const std::vector<shapes::ShapeConstPtr>& ab_shapes = attached_body->getShapes();
151 for (std::size_t j = 0; j < ab_shapes.size(); ++j)
153 render_shapes_->renderShape(link->getVisualNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_,
154 octree_voxel_color_mode_, rcolor, alpha);
155 render_shapes_->renderShape(link->getCollisionNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_,
156 octree_voxel_color_mode_, rcolor, alpha);
159 robot_.setVisualVisible(visual_visible_);
160 robot_.setCollisionVisible(collision_visible_);
161 robot_.setVisible(visible_);
172 robot_.setVisible(visible);
177 visual_visible_ = visible;
178 robot_.setVisualVisible(visible);
183 collision_visible_ = visible;
184 robot_.setCollisionVisible(visible);
189 robot_.setAlpha(alpha);
Object defining bodies that can be attached to robot links.
Update the links of an rviz::Robot using a moveit::core::RobotState.
void update(const moveit::core::RobotStateConstPtr &robot_state)
void setCollisionVisible(bool visible)
Set whether the collision meshes/primitives of the robot should be visible.
void updateKinematicState(const moveit::core::RobotStateConstPtr &robot_state)
void setAlpha(double alpha)
void setVisualVisible(bool visible)
Set whether the visual meshes of the robot should be visible.
void setVisible(bool visible)
Set the robot as a whole to be visible or not.
RobotStateVisualization(Ogre::SceneNode *root_node, rviz_common::DisplayContext *context, const std::string &name, rviz_common::properties::Property *parent_property)
void load(const urdf::ModelInterface &descr, bool visual=true, bool collision=true)
void setDefaultAttachedObjectColor(const std_msgs::msg::ColorRGBA &default_attached_object_color)
void updateAttachedObjectColors(const std_msgs::msg::ColorRGBA &attached_object_color)
update color of all attached object shapes
rclcpp::Logger getLogger()
@ OCTOMAP_OCCUPIED_VOXELS
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.