41#include <rclcpp/qos.hpp>
44#include <rviz_default_plugins/robot/robot.hpp>
45#include <rviz_default_plugins/robot/robot_link.hpp>
47#include <rviz_common/properties/property.hpp>
48#include <rviz_common/properties/string_property.hpp>
49#include <rviz_common/properties/bool_property.hpp>
50#include <rviz_common/properties/float_property.hpp>
51#include <rviz_common/properties/ros_topic_property.hpp>
52#include <rviz_common/properties/color_property.hpp>
53#include <rviz_common/display_context.hpp>
54#include <rviz_common/frame_manager_iface.hpp>
55#include <rviz_common/logging.hpp>
57#include <OgreSceneManager.h>
58#include <OgreSceneNode.h>
70 "Robot Description",
"robot_description",
"The name of the ROS parameter where the URDF for the robot is loaded",
71 this, SLOT(changedRobotDescription()),
this);
74 "Robot State Topic",
"display_robot_state",
75 rosidl_generator_traits::data_type<moveit_msgs::msg::DisplayRobotState>(),
76 "The topic on which the moveit_msgs::msg::DisplayRobotState messages are received",
this,
77 SLOT(changedRobotStateTopic()),
this);
81 new rviz_common::properties::StringProperty(
"Robot Root Link",
"",
82 "Shows the name of the root link for the robot model",
this,
83 SLOT(changedRootLinkName()),
this);
87 "Robot Alpha", 1.0f,
"Specifies the alpha for the robot links",
this, SLOT(changedRobotSceneAlpha()),
this);
92 new rviz_common::properties::ColorProperty(
"Attached Body Color", QColor(150, 50, 150),
93 "The color for the attached bodies",
this,
94 SLOT(changedAttachedBodyColor()),
this);
97 "Specifies whether link highlighting is enabled",
98 this, SLOT(changedEnableLinkHighlight()),
this);
100 new rviz_common::properties::BoolProperty(
"Visual Enabled",
true,
101 "Whether to display the visual representation of the robot.",
this,
102 SLOT(changedEnableVisualVisible()),
this);
104 new rviz_common::properties::BoolProperty(
"Collision Enabled",
false,
105 "Whether to display the collision representation of the robot.",
this,
106 SLOT(changedEnableCollisionVisible()),
this);
109 "Show All Links",
true,
"Toggle all links visibility on or off.",
this, SLOT(changedAllLinks()),
this);
119 Display::onInitialize();
120 auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
121 if (!ros_node_abstraction)
123 RVIZ_COMMON_LOG_WARNING(
"Unable to lock weak_ptr from DisplayContext in RobotStateDisplay constructor");
127 node_ = ros_node_abstraction->get_raw_node();
128 robot_ = std::make_shared<RobotStateVisualization>(scene_node_, context_,
"Robot State",
this);
129 changedEnableVisualVisible();
130 changedEnableCollisionVisible();
131 robot_->setVisible(
false);
143void RobotStateDisplay::changedAllLinks()
145 Property* links_prop = subProp(
"Links");
148 for (
int i = 0; i < links_prop->numChildren(); ++i)
150 Property* link_prop = links_prop->childAt(i);
151 link_prop->setValue(value);
157 rviz_default_plugins::robot::RobotLink* link =
robot_->getRobot().getLink(link_name);
160 link->setColor(color.r, color.g, color.b);
167 rviz_default_plugins::robot::RobotLink* link =
robot_->getRobot().getLink(link_name);
175void RobotStateDisplay::changedEnableLinkHighlight()
179 for (std::pair<const std::string, std_msgs::msg::ColorRGBA>& highlight :
highlights_)
186 for (std::pair<const std::string, std_msgs::msg::ColorRGBA>& highlight :
highlights_)
193void RobotStateDisplay::changedEnableVisualVisible()
198void RobotStateDisplay::changedEnableCollisionVisible()
204 const moveit_msgs::msg::DisplayRobotState::_highlight_links_type& highlight_links)
206 if (highlight_links.empty() &&
highlights_.empty())
209 std::map<std::string, std_msgs::msg::ColorRGBA> highlights;
210 for (
const moveit_msgs::msg::ObjectColor& highlight_link : highlight_links)
212 highlights[highlight_link.id] = highlight_link.color;
217 std::map<std::string, std_msgs::msg::ColorRGBA>::iterator ho =
highlights_.begin();
218 std::map<std::string, std_msgs::msg::ColorRGBA>::iterator hn = highlights.begin();
219 while (ho !=
highlights_.end() || hn != highlights.end())
226 else if (hn == highlights.end())
231 else if (hn->first < ho->first)
236 else if (hn->first > ho->first)
241 else if (hn->second != ho->second)
258void RobotStateDisplay::changedAttachedBodyColor()
263 std_msgs::msg::ColorRGBA color_msg;
264 color_msg.r = color.redF();
265 color_msg.g = color.greenF();
266 color_msg.b = color.blueF();
268 robot_->setDefaultAttachedObjectColor(color_msg);
273void RobotStateDisplay::changedRobotDescription()
279void RobotStateDisplay::changedRootLinkName()
283void RobotStateDisplay::changedRobotSceneAlpha()
289 std_msgs::msg::ColorRGBA color_msg;
290 color_msg.r = color.redF();
291 color_msg.g = color.greenF();
292 color_msg.b = color.blueF();
294 robot_->setDefaultAttachedObjectColor(color_msg);
299void RobotStateDisplay::changedRobotStateTopic()
301 using namespace std::placeholders;
306 robot_->setVisible(
false);
307 setStatus(rviz_common::properties::StatusProperty::Warn,
"RobotState",
"No msg received");
311 [
this](
const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state) {
return newRobotStateCallback(state); });
331 setStatus(rviz_common::properties::StatusProperty::Error,
"RobotState", e.what());
332 robot_->setVisible(
false);
336 if (
robot_->isVisible() != !state_msg->hide)
338 robot_->setVisible(!state_msg->hide);
341 setStatus(rviz_common::properties::StatusProperty::Ok,
"RobotState",
"");
345 setStatus(rviz_common::properties::StatusProperty::Warn,
"RobotState",
"Hidden");
364 robot_->setVisible(visible);
370 rviz_default_plugins::robot::RobotLink* link = robot->getLink(link_name);
374 link->setColor(color.redF(), color.greenF(), color.blueF());
379 rviz_default_plugins::robot::RobotLink* link = robot->getLink(link_name);
393 setStatus(rviz_common::properties::StatusProperty::Error,
"RobotModel",
"`Robot Description` field can't be empty");
408 const srdf::ModelSharedPtr& srdf =
418 setStatus(rviz_common::properties::StatusProperty::Ok,
"RobotModel",
"Loaded successfully");
420 changedEnableVisualVisible();
421 changedEnableCollisionVisible();
423 catch (std::exception& e)
425 setStatus(rviz_common::properties::StatusProperty::Error,
"RobotModel",
426 QString(
"Loading failed: %1").arg(e.what()));
431 setStatus(rviz_common::properties::StatusProperty::Error,
"RobotModel",
"Loading failed");
442 Display::load(config);
450 changedRobotStateTopic();
461 robot_->setVisible(
false);
462 Display::onDisable();
466#if RCLCPP_VERSION_GTE(30, 0, 0)
469 Display::update(wall_dt, ros_dt);
482 Display::update(wall_dt, ros_dt);
501 Ogre::Vector3 position;
502 Ogre::Quaternion orientation;
504 context_->getFrameManager()->getTransform(
getRobotModel()->getModelFrame(), rclcpp::Time(0, 0, RCL_ROS_TIME),
505 position, orientation);
507 scene_node_->setPosition(position);
508 scene_node_->setOrientation(orientation);
513 Display::fixedFrameChanged();
This may be thrown if unrecoverable errors occur.
rviz_common::properties::RosTopicProperty * robot_state_topic_property_
RobotStateVisualizationPtr robot_
void load(const rviz_common::Config &config) override
moveit::core::RobotModelConstPtr robot_model_
void setHighlight(const std::string &link_name, const std_msgs::msg::ColorRGBA &color)
~RobotStateDisplay() override
void update(float wall_dt, float ros_dt) override
const moveit::core::RobotModelConstPtr & getRobotModel() const
void setRobotHighlights(const moveit_msgs::msg::DisplayRobotState::_highlight_links_type &highlight_links)
rviz_common::properties::FloatProperty * robot_alpha_property_
rviz_common::properties::ColorProperty * attached_body_color_property_
rviz_common::properties::StringProperty * root_link_name_property_
rclcpp::Subscription< moveit_msgs::msg::DisplayRobotState >::SharedPtr robot_state_subscriber_
void unsetHighlight(const std::string &link_name)
rclcpp::Node::SharedPtr node_
rviz_common::properties::BoolProperty * enable_visual_visible_
void fixedFrameChanged() override
void calculateOffsetPosition()
Set the scene node's position, given the target frame and the planning frame.
void setVisible(bool visible)
void onInitialize() override
void unsetLinkColor(const std::string &link_name)
void newRobotStateCallback(const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr &state)
rviz_common::properties::BoolProperty * enable_link_highlight_
rviz_common::properties::BoolProperty * enable_collision_visible_
rviz_common::properties::BoolProperty * show_all_links_
rviz_common::properties::StringProperty * robot_description_property_
rdf_loader::RDFLoaderPtr rdf_loader_
void setLinkColor(const std::string &link_name, const QColor &color)
std::map< std::string, std_msgs::msg::ColorRGBA > highlights_
moveit::core::RobotStatePtr robot_state_
void onDisable() override
bool isEmpty(const moveit_msgs::msg::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.