trajectory_display.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, University of Colorado, Boulder
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 the Univ of CO, Boulder 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: Dave Coleman
36  Desc: Wraps a trajectory_visualization playback class for Rviz into a stand alone display
37 */
38 
41 
42 #include <memory>
43 
44 namespace moveit_rviz_plugin
45 {
47 {
48  // The robot description property is only needed when using the trajectory playback standalone (not within motion
49  // planning plugin)
50  robot_description_property_ =
51  new rviz::StringProperty("Robot Description", "robot_description",
52  "The name of the ROS parameter where the URDF for the robot is loaded", this,
53  SLOT(changedRobotDescription()), this);
54 
55  trajectory_visual_ = std::make_shared<TrajectoryVisualization>(this, this);
56 }
57 
59 
61 {
62  Display::onInitialize();
63 
65 }
66 
68 {
69  try
70  {
71  rdf_loader_ = std::make_shared<rdf_loader::RDFLoader>(robot_description_property_->getStdString());
72 
73  if (!rdf_loader_->getURDF())
74  {
75  this->setStatus(rviz::StatusProperty::Error, "Robot Model",
76  "Failed to load from parameter " + robot_description_property_->getString());
77  return;
78  }
79  this->setStatus(rviz::StatusProperty::Ok, "Robot Model", "Successfully loaded");
80 
81  const srdf::ModelSharedPtr& srdf =
82  rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : std::make_shared<srdf::Model>();
83  robot_model_ = std::make_shared<moveit::core::RobotModel>(rdf_loader_->getURDF(), srdf);
84 
85  // Send to child class
86  trajectory_visual_->onRobotModelLoaded(robot_model_);
87  }
88  catch (std::exception& e)
89  {
90  setStatus(rviz::StatusProperty::Error, "RobotModel", QString("Loading failed: %1").arg(e.what()));
91  }
92 }
93 
95 {
96  Display::reset();
98  trajectory_visual_->reset();
99 }
100 
101 void TrajectoryDisplay::load(const rviz::Config& config)
102 {
103  // This property needs to be loaded in onEnable() below, which is triggered
104  // in the beginning of Display::load() before the other property would be available
105  robot_description_property_->load(config.mapGetChild("Robot Description"));
106  Display::load(config);
107 }
108 
110 {
111  Display::onEnable();
112  if (!rdf_loader_)
113  loadRobotModel();
114  trajectory_visual_->onEnable();
115 }
116 
118 {
119  Display::onDisable();
120  trajectory_visual_->onDisable();
121 }
122 
123 void TrajectoryDisplay::update(float wall_dt, float ros_dt)
124 {
125  Display::update(wall_dt, ros_dt);
126  trajectory_visual_->update(wall_dt, ros_dt);
127 }
128 
129 void TrajectoryDisplay::setName(const QString& name)
130 {
131  BoolProperty::setName(name);
132  trajectory_visual_->setName(name);
133 }
134 
136 {
137  if (isEnabled())
138  reset();
139  else
140  loadRobotModel();
141 }
142 
143 } // namespace moveit_rviz_plugin
rviz::Display::isEnabled
bool isEnabled() const
moveit_rviz_plugin::TrajectoryDisplay::onInitialize
void onInitialize() override
Definition: trajectory_display.cpp:92
moveit_rviz_plugin::TrajectoryDisplay::~TrajectoryDisplay
~TrajectoryDisplay() override
moveit_rviz_plugin::TrajectoryDisplay::load
void load(const rviz::Config &config) override
Definition: trajectory_display.cpp:133
rviz::StatusProperty::Error
Error
moveit_rviz_plugin::TrajectoryDisplay::update
void update(float wall_dt, float ros_dt) override
Definition: trajectory_display.cpp:155
moveit_rviz_plugin::TrajectoryDisplay::setName
void setName(const QString &name) override
Definition: trajectory_display.cpp:161
moveit_rviz_plugin::TrajectoryDisplay::rdf_loader_
rdf_loader::RDFLoaderPtr rdf_loader_
Definition: trajectory_display.h:89
srdf::ModelSharedPtr
std::shared_ptr< Model > ModelSharedPtr
trajectory_display.h
moveit_rviz_plugin::TrajectoryDisplay::reset
void reset() override
Definition: trajectory_display.cpp:126
moveit_rviz_plugin::TrajectoryDisplay::loadRobotModel
void loadRobotModel()
Definition: trajectory_display.cpp:99
rviz::Display::setStatus
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
rviz::Display::scene_node_
Ogre::SceneNode * scene_node_
rviz::StringProperty
rviz::StringProperty::getStdString
std::string getStdString()
rviz::StatusProperty::Ok
Ok
rviz::StringProperty::getString
QString getString()
name
name
moveit_rviz_plugin::TrajectoryDisplay::onEnable
void onEnable() override
Definition: trajectory_display.cpp:141
moveit_rviz_plugin::TrajectoryDisplay::onDisable
void onDisable() override
Definition: trajectory_display.cpp:149
moveit_rviz_plugin
Definition: motion_planning_display.h:80
rviz::Display::context_
DisplayContext * context_
srdf
moveit_rviz_plugin::TrajectoryDisplay::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: trajectory_display.h:90
moveit_rviz_plugin::TrajectoryDisplay::changedRobotDescription
void changedRobotDescription()
Slot Event Functions.
Definition: trajectory_display.cpp:167
string_property.h
moveit_rviz_plugin::TrajectoryDisplay::TrajectoryDisplay
TrajectoryDisplay()
Definition: trajectory_display.cpp:78
moveit_rviz_plugin::TrajectoryDisplay::trajectory_visual_
TrajectoryVisualizationPtr trajectory_visual_
Definition: trajectory_display.h:86
config
config
rviz::Property::load
virtual void load(const Config &config)
rviz::Config
moveit_rviz_plugin::TrajectoryDisplay::robot_description_property_
rviz::StringProperty * robot_description_property_
Definition: trajectory_display.h:94
rviz::Display::update_nh_
ros::NodeHandle update_nh_


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