trajectory_display.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036    Desc:   Wraps a trajectory_visualization playback class for Rviz into a stand alone display
00037 */
00038 
00039 #include <moveit/trajectory_rviz_plugin/trajectory_display.h>
00040 #include <rviz/properties/string_property.h>
00041 
00042 namespace moveit_rviz_plugin
00043 {
00044 TrajectoryDisplay::TrajectoryDisplay() : Display()
00045 {
00046   // The robot description property is only needed when using the trajectory playback standalone (not within motion
00047   // planning plugin)
00048   robot_description_property_ = new rviz::StringProperty(
00049       "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
00050       this, SLOT(changedRobotDescription()), this);
00051 
00052   trajectory_visual_.reset(new TrajectoryVisualization(this, this));
00053 }
00054 
00055 TrajectoryDisplay::~TrajectoryDisplay()
00056 {
00057 }
00058 
00059 void TrajectoryDisplay::onInitialize()
00060 {
00061   Display::onInitialize();
00062 
00063   trajectory_visual_->onInitialize(scene_node_, context_, update_nh_);
00064 }
00065 
00066 void TrajectoryDisplay::loadRobotModel()
00067 {
00068   rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString()));
00069 
00070   if (!rdf_loader_->getURDF())
00071   {
00072     ROS_DEBUG_STREAM_NAMED("trajectory_display", "Unable to load robot model from parameter "
00073                                                      << robot_description_property_->getStdString());
00074     return;
00075   }
00076 
00077   const boost::shared_ptr<srdf::Model>& srdf =
00078       rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : boost::shared_ptr<srdf::Model>(new srdf::Model());
00079   robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf));
00080 
00081   // Send to child class
00082   trajectory_visual_->onRobotModelLoaded(robot_model_);
00083 }
00084 
00085 void TrajectoryDisplay::reset()
00086 {
00087   Display::reset();
00088   loadRobotModel();
00089   trajectory_visual_->reset();
00090 }
00091 
00092 void TrajectoryDisplay::onEnable()
00093 {
00094   Display::onEnable();
00095   loadRobotModel();
00096   trajectory_visual_->onEnable();
00097 }
00098 
00099 void TrajectoryDisplay::onDisable()
00100 {
00101   Display::onDisable();
00102   trajectory_visual_->onDisable();
00103 }
00104 
00105 void TrajectoryDisplay::update(float wall_dt, float ros_dt)
00106 {
00107   Display::update(wall_dt, ros_dt);
00108   trajectory_visual_->update(wall_dt, ros_dt);
00109 }
00110 
00111 void TrajectoryDisplay::setName(const QString& name)
00112 {
00113   BoolProperty::setName(name);
00114   trajectory_visual_->setName(name);
00115 }
00116 
00117 void TrajectoryDisplay::changedRobotDescription()
00118 {
00119   loadRobotModel();
00120 
00121   if (isEnabled())
00122     reset();
00123 }
00124 
00125 }  // namespace moveit_rviz_plugin


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:22:14