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 #ifndef MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_
38 #define MOVEIT_MOTION_PLANNING_RVIZ_PLUGIN_MOTION_PLANNING_DISPLAY_
39 
40 #include <rviz/display.h>
42 #include <rviz/panel_dock_widget.h>
45 
46 #ifndef Q_MOC_RUN
50 
54 
55 #include <ros/ros.h>
56 
57 #include <std_msgs/String.h>
58 #include <moveit_msgs/DisplayTrajectory.h>
59 #endif
60 
61 #include <memory>
62 
63 namespace Ogre
64 {
65 class SceneNode;
66 }
67 
68 namespace rviz
69 {
70 class Robot;
71 class Shape;
72 class Property;
73 class StringProperty;
74 class BoolProperty;
75 class FloatProperty;
76 class RosTopicProperty;
77 class EditableEnumProperty;
78 class ColorProperty;
79 class MovableText;
80 }
81 
83 {
85 {
86  Q_OBJECT
87 
88 public:
90 
91  virtual ~MotionPlanningDisplay();
92 
93  virtual void load(const rviz::Config& config);
94  virtual void save(rviz::Config config) const;
95 
96  virtual void update(float wall_dt, float ros_dt);
97  virtual void reset();
98 
99  void setName(const QString& name);
100 
101  robot_state::RobotStateConstPtr getQueryStartState() const
102  {
103  return query_start_state_->getState();
104  }
105 
106  robot_state::RobotStateConstPtr getQueryGoalState() const
107  {
108  return query_goal_state_->getState();
109  }
110 
111  const robot_interaction::RobotInteractionPtr& getRobotInteraction() const
112  {
113  return robot_interaction_;
114  }
115 
117  {
118  return query_start_state_;
119  }
120 
122  {
123  return query_goal_state_;
124  }
125 
127  {
128  trajectory_visual_->dropTrajectory();
129  }
130 
131  void setQueryStartState(const robot_state::RobotState& start);
132  void setQueryGoalState(const robot_state::RobotState& goal);
133 
134  void updateQueryStartState();
135  void updateQueryGoalState();
136 
137  void useApproximateIK(bool flag);
138 
139  // Pick Place
140  void clearPlaceLocationsDisplay();
141  void visualizePlaceLocations(const std::vector<geometry_msgs::PoseStamped>& place_poses);
142  std::vector<std::shared_ptr<rviz::Shape> > place_locations_display_;
143 
144  std::string getCurrentPlanningGroup() const;
145 
146  void changePlanningGroup(const std::string& group);
147 
148  void addStatusText(const std::string& text);
149  void addStatusText(const std::vector<std::string>& text);
150  void setStatusTextColor(const QColor& color);
151  void resetStatusTextColor();
152 
153  void toggleSelectPlanningGroupSubscription(bool enable);
154 
155 private Q_SLOTS:
156 
157  // ******************************************************************************************
158  // Slot Event Functions
159  // ******************************************************************************************
160 
161  void changedQueryStartState();
162  void changedQueryGoalState();
163  void changedQueryMarkerScale();
164  void changedQueryStartColor();
165  void changedQueryGoalColor();
166  void changedQueryStartAlpha();
167  void changedQueryGoalAlpha();
168  void changedQueryCollidingLinkColor();
169  void changedQueryJointViolationColor();
170  void changedAttachedBodyColor() override;
171  void changedPlanningGroup();
172  void changedShowWeightLimit();
173  void changedShowManipulabilityIndex();
174  void changedShowManipulability();
175  void changedShowJointTorques();
176  void changedMetricsSetPayload();
177  void changedMetricsTextHeight();
178  void changedWorkspace();
179  void resetInteractiveMarkers();
180  void motionPanelVisibilityChange(bool enable);
181 
182 protected:
184  {
186  OUTSIDE_BOUNDS_LINK
187  };
188 
189  virtual void onRobotModelLoaded();
190  virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
191  virtual void updateInternal(float wall_dt, float ros_dt);
192 
193  void renderWorkspaceBox();
194  void updateLinkColors();
195 
196  void displayTable(const std::map<std::string, double>& values, const Ogre::ColourValue& color,
197  const Ogre::Vector3& pos, const Ogre::Quaternion& orient);
198  void displayMetrics(bool start);
199 
200  void executeMainLoopJobs();
201  void publishInteractiveMarkers(bool pose_update);
202 
203  void recomputeQueryStartStateMetrics();
204  void recomputeQueryGoalStateMetrics();
205  void drawQueryStartState();
206  void drawQueryGoalState();
207  void scheduleDrawQueryStartState(robot_interaction::RobotInteraction::InteractionHandler* handler,
208  bool error_state_changed);
209  void scheduleDrawQueryGoalState(robot_interaction::RobotInteraction::InteractionHandler* handler,
210  bool error_state_changed);
211 
212  bool isIKSolutionCollisionFree(robot_state::RobotState* state, const robot_state::JointModelGroup* group,
213  const double* ik_solution) const;
214 
215  void computeMetrics(bool start, const std::string& group, double payload);
216  void computeMetricsInternal(std::map<std::string, double>& metrics,
218  const robot_state::RobotState& state, double payload);
219  void updateStateExceptModified(robot_state::RobotState& dest, const robot_state::RobotState& src);
220  void updateBackgroundJobProgressBar();
221  void backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent event, const std::string& jobname);
222 
223  void setQueryStateHelper(bool use_start_state, const std::string& v);
224  void populateMenuHandler(std::shared_ptr<interactive_markers::MenuHandler>& mh);
225 
226  void selectPlanningGroupCallback(const std_msgs::StringConstPtr& msg);
227 
228  // overrides from Display
229  virtual void onInitialize();
230  virtual void onEnable();
231  virtual void onDisable();
232  virtual void fixedFrameChanged();
233 
234  RobotStateVisualizationPtr query_robot_start_;
235  RobotStateVisualizationPtr query_robot_goal_;
236 
237  Ogre::SceneNode* text_display_scene_node_;
240 
243 
244  // render the workspace box
245  std::unique_ptr<rviz::Shape> workspace_box_;
246 
247  // the planning frame
250 
251  // robot interaction
252  robot_interaction::RobotInteractionPtr robot_interaction_;
255  std::shared_ptr<interactive_markers::MenuHandler> menu_handler_start_;
256  std::shared_ptr<interactive_markers::MenuHandler> menu_handler_goal_;
257  std::map<std::string, LinkDisplayStatus> status_links_start_;
258  std::map<std::string, LinkDisplayStatus> status_links_goal_;
259 
262  std::set<std::string> modified_groups_;
263 
266  std::map<std::pair<bool, std::string>, std::map<std::string, double> > computed_metrics_;
268  std::map<std::string, bool> position_only_ik_;
269 
270  // Metric calculations
271  kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_;
272  std::map<std::string, dynamics_solver::DynamicsSolverPtr> dynamics_solver_;
273  boost::mutex update_metrics_lock_;
274 
275  // The trajectory playback component
276  TrajectoryVisualizationPtr trajectory_visual_;
277 
278  // properties to show on side panel
282 
293 
301 
303 };
304 
305 } // namespace moveit_rviz_plugin
306 
307 #endif
::robot_interaction::InteractionHandlerPtr InteractionHandlerPtr
kinematics_metrics::KinematicsMetricsPtr kinematics_metrics_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_start_
RobotStateVisualizationPtr query_robot_start_
Handles drawing the robot at the start configuration.
name
robot_state::RobotStateConstPtr getQueryStartState() const
std::map< std::string, LinkDisplayStatus > status_links_start_
std::shared_ptr< interactive_markers::MenuHandler > menu_handler_goal_
std::vector< std::shared_ptr< rviz::Shape > > place_locations_display_
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
std::map< std::string, bool > position_only_ik_
Some groups use position only ik, calls to the metrics have to be modified appropriately.
robot_interaction::RobotInteractionPtr robot_interaction_
bool text_display_for_start_
indicates whether the text display is for the start state or not
robot_interaction::RobotInteraction::InteractionHandlerPtr query_start_state_
rviz::EditableEnumProperty * planning_group_property_
std::map< std::string, dynamics_solver::DynamicsSolverPtr > dynamics_solver_
config
const robot_interaction::RobotInteractionPtr & getRobotInteraction() const
robot_state::RobotStateConstPtr getQueryGoalState() const
RobotStateVisualizationPtr query_robot_goal_
Handles drawing the robot at the goal configuration.
const robot_interaction::RobotInteraction::InteractionHandlerPtr & getQueryGoalStateHandler() const
const robot_interaction::RobotInteraction::InteractionHandlerPtr & getQueryStartStateHandler() const
robot_interaction::RobotInteraction::InteractionHandlerPtr query_goal_state_
std::map< std::pair< bool, std::string >, std::map< std::string, double > > computed_metrics_
Ogre::SceneNode * text_display_scene_node_
displays texts
std::map< std::string, LinkDisplayStatus > status_links_goal_
rviz::ColorProperty * query_outside_joint_limits_link_color_property_


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:19:08