TransformPublisherDisplay.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2016, Bielefeld University, CITEC
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  * Author: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
00030  */
00031 
00032 #include "TransformPublisherDisplay.h"
00033 #include "TransformBroadcaster.h"
00034 #include "rotation_property.h"
00035 
00036 #include <rviz/properties/string_property.h>
00037 #include <rviz/properties/bool_property.h>
00038 #include <rviz/properties/float_property.h>
00039 #include <rviz/properties/vector_property.h>
00040 #include <rviz/properties/tf_frame_property.h>
00041 #include <rviz/properties/enum_property.h>
00042 
00043 #include <rviz/display_factory.h>
00044 #include <rviz/display_context.h>
00045 #include <rviz/frame_manager.h>
00046 #include <rviz/default_plugin/interactive_markers/interactive_marker.h>
00047 #include <interactive_markers/tools.h>
00048 #include <tf/transform_listener.h>
00049 #include <QDebug>
00050 
00051 namespace vm = visualization_msgs;
00052 const std::string MARKER_NAME = "marker";
00053 
00054 enum MARKER_TYPE { NONE, FRAME, IFRAME, DOF6 };
00055 
00056 namespace agni_tf_tools
00057 {
00058 
00059 void static updatePose(geometry_msgs::Pose &pose,
00060                        const Eigen::Quaterniond &q,
00061                        Ogre::Vector3 p = Ogre::Vector3::ZERO)
00062 {
00063   pose.orientation.w = q.w();
00064   pose.orientation.x = q.x();
00065   pose.orientation.y = q.y();
00066   pose.orientation.z = q.z();
00067 
00068   pose.position.x = p.x;
00069   pose.position.y = p.y;
00070   pose.position.z = p.z;
00071 }
00072 
00073 
00074 TransformPublisherDisplay::TransformPublisherDisplay()
00075   : rviz::Display()
00076   , ignore_updates_(false)
00077 {
00078   translation_property_ = new rviz::VectorProperty("translation", Ogre::Vector3::ZERO, "", this);
00079   rotation_property_ = new RotationProperty(this, "rotation");
00080 
00081   parent_frame_property_ = new rviz::TfFrameProperty(
00082         "parent frame", rviz::TfFrameProperty::FIXED_FRAME_STRING, "", this,
00083         0, true, SLOT(onRefFrameChanged()), this);
00084   adapt_transform_property_ = new rviz::BoolProperty(
00085         "adapt transformation", false,
00086         "Adapt transformation when changing the parent frame? "
00087         "If so, the marker will not move.", this,
00088         SLOT(onAdaptTransformChanged()), this);
00089   onAdaptTransformChanged();
00090 
00091   broadcast_property_ = new rviz::BoolProperty("publish transform", true, "", this,
00092                                                SLOT(onBroadcastEnableChanged()), this);
00093   child_frame_property_ = new rviz::TfFrameProperty(
00094         "child frame", "", "", broadcast_property_,
00095         0, false, SLOT(onFramesChanged()), this);
00096 
00097   connect(translation_property_, SIGNAL(changed()), this, SLOT(onTransformChanged()));
00098   connect(rotation_property_, SIGNAL(quaternionChanged(Eigen::Quaterniond)), this, SLOT(onTransformChanged()));
00099   connect(rotation_property_, SIGNAL(statusUpdate(int,QString,QString)),
00100           this, SLOT(setStatus(int,QString,QString)));
00101   tf_pub_ = new TransformBroadcaster("", "", this);
00102 
00103   marker_property_ = new rviz::EnumProperty("marker type", "interactive frame", "Choose which type of interactive marker to show",
00104                                             this, SLOT(onMarkerTypeChanged()), this);
00105   marker_property_->addOption("none", NONE);
00106   marker_property_->addOption("static frame", FRAME);
00107   marker_property_->addOption("interactive frame", IFRAME);
00108   marker_property_->addOption("6 DoF handles", DOF6);
00109 
00110   marker_scale_property_ = new rviz::FloatProperty("marker scale", 0.2, "", marker_property_,
00111                                                    SLOT(onMarkerScaleChanged()), this);
00112   marker_property_->hide(); // only show when marker is created
00113 }
00114 
00115 TransformPublisherDisplay::~TransformPublisherDisplay()
00116 {
00117 }
00118 
00119 void TransformPublisherDisplay::onInitialize()
00120 {
00121   Display::onInitialize();
00122   parent_frame_property_->setFrameManager(context_->getFrameManager());
00123   child_frame_property_->setFrameManager(context_->getFrameManager());
00124   marker_node_ = getSceneNode()->createChildSceneNode();
00125 
00126   // show some children by default
00127   this->expand();
00128   broadcast_property_->expand();
00129 }
00130 
00131 void TransformPublisherDisplay::reset()
00132 {
00133   Display::reset();
00134 }
00135 
00136 void TransformPublisherDisplay::onEnable()
00137 {
00138   Display::onEnable();
00139   tf_pub_->setEnabled(true);
00140 }
00141 
00142 void TransformPublisherDisplay::onDisable()
00143 {
00144   Display::onDisable();
00145   tf_pub_->setEnabled(false);
00146   createInteractiveMarker(NONE);
00147 }
00148 
00149 void TransformPublisherDisplay::update(float wall_dt, float ros_dt)
00150 {
00151   if (!this->isEnabled()) return;
00152 
00153   Display::update(wall_dt, ros_dt);
00154   // create marker if not yet done
00155   if (!imarker_ && marker_property_->getOptionInt() != NONE &&
00156       !createInteractiveMarker(marker_property_->getOptionInt()))
00157     setStatusStd(StatusProperty::Warn, MARKER_NAME, "Waiting for tf");
00158   else if (imarker_)
00159     imarker_->update(wall_dt); // get online marker updates
00160 }
00161 
00162 
00163 static vm::Marker createArrowMarker(double scale,
00164                                     const Eigen::Vector3d &dir,
00165                                     const QColor &color) {
00166   vm::Marker marker;
00167 
00168   marker.type = vm::Marker::ARROW;
00169   marker.scale.x = scale;
00170   marker.scale.y = 0.1*scale;
00171   marker.scale.z = 0.1*scale;
00172 
00173   updatePose(marker.pose,
00174              Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), dir));
00175 
00176   marker.color.r = color.redF();
00177   marker.color.g = color.greenF();
00178   marker.color.b = color.blueF();
00179   marker.color.a = color.alphaF();
00180 
00181   return marker;
00182 }
00183 
00184 inline void setOrientation(geometry_msgs::Quaternion &q, double w, double x, double y, double z) {
00185   q.w = w;
00186   q.x = x;
00187   q.y = y;
00188   q.z = z;
00189 }
00190 
00191 void TransformPublisherDisplay::addFrameControls(vm::InteractiveMarker &im, double scale, bool interactive)
00192 {
00193   vm::InteractiveMarkerControl ctrl;
00194   setOrientation(ctrl.orientation, 1, 0,0,0);
00195   ctrl.always_visible = true;
00196   if (interactive) {
00197     ctrl.orientation_mode = vm::InteractiveMarkerControl::VIEW_FACING;
00198     ctrl.independent_marker_orientation = true;
00199     ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_ROTATE_3D;
00200   }
00201   ctrl.name = "frame";
00202 
00203   ctrl.markers.push_back(createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitX(), QColor("red")));
00204   ctrl.markers.push_back(createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitY(), QColor("green")));
00205   ctrl.markers.push_back(createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitZ(), QColor("blue")));
00206 
00207   im.controls.push_back(ctrl);
00208 }
00209 
00210 void TransformPublisherDisplay::add6DOFControls(vm::InteractiveMarker &im) {
00211   vm::InteractiveMarkerControl ctrl;
00212   ctrl.always_visible = false;
00213 
00214   setOrientation(ctrl.orientation, 1, 1,0,0);
00215   ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
00216   ctrl.name = "x pos";
00217   im.controls.push_back(ctrl);
00218   ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
00219   ctrl.name = "x rot";
00220   im.controls.push_back(ctrl);
00221 
00222   setOrientation(ctrl.orientation, 1, 0,1,0);
00223   ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
00224   ctrl.name = "y pos";
00225   im.controls.push_back(ctrl);
00226   ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
00227   ctrl.name = "y rot";
00228   im.controls.push_back(ctrl);
00229 
00230   setOrientation(ctrl.orientation, 1, 0,0,1);
00231   ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
00232   ctrl.name = "z pos";
00233   im.controls.push_back(ctrl);
00234   ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
00235   ctrl.name = "z rot";
00236   im.controls.push_back(ctrl);
00237 }
00238 
00239 bool TransformPublisherDisplay::createInteractiveMarker(int type)
00240 {
00241   if (type == NONE) {
00242     if (imarker_)
00243       imarker_.reset();
00244     return true;
00245   }
00246 
00247   float scale = marker_scale_property_->getFloat();
00248 
00249   vm::InteractiveMarker im;
00250   im.name = MARKER_NAME;
00251   im.scale = scale;
00252   if (!fillPoseStamped(im.header, im.pose)) return false;
00253 
00254   if (type == FRAME || type == IFRAME)
00255     addFrameControls(im, 1.0, type == IFRAME);
00256   else if (type == DOF6) {
00257     addFrameControls(im, 0.5, type == IFRAME);
00258     add6DOFControls(im);
00259   }
00260 
00261   imarker_.reset(new rviz::InteractiveMarker(marker_node_, context_));
00262   connect(imarker_.get(), SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)),
00263           this, SLOT(onMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
00264   connect(imarker_.get(), SIGNAL(statusUpdate(StatusProperty::Level,std::string,std::string)),
00265           this, SLOT(setStatusStd(StatusProperty::Level,std::string,std::string)));
00266 
00267   setStatusStd(rviz::StatusProperty::Ok, MARKER_NAME, "Ok");
00268 
00269   // fill in default controls
00270   interactive_markers::autoComplete(im, true);
00271 
00272   imarker_->processMessage(im);
00273   imarker_->setShowVisualAids(false);
00274   imarker_->setShowAxes(false);
00275   imarker_->setShowDescription(false);
00276 
00277   marker_property_->show();
00278   return true;
00279 }
00280 
00281 
00282 bool TransformPublisherDisplay::fillPoseStamped(std_msgs::Header &header,
00283                                                 geometry_msgs::Pose &pose)
00284 {
00285   const std::string &parent_frame = parent_frame_property_->getFrameStd();
00286   std::string error;
00287   if (context_->getFrameManager()->transformHasProblems(parent_frame, ros::Time(), error))
00288   {
00289     setStatusStd(StatusProperty::Error, MARKER_NAME, error);
00290     return false;
00291   }
00292   setStatusStd(StatusProperty::Ok, MARKER_NAME, "");
00293 
00294   const Eigen::Quaterniond &q = rotation_property_->getQuaternion();
00295   const Ogre::Vector3 &p = translation_property_->getVector();
00296   updatePose(pose, q, p);
00297   header.frame_id = parent_frame;
00298   // frame-lock marker to update marker pose with frame updates
00299   header.stamp = ros::Time();
00300   return true;
00301 }
00302 
00303 void TransformPublisherDisplay::setStatus(int level, const QString &name, const QString &text)
00304 {
00305   if (level == rviz::StatusProperty::Ok && text.isEmpty()) {
00306     Display::setStatus(static_cast<rviz::StatusProperty::Level>(level), name, text);
00307     Display::deleteStatus(name);
00308   } else
00309     Display::setStatus(static_cast<rviz::StatusProperty::Level>(level), name, text);
00310 }
00311 
00312 void TransformPublisherDisplay::setStatusStd(rviz::StatusProperty::Level level,
00313                                              const std::string &name, const std::string &text)
00314 {
00315   setStatus(level, QString::fromStdString(name), QString::fromStdString(text));
00316 }
00317 
00318 static bool getTransform(rviz::FrameManager &fm, const std::string &frame, Eigen::Affine3d &tf)
00319 {
00320   Ogre::Vector3 p = Ogre::Vector3::ZERO;
00321   Ogre::Quaternion q = Ogre::Quaternion::IDENTITY;
00322 
00323   bool success = fm.getTransform(frame, ros::Time(), p, q);
00324   tf = Eigen::Translation3d(p.x, p.y, p.z) * Eigen::Quaterniond(q.w, q.x, q.y, q.z);
00325   return success || frame == rviz::TfFrameProperty::FIXED_FRAME_STRING.toStdString();
00326 }
00327 
00328 void TransformPublisherDisplay::onRefFrameChanged()
00329 {
00330   // update pose to be relative to new reference frame
00331   Eigen::Affine3d prevRef, nextRef;
00332   rviz::FrameManager &fm = *context_->getFrameManager();
00333   if (getTransform(fm, prev_parent_frame_, prevRef) &&
00334       getTransform(fm, parent_frame_property_->getFrameStd(), nextRef)) {
00335     const Ogre::Vector3 &p = translation_property_->getVector();
00336     Eigen::Affine3d curPose = Eigen::Translation3d(p.x, p.y, p.z) * rotation_property_->getQuaternion();
00337     Eigen::Affine3d newPose = nextRef.inverse() * prevRef * curPose;
00338     Eigen::Vector3d t = newPose.translation();
00339     ignore_updates_ = true;
00340     translation_property_->setVector(Ogre::Vector3(t.x(), t.y(), t.z()));
00341     rotation_property_->setQuaternion(Eigen::Quaterniond(newPose.rotation()));
00342     ignore_updates_ = false;
00343   }
00344   onAdaptTransformChanged();
00345   onFramesChanged();
00346 }
00347 
00348 void TransformPublisherDisplay::onAdaptTransformChanged()
00349 {
00350   if (adapt_transform_property_->getBool())
00351     prev_parent_frame_ = parent_frame_property_->getFrameStd();
00352   else
00353     prev_parent_frame_ = "";
00354 }
00355 
00356 void TransformPublisherDisplay::onFramesChanged()
00357 {
00358   // update marker pose
00359   vm::InteractiveMarkerPose marker_pose;
00360   fillPoseStamped(marker_pose.header, marker_pose.pose);
00361   if (imarker_) imarker_->processMessage(marker_pose);
00362 
00363   // prepare transform for broadcasting
00364   geometry_msgs::TransformStamped tf;
00365   tf.header.frame_id = parent_frame_property_->getFrameStd();
00366   tf.child_frame_id = child_frame_property_->getFrameStd();
00367   tf.transform.translation.x = marker_pose.pose.position.x;
00368   tf.transform.translation.y = marker_pose.pose.position.y;
00369   tf.transform.translation.z = marker_pose.pose.position.z;
00370   tf.transform.rotation = marker_pose.pose.orientation;
00371   tf_pub_->setValue(tf);
00372 }
00373 
00374 void TransformPublisherDisplay::onTransformChanged()
00375 {
00376   if (ignore_updates_) return;
00377 
00378   vm::InteractiveMarkerPose marker_pose;
00379   fillPoseStamped(marker_pose.header, marker_pose.pose);
00380 
00381   // update marker pose + broadcast pose
00382   ignore_updates_ = true;
00383   if (imarker_) imarker_->processMessage(marker_pose);
00384   ignore_updates_ = false;
00385   tf_pub_->setPose(marker_pose.pose);
00386 }
00387 
00388 void TransformPublisherDisplay::onMarkerFeedback(vm::InteractiveMarkerFeedback &feedback)
00389 {
00390   if (ignore_updates_) return;
00391   if (feedback.event_type != vm::InteractiveMarkerFeedback::POSE_UPDATE) return;
00392 
00393   // convert to parent frame
00394   const geometry_msgs::Point &p_in = feedback.pose.position;
00395   const geometry_msgs::Quaternion &q_in = feedback.pose.orientation;
00396 
00397   tf::Stamped<tf::Pose> pose_in(tf::Transform(tf::Quaternion(q_in.x, q_in.y, q_in.z, q_in.w),
00398                                               tf::Vector3(p_in.x, p_in.y, p_in.z)),
00399                                 feedback.header.stamp, feedback.header.frame_id);
00400   tf::Stamped<tf::Pose> pose_out;
00401   try {
00402     context_->getTFClient()->transformPose(parent_frame_property_->getFrameStd(),
00403                                            pose_in, pose_out);
00404   } catch(const std::runtime_error &e) {
00405     ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s",
00406               feedback.header.frame_id.c_str(),
00407               parent_frame_property_->getFrameStd().c_str(),
00408               e.what());
00409     return;
00410   }
00411 
00412   const tf::Vector3 &p = pose_out.getOrigin();
00413   const tf::Quaternion &q = pose_out.getRotation();
00414 
00415   ignore_updates_ = true;
00416   translation_property_->setVector(Ogre::Vector3(p.x(), p.y(), p.z()));
00417   rotation_property_->setQuaternion(Eigen::Quaterniond(q.w(), q.x(), q.y(), q.z()));
00418   ignore_updates_ = false;
00419 
00420   updatePose(feedback.pose, rotation_property_->getQuaternion(),
00421              translation_property_->getVector());
00422   tf_pub_->setPose(feedback.pose);
00423 }
00424 
00425 void TransformPublisherDisplay::onBroadcastEnableChanged()
00426 {
00427   tf_pub_->setEnabled(broadcast_property_->getBool());
00428 }
00429 
00430 void TransformPublisherDisplay::onMarkerTypeChanged()
00431 {
00432   createInteractiveMarker(marker_property_->getOptionInt());
00433 }
00434 
00435 void TransformPublisherDisplay::onMarkerScaleChanged()
00436 {
00437   if (marker_scale_property_->getFloat() <= 0)
00438     marker_scale_property_->setFloat(0.2);
00439   createInteractiveMarker(marker_property_->getOptionInt());
00440 }
00441 
00442 } // namespace agni_tf_tools


agni_tf_tools
Author(s): Robert Haschke
autogenerated on Sat Jun 8 2019 21:01:20