moveit2
The MoveIt Motion Planning Framework for ROS 2.
motion_planning_display.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, 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, Dave Coleman, Adam Leeper, Sachin Chitta */
36 
37 #pragma once
38 
39 #include <rviz_common/display.hpp>
40 #include <rviz_common/panel_dock_widget.hpp>
43 
44 #ifndef Q_MOC_RUN
48 
52 
53 #include <rclcpp/rclcpp.hpp>
54 
55 #include <std_msgs/msg/string.hpp>
56 #include <moveit_msgs/msg/display_trajectory.hpp>
57 #endif
58 
59 #include <memory>
60 
61 namespace Ogre
62 {
63 class SceneNode;
64 }
65 
66 namespace rviz_rendering
67 {
68 class Shape;
69 class MovableText;
70 } // namespace rviz_rendering
71 
72 namespace rviz_common
73 {
74 namespace properties
75 {
76 class Property;
77 class StringProperty;
78 class BoolProperty;
79 class FloatProperty;
80 class RosTopicProperty;
81 class EditableEnumProperty;
82 class ColorProperty;
83 class MovableText;
84 } // namespace properties
85 } // namespace rviz_common
86 
87 namespace rviz_default_plugins
88 {
89 namespace robot
90 {
91 class Robot;
92 }
93 } // namespace rviz_default_plugins
94 
96 {
98 {
99  Q_OBJECT
100 
101 public:
103 
104  ~MotionPlanningDisplay() override;
105 
106  void load(const rviz_common::Config& config) override;
107  void save(rviz_common::Config config) const override;
108 
109  void update(float wall_dt, float ros_dt) override;
110  void reset() override;
111 
112  moveit::core::RobotStateConstPtr getQueryStartState() const
113  {
114  return query_start_state_->getState();
115  }
116 
117  moveit::core::RobotStateConstPtr getQueryGoalState() const
118  {
119  return query_goal_state_->getState();
120  }
121 
123  {
124  return *previous_state_;
125  }
126 
127  const robot_interaction::RobotInteractionPtr& getRobotInteraction() const
128  {
129  return robot_interaction_;
130  }
131 
132  const robot_interaction::InteractionHandlerPtr& getQueryStartStateHandler() const
133  {
134  return query_start_state_;
135  }
136 
137  const robot_interaction::InteractionHandlerPtr& getQueryGoalStateHandler() const
138  {
139  return query_goal_state_;
140  }
141 
143  {
144  trajectory_visual_->dropTrajectory();
145  }
146 
147  void setQueryStartState(const moveit::core::RobotState& start);
149 
150  void updateQueryStates(const moveit::core::RobotState& current_state);
151  void updateQueryStartState();
152  void updateQueryGoalState();
154 
155  void useApproximateIK(bool flag);
156 
157  // Pick Place
159  void visualizePlaceLocations(const std::vector<geometry_msgs::msg::PoseStamped>& place_poses);
160  std::vector<std::shared_ptr<rviz_rendering::Shape> > place_locations_display_;
161 
162  std::string getCurrentPlanningGroup() const;
163 
164  void changePlanningGroup(const std::string& group);
165 
166  void addStatusText(const std::string& text);
167  void addStatusText(const std::vector<std::string>& text);
168  void setStatusTextColor(const QColor& color);
169  void resetStatusTextColor();
170 
171  void toggleSelectPlanningGroupSubscription(bool enable);
172 
173 Q_SIGNALS:
174  // signals issued when start/goal states of a query changed
177 
178 private Q_SLOTS:
179 
180  // ******************************************************************************************
181  // Slot Event Functions
182  // ******************************************************************************************
183 
184  void changedQueryStartState();
185  void changedQueryGoalState();
186  void changedQueryMarkerScale();
187  void changedQueryStartColor();
188  void changedQueryGoalColor();
189  void changedQueryStartAlpha();
190  void changedQueryGoalAlpha();
191  void changedQueryCollidingLinkColor();
192  void changedQueryJointViolationColor();
193  void changedAttachedBodyColor() override;
194  void changedPlanningGroup();
195  void changedShowWeightLimit();
196  void changedShowManipulabilityIndex();
197  void changedShowManipulability();
198  void changedShowJointTorques();
199  void changedMetricsSetPayload();
200  void changedMetricsTextHeight();
201  void changedWorkspace();
202  void resetInteractiveMarkers();
203  void motionPanelVisibilityChange(bool enable);
204 
205 protected:
207  {
210  };
211 
212  void clearRobotModel() override;
213  void onRobotModelLoaded() override;
214  void onNewPlanningSceneState() override;
216  void updateInternal(float wall_dt, float ros_dt) override;
217 
218  void renderWorkspaceBox();
219  void updateLinkColors();
220 
221  void displayTable(const std::map<std::string, double>& values, const Ogre::ColourValue& color,
222  const Ogre::Vector3& pos, const Ogre::Quaternion& orient);
223  void displayMetrics(bool start);
224 
226  void publishInteractiveMarkers(bool pose_update);
227 
230  void drawQueryStartState();
231  void drawQueryGoalState();
232  void scheduleDrawQueryStartState(robot_interaction::InteractionHandler* handler, bool error_state_changed);
233  void scheduleDrawQueryGoalState(robot_interaction::InteractionHandler* handler, bool error_state_changed);
234 
236  const double* ik_solution) const;
237 
238  void computeMetrics(bool start, const std::string& group, double payload);
239  void computeMetricsInternal(std::map<std::string, double>& metrics,
241  const moveit::core::RobotState& state, double payload);
244  void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string& jobname);
245 
246  void setQueryStateHelper(bool use_start_state, const std::string& v);
247  void populateMenuHandler(std::shared_ptr<interactive_markers::MenuHandler>& mh);
248 
249  void selectPlanningGroupCallback(const std_msgs::msg::String::ConstSharedPtr& msg);
250 
251  // overrides from Display
252  void onInitialize() override;
253  void onEnable() override;
254  void onDisable() override;
255  void fixedFrameChanged() override;
256 
257  RobotStateVisualizationPtr query_robot_start_;
258  RobotStateVisualizationPtr query_robot_goal_;
259 
260  Ogre::SceneNode* text_display_scene_node_;
262  rviz_rendering::MovableText* text_to_display_;
263 
264  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr planning_group_sub_;
265 
266  // render the workspace box
267  std::unique_ptr<rviz_rendering::Shape> workspace_box_;
268 
269  // the planning frame
271  rviz_common::PanelDockWidget* frame_dock_;
272 
273  // robot interaction
274  robot_interaction::RobotInteractionPtr robot_interaction_;
275  robot_interaction::InteractionHandlerPtr query_start_state_;
276  robot_interaction::InteractionHandlerPtr query_goal_state_;
277  std::shared_ptr<interactive_markers::MenuHandler> menu_handler_start_;
278  std::shared_ptr<interactive_markers::MenuHandler> menu_handler_goal_;
279  std::map<std::string, LinkDisplayStatus> status_links_start_;
280  std::map<std::string, LinkDisplayStatus> status_links_goal_;
282  moveit::core::RobotStatePtr previous_state_;
283 
286  std::set<std::string> modified_groups_;
287 
290  std::map<std::pair<bool, std::string>, std::map<std::string, double> > computed_metrics_;
292  std::map<std::string, bool> position_only_ik_;
293 
294  // Metric calculations
295  kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_;
296  std::map<std::string, dynamics_solver::DynamicsSolverPtr> dynamics_solver_;
298 
299  // The trajectory playback component
300  TrajectoryVisualizationPtr trajectory_visual_;
301 
302  // properties to show on side panel
303  rviz_common::properties::Property* path_category_;
304  rviz_common::properties::Property* plan_category_;
305  rviz_common::properties::Property* metrics_category_;
306 
307  rviz_common::properties::EditableEnumProperty* planning_group_property_;
308  rviz_common::properties::BoolProperty* query_start_state_property_;
309  rviz_common::properties::BoolProperty* query_goal_state_property_;
310  rviz_common::properties::FloatProperty* query_marker_scale_property_;
311  rviz_common::properties::ColorProperty* query_start_color_property_;
312  rviz_common::properties::ColorProperty* query_goal_color_property_;
313  rviz_common::properties::FloatProperty* query_start_alpha_property_;
314  rviz_common::properties::FloatProperty* query_goal_alpha_property_;
315  rviz_common::properties::ColorProperty* query_colliding_link_color_property_;
316  rviz_common::properties::ColorProperty* query_outside_joint_limits_link_color_property_;
317 
318  rviz_common::properties::BoolProperty* compute_weight_limit_property_;
319  rviz_common::properties::BoolProperty* show_manipulability_index_property_;
320  rviz_common::properties::BoolProperty* show_manipulability_property_;
321  rviz_common::properties::BoolProperty* show_joint_torques_property_;
322  rviz_common::properties::FloatProperty* metrics_set_payload_property_;
323  rviz_common::properties::FloatProperty* metrics_text_height_property_;
324  rviz_common::properties::BoolProperty* show_workspace_property_;
325 
326  rviz_common::Display* int_marker_display_;
327 };
328 
329 } // namespace moveit_rviz_plugin
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void populateMenuHandler(std::shared_ptr< interactive_markers::MenuHandler > &mh)
RobotStateVisualizationPtr query_robot_start_
Handles drawing the robot at the start configuration.
void scheduleDrawQueryGoalState(robot_interaction::InteractionHandler *handler, bool error_state_changed)
kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_
const robot_interaction::InteractionHandlerPtr & getQueryStartStateHandler() const
rviz_common::properties::FloatProperty * metrics_text_height_property_
void updateInternal(float wall_dt, float ros_dt) override
void onNewPlanningSceneState() override
This is called upon successful retrieval of the (initial) planning scene state.
void save(rviz_common::Config config) const override
moveit::core::RobotStatePtr previous_state_
remember previous start state (updated before starting execution)
void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string &jobname)
rviz_common::properties::BoolProperty * show_joint_torques_property_
void displayTable(const std::map< std::string, double > &values, const Ogre::ColourValue &color, const Ogre::Vector3 &pos, const Ogre::Quaternion &orient)
rviz_common::properties::FloatProperty * query_marker_scale_property_
void updateStateExceptModified(moveit::core::RobotState &dest, const moveit::core::RobotState &src)
bool text_display_for_start_
indicates whether the text display is for the start state or not
robot_interaction::RobotInteractionPtr robot_interaction_
rviz_common::properties::BoolProperty * show_manipulability_property_
void setQueryGoalState(const moveit::core::RobotState &goal)
robot_interaction::InteractionHandlerPtr query_start_state_
std::map< std::string, LinkDisplayStatus > status_links_goal_
void setQueryStartState(const moveit::core::RobotState &start)
rviz_common::properties::ColorProperty * query_outside_joint_limits_link_color_property_
rviz_common::properties::FloatProperty * query_goal_alpha_property_
rviz_common::properties::ColorProperty * query_start_color_property_
void computeMetricsInternal(std::map< std::string, double > &metrics, const robot_interaction::EndEffectorInteraction &eef, const moveit::core::RobotState &state, double payload)
void computeMetrics(bool start, const std::string &group, double payload)
rviz_common::properties::Property * path_category_
rviz_common::properties::FloatProperty * metrics_set_payload_property_
void setQueryStateHelper(bool use_start_state, const std::string &v)
std::map< std::string, bool > position_only_ik_
Some groups use position only ik, calls to the metrics have to be modified appropriately.
rviz_common::properties::ColorProperty * query_goal_color_property_
rviz_common::properties::BoolProperty * compute_weight_limit_property_
std::unique_ptr< rviz_rendering::Shape > workspace_box_
std::map< std::pair< bool, std::string >, std::map< std::string, double > > computed_metrics_
std::map< std::string, LinkDisplayStatus > status_links_start_
Ogre::SceneNode * text_display_scene_node_
displays texts
void updateQueryStates(const moveit::core::RobotState &current_state)
rviz_common::properties::EditableEnumProperty * planning_group_property_
rviz_common::properties::BoolProperty * show_manipulability_index_property_
rviz_common::properties::Property * metrics_category_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_start_
void scheduleDrawQueryStartState(robot_interaction::InteractionHandler *handler, bool error_state_changed)
std::map< std::string, dynamics_solver::DynamicsSolverPtr > dynamics_solver_
moveit::core::RobotStateConstPtr getQueryStartState() const
rviz_common::properties::BoolProperty * show_workspace_property_
rviz_common::properties::BoolProperty * query_goal_state_property_
void load(const rviz_common::Config &config) override
bool isIKSolutionCollisionFree(moveit::core::RobotState *state, const moveit::core::JointModelGroup *group, const double *ik_solution) const
rviz_common::properties::FloatProperty * query_start_alpha_property_
const robot_interaction::InteractionHandlerPtr & getQueryGoalStateHandler() const
std::vector< std::shared_ptr< rviz_rendering::Shape > > place_locations_display_
RobotStateVisualizationPtr query_robot_goal_
Handles drawing the robot at the goal configuration.
void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override
void update(float wall_dt, float ros_dt) override
rviz_common::properties::BoolProperty * query_start_state_property_
rviz_common::properties::Property * plan_category_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_goal_
rclcpp::Subscription< std_msgs::msg::String >::SharedPtr planning_group_sub_
const robot_interaction::RobotInteractionPtr & getRobotInteraction() const
const moveit::core::RobotState & getPreviousState() const
void selectPlanningGroupCallback(const std_msgs::msg::String::ConstSharedPtr &msg)
void onRobotModelLoaded() override
This is an event called by loadRobotModel() in the MainLoop; do not call directly.
rviz_common::properties::ColorProperty * query_colliding_link_color_property_
void visualizePlaceLocations(const std::vector< geometry_msgs::msg::PoseStamped > &place_poses)
moveit::core::RobotStateConstPtr getQueryGoalState() const
robot_interaction::InteractionHandlerPtr query_goal_state_
robot
Definition: pick.py:53