54 #include <OgreSceneManager.h>
55 #include <OgreSceneNode.h>
66 robot_description_property_ =
68 "The name of the ROS parameter where the URDF for the robot is loaded",
this,
69 SLOT(changedRobotDescription()),
this);
71 robot_state_topic_property_ =
73 ros::message_traits::datatype<moveit_msgs::DisplayRobotState>(),
74 "The topic on which the moveit_msgs::DisplayRobotState messages are received",
this,
75 SLOT(changedRobotStateTopic()),
this);
78 root_link_name_property_ =
79 new rviz::StringProperty(
"Robot Root Link",
"",
"Shows the name of the root link for the robot model",
this,
80 SLOT(changedRootLinkName()),
this);
81 root_link_name_property_->setReadOnly(
true);
83 robot_alpha_property_ =
new rviz::FloatProperty(
"Robot Alpha", 1.0f,
"Specifies the alpha for the robot links",
this,
84 SLOT(changedRobotSceneAlpha()),
this);
85 robot_alpha_property_->setMin(0.0);
86 robot_alpha_property_->setMax(1.0);
88 attached_body_color_property_ =
89 new rviz::ColorProperty(
"Attached Body Color", QColor(150, 50, 150),
"The color for the attached bodies",
this,
90 SLOT(changedAttachedBodyColor()),
this);
92 enable_link_highlight_ =
93 new rviz::BoolProperty(
"Show Highlights",
true,
"Specifies whether link highlighting is enabled",
this,
94 SLOT(changedEnableLinkHighlight()),
this);
95 enable_visual_visible_ =
96 new rviz::BoolProperty(
"Visual Enabled",
true,
"Whether to display the visual representation of the robot.",
this,
97 SLOT(changedEnableVisualVisible()),
this);
99 "Whether to display the collision representation of the robot.",
100 this, SLOT(changedEnableCollisionVisible()),
this);
102 show_all_links_ =
new rviz::BoolProperty(
"Show All Links",
true,
"Toggle all links visibility on or off.",
this,
103 SLOT(changedAllLinks()),
this);
113 Display::onInitialize();
117 robot_->setVisible(
false);
134 for (
int i = 0; i < links_prop->numChildren(); ++i)
136 Property* link_prop = links_prop->childAt(i);
137 link_prop->setValue(value);
146 link->
setColor(color.r, color.g, color.b);
165 for (std::pair<const std::string, std_msgs::ColorRGBA>& highlight :
highlights_)
172 for (std::pair<const std::string, std_msgs::ColorRGBA>& highlight :
highlights_)
189 static bool operator!=(
const std_msgs::ColorRGBA& a,
const std_msgs::ColorRGBA& b)
191 return a.r != b.r ||
a.g != b.g ||
a.b != b.b ||
a.a != b.a;
196 if (highlight_links.empty() &&
highlights_.empty())
199 std::map<std::string, std_msgs::ColorRGBA> highlights;
200 for (
const moveit_msgs::ObjectColor& highlight_link : highlight_links)
202 highlights[highlight_link.id] = highlight_link.color;
207 std::map<std::string, std_msgs::ColorRGBA>::iterator ho =
highlights_.begin();
208 std::map<std::string, std_msgs::ColorRGBA>::iterator hn = highlights.begin();
209 while (ho !=
highlights_.end() || hn != highlights.end())
216 else if (hn == highlights.end())
221 else if (hn->first < ho->first)
226 else if (hn->first > ho->first)
231 else if (hn->second != ho->second)
253 std_msgs::ColorRGBA color_msg;
254 color_msg.r = color.redF();
255 color_msg.g = color.greenF();
256 color_msg.b = color.blueF();
258 robot_->setDefaultAttachedObjectColor(color_msg);
279 std_msgs::ColorRGBA color_msg;
280 color_msg.r = color.redF();
281 color_msg.g = color.greenF();
282 color_msg.b = color.blueF();
284 robot_->setDefaultAttachedObjectColor(color_msg);
297 robot_->setVisible(
false);
322 robot_->setVisible(
false);
326 if (
robot_->isVisible() != !state_msg->hide)
328 robot_->setVisible(!state_msg->hide);
350 robot_->setVisible(visible);
359 link->
setColor(color.redF(), color.greenF(), color.blueF());
397 catch (std::exception& e)
432 robot_->setVisible(
false);
433 Display::onDisable();
438 Display::update(wall_dt, ros_dt);
456 Ogre::Vector3 position;
457 Ogre::Quaternion orientation;
467 Display::fixedFrameChanged();