TransformPublisherDisplay.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016, Bielefeld University, CITEC
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Author: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
30  */
31 
33 #include "TransformBroadcaster.h"
34 #include "rotation_property.h"
35 
42 
43 #include <rviz/display_factory.h>
44 #include <rviz/display_context.h>
45 #include <rviz/frame_manager.h>
48 #include <tf/transform_listener.h>
49 #include <QDebug>
50 
51 namespace vm = visualization_msgs;
52 const std::string MARKER_NAME = "marker";
53 
55 
56 namespace agni_tf_tools
57 {
58 
59 void static updatePose(geometry_msgs::Pose &pose,
60  const Eigen::Quaterniond &q,
61  Ogre::Vector3 p = Ogre::Vector3::ZERO)
62 {
63  pose.orientation.w = q.w();
64  pose.orientation.x = q.x();
65  pose.orientation.y = q.y();
66  pose.orientation.z = q.z();
67 
68  pose.position.x = p.x;
69  pose.position.y = p.y;
70  pose.position.z = p.z;
71 }
72 
73 
75  : rviz::Display()
76  , ignore_updates_(false)
77 {
78  translation_property_ = new rviz::VectorProperty("translation", Ogre::Vector3::ZERO, "", this);
79  rotation_property_ = new RotationProperty(this, "rotation");
80 
82  "parent frame", rviz::TfFrameProperty::FIXED_FRAME_STRING, "", this,
83  0, true, SLOT(onRefFrameChanged()), this);
85  "adapt transformation", false,
86  "Adapt transformation when changing the parent frame? "
87  "If so, the marker will not move.", this,
88  SLOT(onAdaptTransformChanged()), this);
90 
91  broadcast_property_ = new rviz::BoolProperty("publish transform", true, "", this,
92  SLOT(onBroadcastEnableChanged()), this);
94  "child frame", "", "", broadcast_property_,
95  0, false, SLOT(onFramesChanged()), this);
96 
97  connect(translation_property_, SIGNAL(changed()), this, SLOT(onTransformChanged()));
98  connect(rotation_property_, SIGNAL(quaternionChanged(Eigen::Quaterniond)), this, SLOT(onTransformChanged()));
99  connect(rotation_property_, SIGNAL(statusUpdate(int,QString,QString)),
100  this, SLOT(setStatus(int,QString,QString)));
101  tf_pub_ = new TransformBroadcaster("", "", this);
102 
103  marker_property_ = new rviz::EnumProperty("marker type", "interactive frame", "Choose which type of interactive marker to show",
104  this, SLOT(onMarkerTypeChanged()), this);
105  marker_property_->addOption("none", NONE);
106  marker_property_->addOption("static frame", FRAME);
107  marker_property_->addOption("interactive frame", IFRAME);
108  marker_property_->addOption("6 DoF handles", DOF6);
109 
110  marker_scale_property_ = new rviz::FloatProperty("marker scale", 0.2, "", marker_property_,
111  SLOT(onMarkerScaleChanged()), this);
112  marker_property_->hide(); // only show when marker is created
113 }
114 
116 {
117 }
118 
120 {
121  Display::onInitialize();
124  marker_node_ = getSceneNode()->createChildSceneNode();
125 
126  // show some children by default
127  this->expand();
129 }
130 
132 {
133  Display::reset();
134 }
135 
137 {
138  Display::onEnable();
139  tf_pub_->setEnabled(true);
140 }
141 
143 {
144  Display::onDisable();
145  tf_pub_->setEnabled(false);
147 }
148 
149 void TransformPublisherDisplay::update(float wall_dt, float ros_dt)
150 {
151  if (!this->isEnabled()) return;
152 
153  Display::update(wall_dt, ros_dt);
154  // create marker if not yet done
155  if (!imarker_ && marker_property_->getOptionInt() != NONE &&
157  setStatusStd(StatusProperty::Warn, MARKER_NAME, "Waiting for tf");
158  else if (imarker_)
159  imarker_->update(wall_dt); // get online marker updates
160 }
161 
162 
163 static vm::Marker createArrowMarker(double scale,
164  const Eigen::Vector3d &dir,
165  const QColor &color) {
166  vm::Marker marker;
167 
168  marker.type = vm::Marker::ARROW;
169  marker.scale.x = scale;
170  marker.scale.y = 0.1*scale;
171  marker.scale.z = 0.1*scale;
172 
173  updatePose(marker.pose,
174  Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), dir));
175 
176  marker.color.r = color.redF();
177  marker.color.g = color.greenF();
178  marker.color.b = color.blueF();
179  marker.color.a = color.alphaF();
180 
181  return marker;
182 }
183 
184 inline void setOrientation(geometry_msgs::Quaternion &q, double w, double x, double y, double z) {
185  q.w = w;
186  q.x = x;
187  q.y = y;
188  q.z = z;
189 }
190 
191 void TransformPublisherDisplay::addFrameControls(vm::InteractiveMarker &im, double scale, bool interactive)
192 {
193  vm::InteractiveMarkerControl ctrl;
194  setOrientation(ctrl.orientation, 1, 0,0,0);
195  ctrl.always_visible = true;
196  if (interactive) {
197  ctrl.orientation_mode = vm::InteractiveMarkerControl::VIEW_FACING;
198  ctrl.independent_marker_orientation = true;
199  ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_ROTATE_3D;
200  }
201  ctrl.name = "frame";
202 
203  ctrl.markers.push_back(createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitX(), QColor("red")));
204  ctrl.markers.push_back(createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitY(), QColor("green")));
205  ctrl.markers.push_back(createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitZ(), QColor("blue")));
206 
207  im.controls.push_back(ctrl);
208 }
209 
210 void TransformPublisherDisplay::add6DOFControls(vm::InteractiveMarker &im) {
211  vm::InteractiveMarkerControl ctrl;
212  ctrl.always_visible = false;
213 
214  setOrientation(ctrl.orientation, 1, 1,0,0);
215  ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
216  ctrl.name = "x pos";
217  im.controls.push_back(ctrl);
218  ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
219  ctrl.name = "x rot";
220  im.controls.push_back(ctrl);
221 
222  setOrientation(ctrl.orientation, 1, 0,1,0);
223  ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
224  ctrl.name = "y pos";
225  im.controls.push_back(ctrl);
226  ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
227  ctrl.name = "y rot";
228  im.controls.push_back(ctrl);
229 
230  setOrientation(ctrl.orientation, 1, 0,0,1);
231  ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
232  ctrl.name = "z pos";
233  im.controls.push_back(ctrl);
234  ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
235  ctrl.name = "z rot";
236  im.controls.push_back(ctrl);
237 }
238 
240 {
241  if (type == NONE) {
242  if (imarker_)
243  imarker_.reset();
244  return true;
245  }
246 
247  float scale = marker_scale_property_->getFloat();
248 
249  vm::InteractiveMarker im;
250  im.name = MARKER_NAME;
251  im.scale = scale;
252  if (!fillPoseStamped(im.header, im.pose)) return false;
253 
254  if (type == FRAME || type == IFRAME)
255  addFrameControls(im, 1.0, type == IFRAME);
256  else if (type == DOF6) {
257  addFrameControls(im, 0.5, type == IFRAME);
258  add6DOFControls(im);
259  }
260 
262  connect(imarker_.get(), SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)),
263  this, SLOT(onMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
264  connect(imarker_.get(), SIGNAL(statusUpdate(StatusProperty::Level,std::string,std::string)),
265  this, SLOT(setStatusStd(StatusProperty::Level,std::string,std::string)));
266 
268 
269  // fill in default controls
271 
272  imarker_->processMessage(im);
273  imarker_->setShowVisualAids(false);
274  imarker_->setShowAxes(false);
275  imarker_->setShowDescription(false);
276 
278  return true;
279 }
280 
281 
282 bool TransformPublisherDisplay::fillPoseStamped(std_msgs::Header &header,
283  geometry_msgs::Pose &pose)
284 {
285  const std::string &parent_frame = parent_frame_property_->getFrameStd();
286  std::string error;
287  if (context_->getFrameManager()->transformHasProblems(parent_frame, ros::Time(), error))
288  {
290  return false;
291  }
293 
294  const Eigen::Quaterniond &q = rotation_property_->getQuaternion();
295  const Ogre::Vector3 &p = translation_property_->getVector();
296  updatePose(pose, q, p);
297  header.frame_id = parent_frame;
298  // frame-lock marker to update marker pose with frame updates
299  header.stamp = ros::Time();
300  return true;
301 }
302 
303 void TransformPublisherDisplay::setStatus(int level, const QString &name, const QString &text)
304 {
305  if (level == rviz::StatusProperty::Ok && text.isEmpty()) {
306  Display::setStatus(static_cast<rviz::StatusProperty::Level>(level), name, text);
307  Display::deleteStatus(name);
308  } else
309  Display::setStatus(static_cast<rviz::StatusProperty::Level>(level), name, text);
310 }
311 
313  const std::string &name, const std::string &text)
314 {
315  setStatus(level, QString::fromStdString(name), QString::fromStdString(text));
316 }
317 
318 static bool getTransform(rviz::FrameManager &fm, const std::string &frame, Eigen::Affine3d &tf)
319 {
320  Ogre::Vector3 p = Ogre::Vector3::ZERO;
321  Ogre::Quaternion q = Ogre::Quaternion::IDENTITY;
322 
323  bool success = fm.getTransform(frame, ros::Time(), p, q);
324  tf = Eigen::Translation3d(p.x, p.y, p.z) * Eigen::Quaterniond(q.w, q.x, q.y, q.z);
325  return success || frame == rviz::TfFrameProperty::FIXED_FRAME_STRING.toStdString();
326 }
327 
329 {
330  // update pose to be relative to new reference frame
331  Eigen::Affine3d prevRef, nextRef;
333  if (getTransform(fm, prev_parent_frame_, prevRef) &&
335  const Ogre::Vector3 &p = translation_property_->getVector();
336  Eigen::Affine3d curPose = Eigen::Translation3d(p.x, p.y, p.z) * rotation_property_->getQuaternion();
337  Eigen::Affine3d newPose = nextRef.inverse() * prevRef * curPose;
338  Eigen::Vector3d t = newPose.translation();
339  ignore_updates_ = true;
340  translation_property_->setVector(Ogre::Vector3(t.x(), t.y(), t.z()));
341  rotation_property_->setQuaternion(Eigen::Quaterniond(newPose.rotation()));
342  ignore_updates_ = false;
343  }
345  onFramesChanged();
346 }
347 
349 {
352  else
353  prev_parent_frame_ = "";
354 }
355 
357 {
358  // update marker pose
359  vm::InteractiveMarkerPose marker_pose;
360  fillPoseStamped(marker_pose.header, marker_pose.pose);
361  if (imarker_) imarker_->processMessage(marker_pose);
362 
363  // prepare transform for broadcasting
364  geometry_msgs::TransformStamped tf;
365  tf.header.frame_id = parent_frame_property_->getFrameStd();
366  tf.child_frame_id = child_frame_property_->getFrameStd();
367  tf.transform.translation.x = marker_pose.pose.position.x;
368  tf.transform.translation.y = marker_pose.pose.position.y;
369  tf.transform.translation.z = marker_pose.pose.position.z;
370  tf.transform.rotation = marker_pose.pose.orientation;
371  tf_pub_->setValue(tf);
372 }
373 
375 {
376  if (ignore_updates_) return;
377 
378  vm::InteractiveMarkerPose marker_pose;
379  fillPoseStamped(marker_pose.header, marker_pose.pose);
380 
381  // update marker pose + broadcast pose
382  ignore_updates_ = true;
383  if (imarker_) imarker_->processMessage(marker_pose);
384  ignore_updates_ = false;
385  tf_pub_->setPose(marker_pose.pose);
386 }
387 
388 void TransformPublisherDisplay::onMarkerFeedback(vm::InteractiveMarkerFeedback &feedback)
389 {
390  if (ignore_updates_) return;
391  if (feedback.event_type != vm::InteractiveMarkerFeedback::POSE_UPDATE) return;
392 
393  // convert to parent frame
394  const geometry_msgs::Point &p_in = feedback.pose.position;
395  const geometry_msgs::Quaternion &q_in = feedback.pose.orientation;
396 
397  tf::Stamped<tf::Pose> pose_in(tf::Transform(tf::Quaternion(q_in.x, q_in.y, q_in.z, q_in.w),
398  tf::Vector3(p_in.x, p_in.y, p_in.z)),
399  feedback.header.stamp, feedback.header.frame_id);
400  tf::Stamped<tf::Pose> pose_out;
401  try {
403  pose_in, pose_out);
404  } catch(const std::runtime_error &e) {
405  ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s",
406  feedback.header.frame_id.c_str(),
408  e.what());
409  return;
410  }
411 
412  const tf::Vector3 &p = pose_out.getOrigin();
413  const tf::Quaternion &q = pose_out.getRotation();
414 
415  ignore_updates_ = true;
416  translation_property_->setVector(Ogre::Vector3(p.x(), p.y(), p.z()));
417  rotation_property_->setQuaternion(Eigen::Quaterniond(q.w(), q.x(), q.y(), q.z()));
418  ignore_updates_ = false;
419 
420  updatePose(feedback.pose, rotation_property_->getQuaternion(),
422  tf_pub_->setPose(feedback.pose);
423 }
424 
426 {
428 }
429 
431 {
433 }
434 
436 {
437  if (marker_scale_property_->getFloat() <= 0)
440 }
441 
442 } // namespace agni_tf_tools
const std::string MARKER_NAME
virtual void expand()
virtual bool setVector(const Ogre::Vector3 &vector)
bool transformHasProblems(const std::string &frame, ros::Time time, std::string &error)
virtual tf::TransformListener * getTFClient() const =0
DisplayContext * context_
Ogre::SceneNode * getSceneNode() const
void setStatusStd(StatusProperty::Level, const std::string &name, const std::string &text)
void setStatus(int level, const QString &name, const QString &text)
virtual float getFloat() const
void add6DOFControls(visualization_msgs::InteractiveMarker &im)
void autoComplete(visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency=true)
static vm::Marker createArrowMarker(double scale, const Eigen::Vector3d &dir, const QColor &color)
void setOrientation(geometry_msgs::Quaternion &q, double w, double x, double y, double z)
void onMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
bool isEnabled() const
virtual bool getBool() const
bool setFloat(float new_value)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Eigen::Quaterniond getQuaternion() const
virtual void addOption(const QString &option, int value=0)
TFSIMD_FORCE_INLINE const tfScalar & z() const
std::string getFrameStd() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
boost::shared_ptr< rviz::InteractiveMarker > imarker_
void setQuaternion(const Eigen::Quaterniond &q)
virtual FrameManager * getFrameManager() const =0
void addFrameControls(visualization_msgs::InteractiveMarker &im, double scale, bool interactive)
void setEnabled(bool bEnabled=true)
virtual Ogre::Vector3 getVector() const
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
static const QString FIXED_FRAME_STRING
static bool getTransform(rviz::FrameManager &fm, const std::string &frame, Eigen::Affine3d &tf)
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void setFrameManager(FrameManager *frame_manager)
void setPose(const geometry_msgs::Pose &pose)
static void updatePose(geometry_msgs::Pose &pose, const Eigen::Quaterniond &q, Ogre::Vector3 p=Ogre::Vector3::ZERO)
void setValue(const geometry_msgs::TransformStamped &tf)
virtual int getOptionInt()
#define ROS_DEBUG(...)
bool fillPoseStamped(std_msgs::Header &header, geometry_msgs::Pose &pose)


agni_tf_tools
Author(s): Robert Haschke
autogenerated on Fri Jun 7 2019 22:04:59