51 namespace vm = visualization_msgs;
60 const Eigen::Quaterniond &q,
61 Ogre::Vector3 p = Ogre::Vector3::ZERO)
63 pose.orientation.w = q.w();
64 pose.orientation.x = q.x();
65 pose.orientation.y = q.y();
66 pose.orientation.z = q.z();
68 pose.position.x = p.x;
69 pose.position.y = p.y;
70 pose.position.z = p.z;
76 , ignore_updates_(false)
85 "adapt transformation",
false,
86 "Adapt transformation when changing the parent frame? " 87 "If so, the marker will not move.",
this,
100 this, SLOT(
setStatus(
int,QString,QString)));
121 Display::onInitialize();
144 Display::onDisable();
153 Display::update(wall_dt, ros_dt);
164 const Eigen::Vector3d &dir,
165 const QColor &color) {
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;
174 Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), dir));
176 marker.color.r = color.redF();
177 marker.color.g = color.greenF();
178 marker.color.b = color.blueF();
179 marker.color.a = color.alphaF();
184 inline void setOrientation(geometry_msgs::Quaternion &q,
double w,
double x,
double y,
double z) {
193 vm::InteractiveMarkerControl ctrl;
195 ctrl.always_visible =
true;
197 ctrl.orientation_mode = vm::InteractiveMarkerControl::VIEW_FACING;
198 ctrl.independent_marker_orientation =
true;
199 ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_ROTATE_3D;
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")));
207 im.controls.push_back(ctrl);
211 vm::InteractiveMarkerControl ctrl;
212 ctrl.always_visible =
false;
215 ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
217 im.controls.push_back(ctrl);
218 ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
220 im.controls.push_back(ctrl);
223 ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
225 im.controls.push_back(ctrl);
226 ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
228 im.controls.push_back(ctrl);
231 ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
233 im.controls.push_back(ctrl);
234 ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
236 im.controls.push_back(ctrl);
249 vm::InteractiveMarker im;
256 else if (type ==
DOF6) {
262 connect(
imarker_.get(), SIGNAL(userFeedback(visualization_msgs::InteractiveMarkerFeedback&)),
263 this, SLOT(
onMarkerFeedback(visualization_msgs::InteractiveMarkerFeedback&)));
275 imarker_->setShowDescription(
false);
283 geometry_msgs::Pose &pose)
297 header.frame_id = parent_frame;
306 Display::setStatus(static_cast<rviz::StatusProperty::Level>(level), name, text);
307 Display::deleteStatus(name);
309 Display::setStatus(static_cast<rviz::StatusProperty::Level>(level), name, text);
313 const std::string &name,
const std::string &text)
315 setStatus(level, QString::fromStdString(name), QString::fromStdString(text));
320 Ogre::Vector3 p = Ogre::Vector3::ZERO;
321 Ogre::Quaternion q = Ogre::Quaternion::IDENTITY;
324 tf = Eigen::Translation3d(p.x, p.y, p.z) * Eigen::Quaterniond(q.w, q.x, q.y, q.z);
331 Eigen::Affine3d prevRef, nextRef;
337 Eigen::Affine3d newPose = nextRef.inverse() * prevRef * curPose;
338 Eigen::Vector3d t = newPose.translation();
359 vm::InteractiveMarkerPose marker_pose;
364 geometry_msgs::TransformStamped
tf;
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;
378 vm::InteractiveMarkerPose marker_pose;
391 if (feedback.event_type != vm::InteractiveMarkerFeedback::POSE_UPDATE)
return;
394 const geometry_msgs::Point &p_in = feedback.pose.position;
395 const geometry_msgs::Quaternion &q_in = feedback.pose.orientation;
399 feedback.header.stamp, feedback.header.frame_id);
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(),
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
virtual float getFloat() const
void autoComplete(visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency=true)
virtual bool getBool() const
bool setFloat(float new_value)
TFSIMD_FORCE_INLINE const tfScalar & x() 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
virtual FrameManager * getFrameManager() const =0
virtual Ogre::Vector3 getVector() const
static const QString FIXED_FRAME_STRING
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void setFrameManager(FrameManager *frame_manager)
virtual int getOptionInt()