moveit2
The MoveIt Motion Planning Framework for ROS 2.
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_common/display.hpp>
40 
41 #ifndef Q_MOC_RUN
44 #include <moveit_msgs/msg/display_robot_state.hpp>
45 #include <rclcpp/rclcpp.hpp>
46 #endif
47 
48 namespace rviz_common
49 {
50 namespace properties
51 {
52 class Robot;
53 class StringProperty;
54 class BoolProperty;
55 class FloatProperty;
56 class RosTopicProperty;
57 class ColorProperty;
58 } // namespace properties
59 } // namespace rviz_common
60 
61 namespace moveit_rviz_plugin
62 {
63 class RobotStateVisualization;
64 
65 class RobotStateDisplay : public rviz_common::Display
66 {
67  Q_OBJECT
68 
69 public:
71  ~RobotStateDisplay() override;
72 
73  void load(const rviz_common::Config& config) override;
74  void update(float wall_dt, float ros_dt) override;
75  void reset() override;
76 
77  const moveit::core::RobotModelConstPtr& getRobotModel() const
78  {
79  return robot_model_;
80  }
81 
82  void setLinkColor(const std::string& link_name, const QColor& color);
83  void unsetLinkColor(const std::string& link_name);
84 
85 public Q_SLOTS:
86  void setVisible(bool visible);
87 
88 private Q_SLOTS:
89 
90  // ******************************************************************************************
91  // Slot Event Functions
92  // ******************************************************************************************
93  void changedRobotDescription();
94  void changedRootLinkName();
95  void changedRobotSceneAlpha();
96  void changedAttachedBodyColor();
97  void changedRobotStateTopic();
98  void changedEnableLinkHighlight();
99  void changedEnableVisualVisible();
100  void changedEnableCollisionVisible();
101  void changedAllLinks();
102 
103 protected:
104  void initializeLoader();
105  void loadRobotModel();
106 
111 
112  void setLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name, const QColor& color);
113  void unsetLinkColor(rviz_default_plugins::robot::Robot* robot, const std::string& link_name);
114 
115  void newRobotStateCallback(const moveit_msgs::msg::DisplayRobotState::ConstSharedPtr& state);
116 
117  void setRobotHighlights(const moveit_msgs::msg::DisplayRobotState::_highlight_links_type& highlight_links);
118  void setHighlight(const std::string& link_name, const std_msgs::msg::ColorRGBA& color);
119  void unsetHighlight(const std::string& link_name);
120 
121  // overrides from Display
122  void onInitialize() override;
123  void onEnable() override;
124  void onDisable() override;
125  void fixedFrameChanged() override;
126 
127  // render the robot
128  rclcpp::Node::SharedPtr node_;
129  rclcpp::Subscription<moveit_msgs::msg::DisplayRobotState>::SharedPtr robot_state_subscriber_;
130 
131  RobotStateVisualizationPtr robot_;
132  rdf_loader::RDFLoaderPtr rdf_loader_;
133  moveit::core::RobotModelConstPtr robot_model_;
134  moveit::core::RobotStatePtr robot_state_;
135  std::map<std::string, std_msgs::msg::ColorRGBA> highlights_;
137 
138  rviz_common::properties::StringProperty* robot_description_property_;
139  rviz_common::properties::StringProperty* root_link_name_property_;
140  rviz_common::properties::RosTopicProperty* robot_state_topic_property_;
141  rviz_common::properties::FloatProperty* robot_alpha_property_;
142  rviz_common::properties::ColorProperty* attached_body_color_property_;
143  rviz_common::properties::BoolProperty* enable_link_highlight_;
144  rviz_common::properties::BoolProperty* enable_visual_visible_;
145  rviz_common::properties::BoolProperty* enable_collision_visible_;
146  rviz_common::properties::BoolProperty* show_all_links_;
147 };
148 
149 } // namespace moveit_rviz_plugin
rviz_common::properties::RosTopicProperty * robot_state_topic_property_
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)
void update(float wall_dt, float ros_dt) override
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)
rviz_common::properties::BoolProperty * enable_visual_visible_
void calculateOffsetPosition()
Set the scene node's position, given the target frame and the planning frame.
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_
void setLinkColor(const std::string &link_name, const QColor &color)
std::map< std::string, std_msgs::msg::ColorRGBA > highlights_
moveit::core::RobotStatePtr robot_state_
const moveit::core::RobotModelConstPtr & getRobotModel() const
robot
Definition: pick.py:53