50 namespace vm = visualization_msgs;
58 const Eigen::Quaterniond& q,
59 Ogre::Vector3 p = Ogre::Vector3::ZERO) {
60 pose.orientation.w = q.w();
61 pose.orientation.x = q.x();
62 pose.orientation.y = q.y();
63 pose.orientation.z = q.z();
65 pose.position.x = p.x;
66 pose.position.y = p.y;
67 pose.position.z = p.z;
80 "Adapt transformation when changing the parent frame? " 81 "If so, the marker will not move.",
100 "Choose which type of interactive marker to show",
this,
116 Display::onInitialize();
138 Display::onDisable();
147 Display::update(wall_dt, ros_dt);
157 static vm::Marker
createArrowMarker(
double scale,
const Eigen::Vector3d& dir,
const QColor& color) {
160 marker.type = vm::Marker::ARROW;
161 marker.scale.x = scale;
162 marker.scale.y = 0.1 * scale;
163 marker.scale.z = 0.1 * scale;
165 updatePose(marker.pose, Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), dir));
167 marker.color.r = color.redF();
168 marker.color.g = color.greenF();
169 marker.color.b = color.blueF();
170 marker.color.a = color.alphaF();
175 inline void setOrientation(geometry_msgs::Quaternion& q,
double w,
double x,
double y,
double z) {
185 vm::InteractiveMarkerControl ctrl;
187 ctrl.always_visible =
true;
189 ctrl.orientation_mode = vm::InteractiveMarkerControl::VIEW_FACING;
190 ctrl.independent_marker_orientation =
true;
191 ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_ROTATE_3D;
195 ctrl.markers.push_back(
createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitX(), QColor(
"red")));
196 ctrl.markers.push_back(
createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitY(), QColor(
"green")));
197 ctrl.markers.push_back(
createArrowMarker(im.scale * scale, Eigen::Vector3d::UnitZ(), QColor(
"blue")));
199 im.controls.push_back(ctrl);
203 vm::InteractiveMarkerControl ctrl;
204 ctrl.always_visible =
false;
207 ctrl.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS;
209 im.controls.push_back(ctrl);
210 ctrl.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS;
212 im.controls.push_back(ctrl);
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);
240 vm::InteractiveMarker im;
248 else if (type ==
DOF6) {
267 imarker_->setShowDescription(
false);
290 header.frame_id = parent_frame;
298 const QString& text) {
300 Display::setStatus(level, name, text);
301 Display::deleteStatus(name);
303 Display::setStatus(level, name, text);
307 const std::string& name,
308 const std::string& text) {
309 setStatus(level, QString::fromStdString(name), QString::fromStdString(text));
313 Ogre::Vector3 p = Ogre::Vector3::ZERO;
314 Ogre::Quaternion q = Ogre::Quaternion::IDENTITY;
317 tf = Eigen::Translation3d(p.x, p.y, p.z) * Eigen::Quaterniond(q.w, q.x, q.y, q.z);
323 Eigen::Affine3d prevRef, nextRef;
329 Eigen::Affine3d newPose = nextRef.inverse() * prevRef * curPose;
330 Eigen::Vector3d
t = newPose.translation();
349 vm::InteractiveMarkerPose marker_pose;
353 imarker_->processMessage(marker_pose);
356 geometry_msgs::TransformStamped
tf;
359 tf.transform.translation.x = marker_pose.pose.position.x;
360 tf.transform.translation.y = marker_pose.pose.position.y;
361 tf.transform.translation.z = marker_pose.pose.position.z;
362 tf.transform.rotation = marker_pose.pose.orientation;
370 vm::InteractiveMarkerPose marker_pose;
377 imarker_->processMessage(marker_pose);
385 if (feedback.event_type != vm::InteractiveMarkerFeedback::POSE_UPDATE)
389 geometry_msgs::Pose pose;
393 feedback.header.frame_id,
394 feedback.header.stamp));
395 }
catch (
const std::runtime_error& e) {
396 ROS_DEBUG(
"Error transforming from frame '%s' to frame '%s': %s", feedback.header.frame_id.c_str(),
401 const geometry_msgs::Point& p = pose.position;
402 const geometry_msgs::Quaternion& q = pose.orientation;
virtual bool setVector(const Ogre::Vector3 &vector)
bool transformHasProblems(const std::string &frame, ros::Time time, std::string &error)
DisplayContext * context_
Ogre::SceneNode * getSceneNode() const
virtual float getFloat() const
INTERACTIVE_MARKERS_PUBLIC void autoComplete(visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency=true)
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
geometry_msgs::TransformStamped t
virtual bool getBool() const
bool setFloat(float new_value)
virtual void addOption(const QString &option, int value=0)
std::string getFrameStd() const
virtual FrameManager * getFrameManager() const =0
void userFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
virtual Ogre::Vector3 getVector() const
static const QString FIXED_FRAME_STRING
void statusUpdate(StatusProperty::Level level, const std::string &name, const std::string &text)
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void setFrameManager(FrameManager *frame_manager)
virtual int getOptionInt()