robot_state_display.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #pragma once
38 
39 #include <rviz/display.h>
40 
41 #ifndef Q_MOC_RUN
44 #include <moveit_msgs/DisplayRobotState.h>
45 #include <ros/ros.h>
46 #endif
47 
48 namespace rviz
49 {
50 class Robot;
51 class StringProperty;
52 class BoolProperty;
53 class FloatProperty;
54 class RosTopicProperty;
55 class ColorProperty;
56 } // namespace rviz
57 
58 namespace moveit_rviz_plugin
59 {
60 class RobotStateVisualization;
61 
63 {
64  Q_OBJECT
65 
66 public:
68  ~RobotStateDisplay() override;
69 
70  void load(const rviz::Config& config) override;
71  void update(float wall_dt, float ros_dt) override;
72  void reset() override;
73 
74  const moveit::core::RobotModelConstPtr& getRobotModel() const
75  {
76  return robot_model_;
77  }
78 
79  void setLinkColor(const std::string& link_name, const QColor& color);
80  void unsetLinkColor(const std::string& link_name);
81 
82 public Q_SLOTS:
83  void setVisible(bool visible);
84 
85 private Q_SLOTS:
86 
87  // ******************************************************************************************
88  // Slot Event Functions
89  // ******************************************************************************************
91  void changedRootLinkName();
98  void changedAllLinks();
99 
100 protected:
101  void loadRobotModel();
102 
107 
108  void setLinkColor(rviz::Robot* robot, const std::string& link_name, const QColor& color);
109  void unsetLinkColor(rviz::Robot* robot, const std::string& link_name);
110 
111  void newRobotStateCallback(const moveit_msgs::DisplayRobotState::ConstPtr& state);
112 
113  void setRobotHighlights(const moveit_msgs::DisplayRobotState::_highlight_links_type& highlight_links);
114  void setHighlight(const std::string& link_name, const std_msgs::ColorRGBA& color);
115  void unsetHighlight(const std::string& link_name);
116 
117  // overrides from Display
118  void onInitialize() override;
119  void onEnable() override;
120  void onDisable() override;
121  void fixedFrameChanged() override;
122 
123  // render the robot
126 
127  RobotStateVisualizationPtr robot_;
128  rdf_loader::RDFLoaderPtr rdf_loader_;
129  moveit::core::RobotModelConstPtr robot_model_;
130  moveit::core::RobotStatePtr robot_state_;
131  std::map<std::string, std_msgs::ColorRGBA> highlights_;
133 
143 };
144 
145 } // namespace moveit_rviz_plugin
moveit_rviz_plugin::RobotStateDisplay::show_all_links_
rviz::BoolProperty * show_all_links_
Definition: robot_state_display.h:142
rviz::RosTopicProperty
moveit_rviz_plugin::RobotStateDisplay::reset
void reset() override
Definition: robot_state_display.cpp:152
moveit_rviz_plugin::RobotStateDisplay::changedRobotSceneAlpha
void changedRobotSceneAlpha()
Definition: robot_state_display.cpp:305
moveit_rviz_plugin::RobotStateDisplay::calculateOffsetPosition
void calculateOffsetPosition()
Set the scene node's position, given the target frame and the planning frame.
Definition: robot_state_display.cpp:483
moveit_rviz_plugin::RobotStateDisplay::rdf_loader_
rdf_loader::RDFLoaderPtr rdf_loader_
Definition: robot_state_display.h:128
moveit_rviz_plugin::RobotStateDisplay::highlights_
std::map< std::string, std_msgs::ColorRGBA > highlights_
Definition: robot_state_display.h:131
ros.h
moveit_rviz_plugin::RobotStateDisplay::root_link_name_property_
rviz::StringProperty * root_link_name_property_
Definition: robot_state_display.h:135
moveit_rviz_plugin::RobotStateDisplay::root_nh_
ros::NodeHandle root_nh_
Definition: robot_state_display.h:124
moveit_rviz_plugin::RobotStateDisplay::changedEnableLinkHighlight
void changedEnableLinkHighlight()
Definition: robot_state_display.cpp:193
rdf_loader.h
rviz::BoolProperty
moveit_rviz_plugin::RobotStateDisplay::update
void update(float wall_dt, float ros_dt) override
Definition: robot_state_display.cpp:468
moveit_rviz_plugin::RobotStateDisplay::changedRobotDescription
void changedRobotDescription()
Definition: robot_state_display.cpp:295
moveit_rviz_plugin::RobotStateDisplay::enable_link_highlight_
rviz::BoolProperty * enable_link_highlight_
Definition: robot_state_display.h:139
moveit_rviz_plugin::RobotStateDisplay::robot_alpha_property_
rviz::FloatProperty * robot_alpha_property_
Definition: robot_state_display.h:137
display.h
moveit_rviz_plugin::RobotStateDisplay::changedEnableVisualVisible
void changedEnableVisualVisible()
Definition: robot_state_display.cpp:211
moveit_rviz_plugin::RobotStateDisplay::setLinkColor
void setLinkColor(const std::string &link_name, const QColor &color)
Definition: robot_state_display.cpp:370
moveit_rviz_plugin::RobotStateDisplay::changedEnableCollisionVisible
void changedEnableCollisionVisible()
Definition: robot_state_display.cpp:216
rviz::ColorProperty
moveit_rviz_plugin::RobotStateDisplay::enable_visual_visible_
rviz::BoolProperty * enable_visual_visible_
Definition: robot_state_display.h:140
rviz::Display
rviz::FloatProperty
moveit_rviz_plugin::RobotStateDisplay::changedAttachedBodyColor
void changedAttachedBodyColor()
Definition: robot_state_display.cpp:280
rviz
moveit_rviz_plugin::RobotStateDisplay::robot_state_topic_property_
rviz::RosTopicProperty * robot_state_topic_property_
Definition: robot_state_display.h:136
rviz::StringProperty
moveit_rviz_plugin::RobotStateDisplay::robot_
RobotStateVisualizationPtr robot_
Definition: robot_state_display.h:127
moveit_rviz_plugin::RobotStateDisplay::robot_description_property_
rviz::StringProperty * robot_description_property_
Definition: robot_state_display.h:134
moveit_rviz_plugin::RobotStateDisplay::onInitialize
void onInitialize() override
Definition: robot_state_display.cpp:143
moveit_rviz_plugin::RobotStateDisplay::onDisable
void onDisable() override
Definition: robot_state_display.cpp:460
moveit_rviz_plugin::RobotStateDisplay::~RobotStateDisplay
~RobotStateDisplay() override
Robot
rviz::Robot
moveit_rviz_plugin::RobotStateDisplay::unsetHighlight
void unsetHighlight(const std::string &link_name)
Definition: robot_state_display.cpp:183
moveit_rviz_plugin::RobotStateDisplay::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: robot_state_display.h:74
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit_rviz_plugin::RobotStateDisplay::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: robot_state_display.h:129
moveit_rviz_plugin::RobotStateDisplay::update_state_
bool update_state_
Definition: robot_state_display.h:132
moveit_rviz_plugin::RobotStateDisplay::attached_body_color_property_
rviz::ColorProperty * attached_body_color_property_
Definition: robot_state_display.h:138
moveit_rviz_plugin::RobotStateDisplay::load
void load(const rviz::Config &config) override
Definition: robot_state_display.cpp:440
moveit_rviz_plugin::RobotStateDisplay::setVisible
void setVisible(bool visible)
Definition: robot_state_display.cpp:380
moveit_rviz_plugin::RobotStateDisplay::unsetLinkColor
void unsetLinkColor(const std::string &link_name)
Definition: robot_state_display.cpp:375
moveit_rviz_plugin::RobotStateDisplay::changedRootLinkName
void changedRootLinkName()
Definition: robot_state_display.cpp:301
moveit_rviz_plugin::RobotStateDisplay::fixedFrameChanged
void fixedFrameChanged() override
Definition: robot_state_display.cpp:497
moveit_rviz_plugin::RobotStateDisplay::changedRobotStateTopic
void changedRobotStateTopic()
Definition: robot_state_display.cpp:321
moveit_rviz_plugin::RobotStateDisplay::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: robot_state_display.h:130
moveit_rviz_plugin::RobotStateDisplay::setHighlight
void setHighlight(const std::string &link_name, const std_msgs::ColorRGBA &color)
Definition: robot_state_display.cpp:173
robot_state_visualization.h
moveit_rviz_plugin::RobotStateDisplay::RobotStateDisplay
RobotStateDisplay()
Definition: robot_state_display.cpp:96
moveit_rviz_plugin::RobotStateDisplay::newRobotStateCallback
void newRobotStateCallback(const moveit_msgs::DisplayRobotState::ConstPtr &state)
Definition: robot_state_display.cpp:336
moveit_rviz_plugin::RobotStateDisplay::enable_collision_visible_
rviz::BoolProperty * enable_collision_visible_
Definition: robot_state_display.h:141
moveit_rviz_plugin::RobotStateDisplay
Definition: robot_state_display.h:62
moveit_rviz_plugin::RobotStateDisplay::onEnable
void onEnable() override
Definition: robot_state_display.cpp:448
moveit_rviz_plugin::RobotStateDisplay::loadRobotModel
void loadRobotModel()
Definition: robot_state_display.cpp:406
moveit_rviz_plugin::RobotStateDisplay::setRobotHighlights
void setRobotHighlights(const moveit_msgs::DisplayRobotState::_highlight_links_type &highlight_links)
Definition: robot_state_display.cpp:226
config
config
rviz::Config
moveit_rviz_plugin::RobotStateDisplay::changedAllLinks
void changedAllLinks()
Definition: robot_state_display.cpp:161
ros::NodeHandle
ros::Subscriber
moveit_rviz_plugin::RobotStateDisplay::robot_state_subscriber_
ros::Subscriber robot_state_subscriber_
Definition: robot_state_display.h:125


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:26