interactive_marker_control.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
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 
30 #include <OgreViewport.h>
31 #include <OgreCamera.h>
32 #include <OgreSceneNode.h>
33 #include <OgreSceneManager.h>
34 #include <OgrePass.h>
35 #include <OgreMaterial.h>
36 #include <OgreEntity.h>
37 #include <OgreSubEntity.h>
38 #include <OgreSharedPtr.h>
39 #include <OgreTechnique.h>
40 
41 #include "rviz/display_context.h"
43 #include "rviz/render_panel.h"
44 #include "rviz/load_resource.h"
46 #include "rviz/geometry.h"
47 #include "rviz/frame_manager.h"
48 
49 #include "rviz/ogre_helpers/line.h"
50 
53 
56 
57 #define NO_HIGHLIGHT_VALUE 0.0
58 #define ACTIVE_HIGHLIGHT_VALUE 0.5
59 #define HOVER_HIGHLIGHT_VALUE 0.3
60 
61 namespace rviz
62 {
64  Ogre::SceneNode* reference_node,
65  InteractiveMarker* parent)
66  : mouse_dragging_(false)
67  , drag_viewport_(nullptr)
68  , context_(context)
69  , reference_node_(reference_node)
70  , control_frame_node_(reference_node_->createChildSceneNode())
71  , markers_node_(reference_node_->createChildSceneNode())
72  , parent_(parent)
73  , rotation_(0)
74  , grab_point_in_reference_frame_(0, 0, 0)
75  , interaction_enabled_(false)
76  , visible_(true)
77  , view_facing_(false)
78  , mouse_down_(false)
79  , line_(new Line(context->getSceneManager(), control_frame_node_))
80  , show_visual_aids_(false)
81 {
82  line_->setVisible(false);
83 }
84 
85 void InteractiveMarkerControl::makeMarkers(const visualization_msgs::InteractiveMarkerControl& message)
86 {
87  for (const auto& marker_msg_const : message.markers)
88  {
89  if (!checkMarkerMsg(marker_msg_const, nullptr))
90  continue; // ignore invalid markers
91 
92  // create a marker with the given type
93  MarkerBasePtr marker(createMarker(marker_msg_const.type, nullptr, context_, markers_node_));
94 
95  PointsMarkerPtr points_marker = boost::dynamic_pointer_cast<PointsMarker>(marker);
96  if (points_marker)
97  {
98  points_markers_.push_back(points_marker);
99  }
100 
101  visualization_msgs::MarkerPtr marker_msg(new visualization_msgs::Marker(marker_msg_const));
102 
103  if (marker_msg->header.frame_id.empty())
104  {
105  // Put Marker into fixed frame, so the constructor does not apply any tf transform.
106  // This effectively discards any tf information in the Marker and interprets its pose
107  // as relative to the Interactive Marker.
108  marker_msg->header.frame_id = context_->getFrameManager()->getFixedFrame();
109  marker->setMessage(marker_msg);
110  }
111  else
112  {
113  marker->setMessage(marker_msg);
114  // The marker will set its position relative to the fixed frame,
115  // but we have attached it to our own scene node, so we will have to correct for that.
116  marker->setPosition(markers_node_->convertWorldToLocalPosition(marker->getPosition()));
117  marker->setOrientation(markers_node_->convertWorldToLocalOrientation(marker->getOrientation()));
118  }
119  marker->setInteractiveObject(shared_from_this());
120 
121  addHighlightPass(marker->getMaterials());
122 
123  markers_.push_back(marker);
124  }
125 }
126 
128 {
129  context_->getSceneManager()->destroySceneNode(control_frame_node_);
130  context_->getSceneManager()->destroySceneNode(markers_node_);
131 
132  if (view_facing_)
133  {
134  context_->getSceneManager()->removeListener(this);
135  }
136 }
137 
138 void InteractiveMarkerControl::processMessage(const visualization_msgs::InteractiveMarkerControl& message)
139 {
140  name_ = message.name;
141  description_ = QString::fromStdString(message.description);
142  interaction_mode_ = message.interaction_mode;
143  always_visible_ = message.always_visible;
144  orientation_mode_ = message.orientation_mode;
145 
146  control_orientation_ = Ogre::Quaternion(message.orientation.w, message.orientation.x,
147  message.orientation.y, message.orientation.z);
148  control_orientation_.normalise();
149 
150  bool new_view_facingness =
151  (message.orientation_mode == visualization_msgs::InteractiveMarkerControl::VIEW_FACING);
152  if (new_view_facingness != view_facing_)
153  {
154  if (new_view_facingness)
155  {
156  context_->getSceneManager()->addListener(this);
157  }
158  else
159  {
160  context_->getSceneManager()->removeListener(this);
161  }
162  view_facing_ = new_view_facingness;
163  }
164 
165  independent_marker_orientation_ = message.independent_marker_orientation;
166 
167  // highlight_passes_ have raw pointers into the markers_, so must
168  // clear them at the same time.
169  highlight_passes_.clear();
170  markers_.clear();
171  points_markers_.clear();
172 
173  // Initially, the pose of this marker's node and the interactive
174  // marker are identical, but that may change.
175  control_frame_node_->setPosition(parent_->getPosition());
176  markers_node_->setPosition(parent_->getPosition());
177 
178  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::INHERIT)
179  {
180  control_frame_node_->setOrientation(parent_->getOrientation());
181  markers_node_->setOrientation(parent_->getOrientation());
182  }
183  else
184  {
185  control_frame_node_->setOrientation(Ogre::Quaternion::IDENTITY);
186  markers_node_->setOrientation(Ogre::Quaternion::IDENTITY);
187  }
188 
189  makeMarkers(message);
190 
191  status_msg_ = description_ + " ";
192 
193  Ogre::Vector3 control_dir = control_orientation_.xAxis() * 10000.0;
194  line_->setPoints(control_dir, -1 * control_dir);
195  line_->setVisible(false);
196 
197  // Create our own custom cursor
198  switch (interaction_mode_)
199  {
200  case visualization_msgs::InteractiveMarkerControl::NONE:
202  break;
203  case visualization_msgs::InteractiveMarkerControl::MENU:
204  cursor_ = rviz::makeIconCursor("package://rviz/icons/menu.svg");
205  status_msg_ += "<b>Left-Click:</b> Show menu.";
206  break;
207  case visualization_msgs::InteractiveMarkerControl::BUTTON:
209  status_msg_ += "<b>Left-Click:</b> Activate. ";
210  break;
211  case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
212  cursor_ = rviz::makeIconCursor("package://rviz/icons/move1d.svg");
213  status_msg_ += "<b>Left-Click:</b> Move. ";
214  break;
215  case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
216  cursor_ = rviz::makeIconCursor("package://rviz/icons/move2d.svg");
217  status_msg_ += "<b>Left-Click:</b> Move. ";
218  break;
219  case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
220  cursor_ = rviz::makeIconCursor("package://rviz/icons/rotate.svg");
221  status_msg_ += "<b>Left-Click:</b> Rotate. ";
222  break;
223  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
224  cursor_ = rviz::makeIconCursor("package://rviz/icons/moverotate.svg");
225  status_msg_ += "<b>Left-Click:</b> Move / Rotate. ";
226  break;
227  case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
228  cursor_ = rviz::makeIconCursor("package://rviz/icons/move2d.svg");
229  status_msg_ +=
230  "<b>Left-Click:</b> Move X/Y. <b>Shift + Left-Click / Left-Click + Wheel:</b> Move Z. ";
231  break;
232  case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
233  cursor_ = rviz::makeIconCursor("package://rviz/icons/rotate.svg");
234  status_msg_ += "<b>Left-Click:</b> Rotate around X/Y. <b>Shift-Left-Click:</b> Rotate around Z. ";
235  break;
236  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
237  cursor_ = rviz::makeIconCursor("package://rviz/icons/moverotate.svg");
238  status_msg_ += "<b>Left-Click:</b> Move X/Y. <b>Shift + Left-Click / Left-Click + Wheel:</b> Move "
239  "Z. <b>Ctrl + Left-Click:</b> Rotate around X/Y. <b>Ctrl + Shift + Left-Click:</b> "
240  "Rotate around Z. ";
241  break;
242  }
243 
244  if (parent_->hasMenu() && interaction_mode_ != visualization_msgs::InteractiveMarkerControl::MENU)
245  {
246  status_msg_ += "<b>Right-Click:</b> Show context menu.";
247  }
248 
249  // It's not clear to me why this one setOrientation() call needs to
250  // be here and not above makeMarkers() with the other
251  // setOrientation() calls, but it works correctly when here and
252  // incorrectly when there. Sorry. -hersh
253  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
255  {
256  markers_node_->setOrientation(parent_->getOrientation());
257  }
258 
260 }
261 
262 // This is an Ogre::SceneManager::Listener function, and is configured
263 // to be called only if this is a VIEW_FACING control.
264 void InteractiveMarkerControl::preFindVisibleObjects(Ogre::SceneManager* /*source*/,
265  Ogre::SceneManager::IlluminationRenderStage /*irs*/,
266  Ogre::Viewport* v)
267 {
269 }
270 
272 {
273  Ogre::Quaternion x_view_facing_rotation =
274  control_orientation_.xAxis().getRotationTo(v->getCamera()->getDerivedDirection());
275 
276  // rotate so z axis is up
277  Ogre::Vector3 z_axis_2 = x_view_facing_rotation * control_orientation_.zAxis();
278  Ogre::Quaternion align_yz_rotation = z_axis_2.getRotationTo(v->getCamera()->getDerivedUp());
279 
280  // rotate
281  Ogre::Quaternion rotate_around_x = Ogre::Quaternion(rotation_, v->getCamera()->getDerivedDirection());
282 
283  Ogre::Quaternion rotation = reference_node_->convertWorldToLocalOrientation(
284  rotate_around_x * align_yz_rotation * x_view_facing_rotation);
285 
286  control_frame_node_->setOrientation(rotation);
287 
289  {
290  markers_node_->setOrientation(rotation);
291  // we need to refresh the node manually, since the scene manager will only do this one frame
292  // later otherwise
293  markers_node_->_update(true, false);
294  }
295 }
296 
298 {
299  return visible_ || always_visible_;
300 }
301 
303 {
304  visible_ = visible;
305 
306  if (always_visible_)
307  {
308  markers_node_->setVisible(visible_);
309  }
310  else
311  {
313  }
314 }
315 
317 {
318  if (mouse_dragging_)
319  {
321  }
322 }
323 
325 {
326  if (mouse_down_)
327  {
328  return;
329  }
330 
331  interaction_enabled_ = enable;
333  if (!enable)
334  {
336  }
337 }
338 
339 void InteractiveMarkerControl::interactiveMarkerPoseChanged(Ogre::Vector3 int_marker_position,
340  Ogre::Quaternion int_marker_orientation)
341 {
342  control_frame_node_->setPosition(int_marker_position);
343  markers_node_->setPosition(int_marker_position);
344 
345  switch (orientation_mode_)
346  {
347  case visualization_msgs::InteractiveMarkerControl::INHERIT:
348  control_frame_node_->setOrientation(int_marker_orientation);
349  markers_node_->setOrientation(control_frame_node_->getOrientation());
350  break;
351 
352  case visualization_msgs::InteractiveMarkerControl::FIXED:
353  {
354  control_frame_node_->setOrientation(Ogre::Quaternion(rotation_, control_orientation_.xAxis()));
355  markers_node_->setOrientation(control_frame_node_->getOrientation());
356  break;
357  }
358 
359  case visualization_msgs::InteractiveMarkerControl::VIEW_FACING:
360  if (drag_viewport_)
361  {
363  }
365  {
366  markers_node_->setOrientation(int_marker_orientation);
367  }
368  break;
369 
370  default:
371  break;
372  }
373 }
374 
375 Ogre::Vector3 InteractiveMarkerControl::closestPointOnLineToPoint(const Ogre::Vector3& line_start,
376  const Ogre::Vector3& line_dir,
377  const Ogre::Vector3& test_point)
378 {
379  // Find closest point on projected ray to mouse point
380  // Math: if P is the start of the ray, v is the direction vector of
381  // the ray (not normalized), and X is the test point, then the
382  // closest point on the line to X is given by:
383  //
384  // (X-P).v
385  // P + v * -------
386  // v.v
387  // where "." is the dot product.
388  double factor = (test_point - line_start).dotProduct(line_dir) / line_dir.dotProduct(line_dir);
389  Ogre::Vector3 closest_point = line_start + line_dir * factor;
390  return closest_point;
391 }
392 
393 void InteractiveMarkerControl::rotate(Ogre::Ray& mouse_ray)
394 {
395  Ogre::Vector3 intersection_3d;
396  Ogre::Vector2 intersection_2d;
397  float ray_t;
398 
399  Ogre::Vector3 rotation_axis = control_frame_orientation_at_mouse_down_ * control_orientation_.xAxis();
400 
401  // Find rotation_center = 3D point closest to grab_point_ which is
402  // on the rotation axis, relative to the reference frame.
403  Ogre::Vector3 rotation_center = closestPointOnLineToPoint(
404  control_frame_node_->getPosition(), rotation_axis, grab_point_in_reference_frame_);
405 
406  // Find intersection of mouse_ray with plane centered at rotation_center.
407  if (intersectSomeYzPlane(mouse_ray, rotation_center, control_frame_orientation_at_mouse_down_,
408  intersection_3d, intersection_2d, ray_t))
409  {
410  // Not that efficient, but reduces code duplication...
411  rotate(intersection_3d);
412  }
413 }
414 
415 void InteractiveMarkerControl::rotate(const Ogre::Vector3& cursor_in_reference_frame)
416 {
417  Ogre::Vector3 rotation_axis = control_frame_orientation_at_mouse_down_ * control_orientation_.xAxis();
418 
419  // Find rotation_center = 3D point closest to grab_point_ which is
420  // on the rotation axis, relative to the reference frame.
421  Ogre::Vector3 rotation_center = closestPointOnLineToPoint(
422  control_frame_node_->getPosition(), rotation_axis, grab_point_in_reference_frame_);
423 
424 
425  Ogre::Vector3 grab_rel_center = grab_point_in_reference_frame_ - rotation_center;
426  Ogre::Vector3 center_to_cursor = cursor_in_reference_frame - rotation_center;
427  Ogre::Vector3 center_to_cursor_radial =
428  center_to_cursor - center_to_cursor.dotProduct(rotation_axis) * rotation_axis;
429 
430  Ogre::Quaternion orientation_change_since_mouse_down =
431  grab_rel_center.getRotationTo(center_to_cursor_radial, rotation_axis);
432 
433  Ogre::Radian rot;
434  Ogre::Vector3 axis;
435  orientation_change_since_mouse_down.ToAngleAxis(rot, axis);
436  // Quaternion::ToAngleAxis() always gives a positive angle. The
437  // axis it emits (in this case) will either be equal to
438  // rotation_axis or will be the negative of it. Doing the
439  // dot-product then gives either 1.0 or -1.0, which is just what
440  // we need to get the sign for our angle.
441  Ogre::Radian rotation_since_mouse_down = rot * axis.dotProduct(rotation_axis);
442 
443  rotation_ = rotation_at_mouse_down_ + rotation_since_mouse_down;
445  orientation_change_since_mouse_down * parent_orientation_at_mouse_down_, name_);
446 }
447 
448 Ogre::Ray
450 {
451  float width = event.viewport->getActualWidth() - 1;
452  float height = event.viewport->getActualHeight() - 1;
453 
454  Ogre::Ray mouse_ray =
455  event.viewport->getCamera()->getCameraToViewportRay((x + .5) / width, (y + .5) / height);
456 
457  // convert ray into reference frame
458  mouse_ray.setOrigin(reference_node_->convertWorldToLocalPosition(mouse_ray.getOrigin()));
459  mouse_ray.setDirection(reference_node_->convertWorldToLocalOrientation(Ogre::Quaternion::IDENTITY) *
460  mouse_ray.getDirection());
461 
462  return mouse_ray;
463 }
464 
466 {
467  mouse_x_at_drag_begin_ = event.x;
468  mouse_y_at_drag_begin_ = event.y;
469  modifiers_at_drag_begin_ = event.modifiers;
470 
472 
473  // ensure direction is unit vector
474  mouse_ray_at_drag_begin_.setDirection(mouse_ray_at_drag_begin_.getDirection().normalisedCopy());
475 }
476 
478 {
479  dx = event.x - mouse_x_at_drag_begin_;
480  dy = event.y - mouse_y_at_drag_begin_;
481  if (dx == 0 && dy == 0)
482  return false;
483 
486  return true;
487 }
488 
490 {
491  int dx;
492  int dy;
493 
494  if (!getRelativeMouseMotion(event, dx, dy))
495  return;
496 
497  static const double MOUSE_SCALE = 2 * 3.14 / 300; // 300 pixels = 360deg
498  Ogre::Radian rx(dx * MOUSE_SCALE);
499  Ogre::Radian ry(dy * MOUSE_SCALE);
500 
501  Ogre::Quaternion up_rot(rx, event.viewport->getCamera()->getRealUp());
502  Ogre::Quaternion right_rot(ry, event.viewport->getCamera()->getRealRight());
503 
504  parent_->setPose(parent_->getPosition(), up_rot * right_rot * parent_->getOrientation(), name_);
505 }
506 
508 {
509  int dx;
510  int dy;
511 
512  getRelativeMouseMotion(event, dx, dy);
513  if (std::abs(dy) > std::abs(dx))
514  dx = dy;
515  if (dx == 0)
516  return;
517 
518  static const double MOUSE_SCALE = 2 * 3.14 / 300; // 300 pixels = 360deg
519  Ogre::Radian rx(dx * MOUSE_SCALE);
520 
521  Ogre::Quaternion rot(rx, event.viewport->getCamera()->getRealDirection());
522 
524 }
525 
527 {
528  int dx;
529  int dy;
530 
531  getRelativeMouseMotion(event, dx, dy);
532  if (std::abs(dx) > std::abs(dy))
533  dy = -dx;
534  if (dy == 0)
535  return;
536 
537  double distance = -dy * mouse_z_scale_;
538  Ogre::Vector3 delta = distance * mouse_ray_at_drag_begin_.getDirection();
539 
541 
543 }
544 
546 {
547  // wheel_delta is in 1/8 degree and usually jumps 15 degrees at a time
548  static const double WHEEL_TO_PIXEL_SCALE = (1.0 / 8.0) * (2.0 / 15.0); // 2 pixels per click
549 
550  double distance = event.wheel_delta * WHEEL_TO_PIXEL_SCALE;
551  Ogre::Vector3 delta = distance * mouse_ray_at_drag_begin_.getDirection();
552 
554 
556 }
557 
558 void InteractiveMarkerControl::moveViewPlane(Ogre::Ray& mouse_ray, const ViewportMouseEvent& event)
559 {
560  // find plane on which mouse is moving
561  Ogre::Plane plane(event.viewport->getCamera()->getRealDirection(), grab_point_in_reference_frame_);
562 
563  // find intersection of mouse with the plane
564  std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(plane);
565  if (!intersection.first)
566  return;
567  Ogre::Vector3 mouse_position_on_plane = mouse_ray.getPoint(intersection.second);
568 
569  // move parent so grab position relative to parent coincides with new mouse position.
570  parent_->setPose(mouse_position_on_plane - grab_point_in_reference_frame_ +
573 }
574 
575 void InteractiveMarkerControl::movePlane(Ogre::Ray& mouse_ray)
576 {
577  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING && drag_viewport_)
578  {
580  }
581 
582  Ogre::Vector3 intersection_3d;
583  Ogre::Vector2 intersection_2d;
584  float ray_t;
585 
587  control_frame_node_->getOrientation(), intersection_3d, intersection_2d,
588  ray_t))
589  {
592  }
593 }
594 
595 void InteractiveMarkerControl::movePlane(const Ogre::Vector3& cursor_position_in_reference_frame)
596 {
597  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING && drag_viewport_)
598  {
600  }
601 
602  Ogre::Vector3 plane_normal = (control_frame_node_->getOrientation() * control_orientation_.xAxis());
603  Ogre::Vector3 displacement = (cursor_position_in_reference_frame - grab_point_in_reference_frame_);
604  Ogre::Vector3 displacement_on_plane =
605  displacement - displacement.dotProduct(plane_normal) * plane_normal;
606 
607  // set position of parent to parent_position_at_mouse_down_ + displacement_on_plane.
609  name_);
610 }
611 
614 void InteractiveMarkerControl::worldToScreen(const Ogre::Vector3& pos_rel_reference,
615  const Ogre::Viewport* viewport,
616  Ogre::Vector2& screen_pos)
617 {
618  Ogre::Vector3 world_pos = reference_node_->convertLocalToWorldPosition(pos_rel_reference);
619 
620  const Ogre::Camera* cam = viewport->getCamera();
621  Ogre::Vector3 homogeneous_screen_position =
622  cam->getProjectionMatrix() * (cam->getViewMatrix() * world_pos);
623 
624  double half_width = viewport->getActualWidth() / 2.0;
625  double half_height = viewport->getActualHeight() / 2.0;
626 
627  screen_pos.x = half_width + (half_width * homogeneous_screen_position.x) - .5;
628  screen_pos.y = half_height + (half_height * -homogeneous_screen_position.y) - .5;
629 }
630 
634 bool InteractiveMarkerControl::findClosestPoint(const Ogre::Ray& target_ray,
635  const Ogre::Ray& mouse_ray,
636  Ogre::Vector3& closest_point)
637 {
638  // Find the closest point on target_ray to any point on mouse_ray.
639  //
640  // Math taken from http://paulbourke.net/geometry/lineline3d/
641  // line P1->P2 is target_ray
642  // line P3->P4 is mouse_ray
643 
644  Ogre::Vector3 v13 = target_ray.getOrigin() - mouse_ray.getOrigin();
645  Ogre::Vector3 v43 = mouse_ray.getDirection();
646  Ogre::Vector3 v21 = target_ray.getDirection();
647  double d1343 = v13.dotProduct(v43);
648  double d4321 = v43.dotProduct(v21);
649  double d1321 = v13.dotProduct(v21);
650  double d4343 = v43.dotProduct(v43);
651  double d2121 = v21.dotProduct(v21);
652 
653  double denom = d2121 * d4343 - d4321 * d4321;
654  if (fabs(denom) <= Ogre::Matrix3::EPSILON)
655  {
656  return false;
657  }
658  double numer = d1343 * d4321 - d1321 * d4343;
659 
660  double mua = numer / denom;
661  closest_point = target_ray.getPoint(mua);
662  return true;
663 }
664 
665 void InteractiveMarkerControl::moveAxis(const Ogre::Ray& /*mouse_ray*/, const ViewportMouseEvent& event)
666 {
667  // compute control-axis ray based on grab_point_, etc.
668  Ogre::Ray control_ray;
669  control_ray.setOrigin(grab_point_in_reference_frame_);
670  control_ray.setDirection(control_frame_node_->getOrientation() * control_orientation_.xAxis());
671 
672  // project control-axis ray onto screen.
673  Ogre::Vector2 control_ray_screen_start, control_ray_screen_end;
674  worldToScreen(control_ray.getOrigin(), event.viewport, control_ray_screen_start);
675  worldToScreen(control_ray.getPoint(1), event.viewport, control_ray_screen_end);
676 
677  Ogre::Vector2 mouse_point(event.x, event.y);
678 
679  // Find closest point on projected ray to mouse point
680  // Math: if P is the start of the ray, v is the direction vector of
681  // the ray (not normalized), and X is the test point, then the
682  // closest point on the line to X is given by:
683  //
684  // (X-P).v
685  // P + v * -------
686  // v.v
687  // where "." is the dot product.
688  Ogre::Vector2 control_ray_screen_dir = control_ray_screen_end - control_ray_screen_start;
689  double denominator = control_ray_screen_dir.dotProduct(control_ray_screen_dir);
690  if (fabs(denominator) >
691  Ogre::Matrix3::EPSILON) // If the control ray is not straight in line with the view.
692  {
693  double factor =
694  (mouse_point - control_ray_screen_start).dotProduct(control_ray_screen_dir) / denominator;
695 
696  Ogre::Vector2 closest_screen_point = control_ray_screen_start + control_ray_screen_dir * factor;
697 
698  // make a new "mouse ray" for the point on the projected ray
699  Ogre::Ray new_mouse_ray =
700  getMouseRayInReferenceFrame(event, closest_screen_point.x, closest_screen_point.y);
701 
702  // find closest point on control-axis ray to new mouse ray (should intersect actually)
703  Ogre::Vector3 closest_point;
704  if (findClosestPoint(control_ray, new_mouse_ray, closest_point))
705  {
706  // set position of parent to closest_point - grab_point_ + parent_position_at_mouse_down_.
709  }
710  }
711 }
712 
713 void InteractiveMarkerControl::moveAxis(const Ogre::Vector3& cursor_position_in_reference_frame)
714 {
715  Ogre::Vector3 control_unit_direction =
716  (control_frame_node_->getOrientation() * control_orientation_.xAxis());
717  Ogre::Vector3 displacement_along_axis =
718  (cursor_position_in_reference_frame - grab_point_in_reference_frame_)
719  .dotProduct(control_unit_direction) *
720  control_unit_direction;
721 
722  // set position of parent to closest_point - grab_point_ + parent_position_at_mouse_down_.
724  name_);
725 }
726 
727 void InteractiveMarkerControl::moveRotate(Ogre::Ray& mouse_ray)
728 {
729  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING && drag_viewport_)
730  {
732  }
733 
734  Ogre::Vector3 new_drag_rel_ref;
735  Ogre::Vector2 intersection_2d;
736  float ray_t;
737 
738  // get rotation axis rel ref (constant for entire drag)
739  // - rotation_axis_
740 
741  // get current rotation center rel ref
742  // - compute rotation center rel control frame at mouse-down (constant for entire drag)
743  // - current rotation center rel ref = current control frame * above
744  Ogre::Matrix4 control_rel_ref;
745  control_rel_ref.makeTransform(control_frame_node_->getPosition(), Ogre::Vector3::UNIT_SCALE,
746  control_frame_node_->getOrientation());
747  Ogre::Vector3 rotation_center = control_rel_ref * rotation_center_rel_control_;
748 
749  // get previous 3D drag point rel ref
750  // - compute grab point rel control frame at mouse-down (constant for entire drag)
751  // - prev_drag_rel_ref = current control frame + above
752  Ogre::Vector3 prev_drag_rel_ref = control_rel_ref * grab_point_rel_control_;
753 
754  // get new 3D drag point rel ref (this is "intersection_3d" in rotate().)
755  // - intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_orientation )
756  if (intersectSomeYzPlane(mouse_ray, rotation_center, control_frame_node_->getOrientation(),
757  new_drag_rel_ref, intersection_2d, ray_t))
758  {
759  // compute rotation angle from old drag point to new.
760  // - prev_rel_center = prev_drag_rel_ref - rotation_center
761  // - new_rel_center = new_drag_rel_ref - rotation_center
762  // - rotation_change = prev_rel_center.getRotationTo( new_rel_center, rotation_axis )
763  // - get Radians of angle change
764  // - rotation_ += angle_change
765  // - parent_->rotate(rotation_change)
766  Ogre::Vector3 prev_rel_center = prev_drag_rel_ref - rotation_center;
767  Ogre::Vector3 new_rel_center = new_drag_rel_ref - rotation_center;
768  if (new_rel_center.length() > Ogre::Matrix3::EPSILON)
769  {
770  Ogre::Quaternion rotation_change = prev_rel_center.getRotationTo(new_rel_center, rotation_axis_);
771  Ogre::Radian rot;
772  Ogre::Vector3 axis;
773  rotation_change.ToAngleAxis(rot, axis);
774  // Quaternion::ToAngleAxis() always gives a positive angle. The
775  // axis it emits (in this case) will either be equal to
776  // rotation_axis or will be the negative of it. Doing the
777  // dot-product then gives either 1.0 or -1.0, which is just what
778  // we need to get the sign for our angle.
779  Ogre::Radian angle_change = rot * axis.dotProduct(rotation_axis_);
780  rotation_ += angle_change;
781  parent_->rotate(rotation_change, name_);
782 
783  // compute translation from rotated old drag point to new drag point.
784  // - pos_change = new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length())
785  // - parent_->translate(pos_change)
786  parent_->translate(new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length()),
787  name_);
788  }
789  }
790 }
791 
792 void InteractiveMarkerControl::moveRotate(const Ogre::Vector3& cursor_position_in_reference_frame,
793  bool lock_axis)
794 {
795  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING && drag_viewport_)
796  {
798  }
799 
800  // get rotation axis rel ref (constant for entire drag)
801  // - rotation_axis_
802 
803  // get current rotation center rel ref
804  // - compute rotation center rel control frame at mouse-down (constant for entire drag)
805  // - current rotation center rel ref = current control frame * above
806  Ogre::Matrix4 control_rel_ref;
807  control_rel_ref.makeTransform(control_frame_node_->getPosition(), Ogre::Vector3::UNIT_SCALE,
808  control_frame_node_->getOrientation());
809  Ogre::Vector3 rotation_center = control_rel_ref * rotation_center_rel_control_;
810 
811  // get previous 3D drag point rel ref
812  // - compute grab point rel control frame at mouse-down (constant for entire drag)
813  // - prev_drag_rel_ref = current control frame + above
814  Ogre::Vector3 prev_drag_rel_ref = control_rel_ref * grab_point_rel_control_;
815 
816 
817  Ogre::Vector3 new_drag_rel_ref = cursor_position_in_reference_frame;
818  if (lock_axis)
819  {
820  Ogre::Vector3 plane_normal = (control_frame_node_->getOrientation() * control_orientation_.xAxis());
821  Ogre::Vector3 perpendicular_offset =
822  (new_drag_rel_ref - prev_drag_rel_ref).dotProduct(plane_normal) * plane_normal;
823  new_drag_rel_ref -= perpendicular_offset;
824  }
825 
826  // Ogre::Vector3 new_drag_rel_ref = cursor_position_in_reference_frame;
827  // compute rotation angle from old drag point to new.
828  // - prev_rel_center = prev_drag_rel_ref - rotation_center
829  // - new_rel_center = new_drag_rel_ref - rotation_center
830  // - rotation_change = prev_rel_center.getRotationTo( new_rel_center, rotation_axis )
831  // - get Radians of angle change
832  // - rotation_ += angle_change
833  // - parent_->rotate(rotation_change)
834  Ogre::Vector3 prev_rel_center = prev_drag_rel_ref - rotation_center;
835  Ogre::Vector3 new_rel_center = new_drag_rel_ref - rotation_center;
836  if (new_rel_center.length() > Ogre::Matrix3::EPSILON)
837  {
838  Ogre::Quaternion rotation_change = prev_rel_center.getRotationTo(new_rel_center, rotation_axis_);
839  Ogre::Radian rot;
840  Ogre::Vector3 axis;
841  rotation_change.ToAngleAxis(rot, axis);
842  // Quaternion::ToAngleAxis() always gives a positive angle. The
843  // axis it emits (in this case) will either be equal to
844  // rotation_axis or will be the negative of it. Doing the
845  // dot-product then gives either 1.0 or -1.0, which is just what
846  // we need to get the sign for our angle.
847  Ogre::Radian angle_change = rot * axis.dotProduct(rotation_axis_);
848  rotation_ += angle_change;
849  parent_->rotate(rotation_change, name_);
850 
851  // compute translation from rotated old drag point to new drag point.
852  // - pos_change = new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length())
853  // - parent_->translate(pos_change)
854  parent_->translate(new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length()),
855  name_);
856  }
857 }
858 
859 void InteractiveMarkerControl::move3D(const Ogre::Vector3& cursor_position_in_reference_frame,
860  const Ogre::Quaternion& cursor_orientation_in_reference_frame)
861 {
862  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING && drag_viewport_)
863  {
865  }
866 
867  // parent_to_cursor_in_cursor_frame_at_grab_ = cursor_3D_orientation.Inverse()*(cursor_3D_pos -
868  // parent_->getPosition());
869  // rotation_cursor_to_parent_at_grab_ = cursor_3D_orientation.Inverse()*parent->getOrientation();
870 
871 
872  Ogre::Vector3 world_to_cursor_in_world_frame =
873  reference_node_->convertLocalToWorldPosition(cursor_position_in_reference_frame);
874  Ogre::Quaternion rotation_world_to_cursor =
875  reference_node_->convertLocalToWorldOrientation(cursor_orientation_in_reference_frame);
876 
877  // Ogre::Vector3 marker_to_cursor_in_cursor_frame =
878  // orientation_world_to_cursor.Inverse()*reference_node_->getOrientation()*grab_point_in_reference_frame_;
879 
880  Ogre::Vector3 world_to_cursor_in_cursor_frame =
881  rotation_world_to_cursor.Inverse() * world_to_cursor_in_world_frame;
882  Ogre::Vector3 world_to_marker_in_world_frame =
883  rotation_world_to_cursor *
884  (world_to_cursor_in_cursor_frame - parent_to_cursor_in_cursor_frame_at_grab_);
885  Ogre::Vector3 marker_position_in_reference_frame =
886  reference_node_->convertWorldToLocalPosition(world_to_marker_in_world_frame);
887  // Ogre::Quaternion marker_orientation_in_reference_frame =
888  // reference_node_->convertWorldToLocalOrientation(rotation_world_to_cursor*rotation_cursor_to_parent_at_grab_);
889 
890  parent_->setPose(marker_position_in_reference_frame, parent_->getOrientation(), name_);
891 }
892 
893 void InteractiveMarkerControl::rotate3D(const Ogre::Vector3& /*cursor_position_in_reference_frame*/,
894  const Ogre::Quaternion& cursor_orientation_in_reference_frame)
895 {
896  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING && drag_viewport_)
897  {
899  }
900 
901  // parent_to_cursor_in_cursor_frame_at_grab_ = cursor_3D_orientation.Inverse()*(cursor_3D_pos -
902  // parent_->getPosition());
903  // rotation_cursor_to_parent_at_grab_ = cursor_3D_orientation.Inverse()*parent->getOrientation();
904 
905 
906  // Ogre::Vector3 world_to_cursor_in_world_frame =
907  // reference_node_->convertLocalToWorldPosition(cursor_position_in_reference_frame);
908  Ogre::Quaternion rotation_world_to_cursor =
909  reference_node_->convertLocalToWorldOrientation(cursor_orientation_in_reference_frame);
910 
911  // Ogre::Vector3 marker_to_cursor_in_cursor_frame =
912  // orientation_world_to_cursor.Inverse()*reference_node_->getOrientation()*grab_point_in_reference_frame_;
913 
914  // Ogre::Vector3 world_to_cursor_in_cursor_frame =
915  // rotation_world_to_cursor.Inverse()*world_to_cursor_in_world_frame;
916  // Ogre::Vector3 world_to_marker_in_world_frame =
917  // rotation_world_to_cursor*(world_to_cursor_in_cursor_frame -
918  // parent_to_cursor_in_cursor_frame_at_grab_);
919  // Ogre::Vector3 marker_position_in_reference_frame =
920  // reference_node_->convertWorldToLocalPosition(world_to_marker_in_world_frame);
921  Ogre::Quaternion marker_orientation_in_reference_frame =
922  reference_node_->convertWorldToLocalOrientation(rotation_world_to_cursor *
924 
925  parent_->setPose(parent_->getPosition(), marker_orientation_in_reference_frame, name_);
926 }
927 
928 
929 void InteractiveMarkerControl::moveRotate3D(const Ogre::Vector3& cursor_position_in_reference_frame,
930  const Ogre::Quaternion& cursor_orientation_in_reference_frame)
931 {
932  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING && drag_viewport_)
933  {
935  }
936 
937  // parent_to_cursor_in_cursor_frame_at_grab_ = cursor_3D_orientation.Inverse()*(cursor_3D_pos -
938  // parent_->getPosition());
939  // rotation_cursor_to_parent_at_grab_ = cursor_3D_orientation.Inverse()*parent->getOrientation();
940 
941 
942  Ogre::Vector3 world_to_cursor_in_world_frame =
943  reference_node_->convertLocalToWorldPosition(cursor_position_in_reference_frame);
944  Ogre::Quaternion rotation_world_to_cursor =
945  reference_node_->convertLocalToWorldOrientation(cursor_orientation_in_reference_frame);
946 
947  // Ogre::Vector3 marker_to_cursor_in_cursor_frame =
948  // orientation_world_to_cursor.Inverse()*reference_node_->getOrientation()*grab_point_in_reference_frame_;
949 
950  Ogre::Vector3 world_to_cursor_in_cursor_frame =
951  rotation_world_to_cursor.Inverse() * world_to_cursor_in_world_frame;
952  Ogre::Vector3 world_to_marker_in_world_frame =
953  rotation_world_to_cursor *
954  (world_to_cursor_in_cursor_frame - parent_to_cursor_in_cursor_frame_at_grab_);
955  Ogre::Vector3 marker_position_in_reference_frame =
956  reference_node_->convertWorldToLocalPosition(world_to_marker_in_world_frame);
957  Ogre::Quaternion marker_orientation_in_reference_frame =
958  reference_node_->convertWorldToLocalOrientation(rotation_world_to_cursor *
960 
961  parent_->setPose(marker_position_in_reference_frame, marker_orientation_in_reference_frame, name_);
962 }
963 
965 {
966  if (hl == NO_HIGHLIGHT)
968  if (hl == HOVER_HIGHLIGHT)
970  if (hl == ACTIVE_HIGHLIGHT)
972 }
973 
975 {
976  std::set<Ogre::Pass*>::iterator it;
977  for (it = highlight_passes_.begin(); it != highlight_passes_.end(); it++)
978  {
979  (*it)->setAmbient(a, a, a);
980  }
981 
982  std::vector<PointsMarkerPtr>::iterator pm_it;
983  for (pm_it = points_markers_.begin(); pm_it != points_markers_.end(); pm_it++)
984  {
985  (*pm_it)->setHighlightColor(a, a, a);
986  }
987 }
988 
990 {
991  dragging_in_place_event_ = event;
992  dragging_in_place_event_.type = QEvent::MouseMove;
993 }
994 
996 {
997  // aleeper: Why is this check here? What happens when this mouse_dragging_ check isn't done at all?
998  // Or as an alternative to this minor API change, we could just manually set mouse_dragging_
999  // to true before calling this function from the 3D cursor code.
1000  // BUT that would be a hack...
1001  if (mouse_dragging_ || force)
1002  {
1003  line_->setVisible(false);
1004  mouse_dragging_ = false;
1005  drag_viewport_ = nullptr;
1006  parent_->stopDragging();
1007  }
1008 }
1009 
1010 // Almost a wholesale copy of the mouse event code... can these be combined?
1012  const Ogre::Vector3& cursor_3D_pos,
1013  const Ogre::Quaternion& cursor_3D_orientation)
1014 {
1015  // change dragging state
1016  switch (interaction_mode_)
1017  {
1018  case visualization_msgs::InteractiveMarkerControl::BUTTON:
1019  if (event.leftUp())
1020  {
1021  Ogre::Vector3 point_rel_world = cursor_3D_pos;
1022  bool got_3D_point = true;
1023 
1024  visualization_msgs::InteractiveMarkerFeedback feedback;
1025  feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK;
1026  feedback.control_name = name_;
1027  feedback.marker_name = parent_->getName();
1028  parent_->publishFeedback(feedback, got_3D_point, point_rel_world);
1029  }
1030  break;
1031 
1032  case visualization_msgs::InteractiveMarkerControl::MENU:
1033  if (event.leftUp())
1034  {
1035  // Save the 3D mouse point to send with the menu feedback, if any.
1036  Ogre::Vector3 three_d_point = cursor_3D_pos;
1037  bool valid_point = true;
1038  Ogre::Vector2 mouse_pos = project3DPointToViewportXY(event.viewport, three_d_point);
1039  QCursor::setPos(event.panel->mapToGlobal(QPoint(mouse_pos.x, mouse_pos.y)));
1040  parent_->showMenu(event, name_, three_d_point, valid_point);
1041  }
1042  break;
1043 
1044  case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
1045  case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
1046  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
1047  case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
1048  case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
1049  case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
1050  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
1051  if (event.leftDown())
1052  {
1054  drag_viewport_ = event.viewport;
1055 
1056  // recordDraggingInPlaceEvent( event );
1057  grab_point_in_reference_frame_ = reference_node_->convertWorldToLocalPosition(cursor_3D_pos);
1059  reference_node_->convertWorldToLocalOrientation(cursor_3D_orientation);
1060 
1062  cursor_3D_orientation.Inverse() *
1063  (cursor_3D_pos - reference_node_->convertLocalToWorldPosition(parent_->getPosition()));
1065  cursor_3D_orientation.Inverse() *
1066  reference_node_->convertLocalToWorldOrientation(parent_->getOrientation());
1067 
1070 
1071  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
1073  {
1075  }
1078 
1079  rotation_axis_ = control_frame_node_->getOrientation() * control_orientation_.xAxis();
1080 
1081  // Find rotation_center = 3D point closest to grab_point_ which is
1082  // on the rotation axis, relative to the reference frame.
1083  Ogre::Vector3 rotation_center_rel_ref = closestPointOnLineToPoint(
1085  Ogre::Matrix4 reference_rel_control_frame;
1086  reference_rel_control_frame.makeInverseTransform(control_frame_node_->getPosition(),
1087  Ogre::Vector3::UNIT_SCALE,
1088  control_frame_node_->getOrientation());
1089  rotation_center_rel_control_ = reference_rel_control_frame * rotation_center_rel_ref;
1090  grab_point_rel_control_ = reference_rel_control_frame * grab_point_in_reference_frame_;
1091  }
1092  if (event.leftUp())
1093  {
1094  // Alternatively, could just manually set mouse_dragging_ to true.
1095  stopDragging(true);
1096  }
1097  break;
1098 
1099  default:
1100  break;
1101  }
1102 
1103  if (event.leftDown())
1104  {
1106  }
1107  else if (event.leftUp())
1108  {
1110  }
1111 
1112  if (!parent_->handle3DCursorEvent(event, cursor_3D_pos, cursor_3D_orientation, name_))
1113  {
1114  if (event.type == QEvent::MouseMove && event.left()) // && mouse_dragging_)
1115  {
1116  Ogre::Vector3 cursor_position_in_reference_frame =
1117  reference_node_->convertWorldToLocalPosition(cursor_3D_pos);
1118  Ogre::Quaternion cursor_orientation_in_reference_frame =
1119  reference_node_->convertWorldToLocalOrientation(cursor_3D_orientation);
1120 
1121  switch (interaction_mode_)
1122  {
1123  case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
1124  rotate(cursor_position_in_reference_frame);
1125  break;
1126 
1127  case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
1128  moveAxis(cursor_position_in_reference_frame);
1129  break;
1130 
1131  case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
1132  movePlane(cursor_position_in_reference_frame);
1133  break;
1134 
1135  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
1136  moveRotate(cursor_position_in_reference_frame, true);
1137  break;
1138 
1139  case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
1140  move3D(cursor_position_in_reference_frame, cursor_orientation_in_reference_frame);
1141  break;
1142 
1143  case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
1144  rotate3D(cursor_position_in_reference_frame, cursor_orientation_in_reference_frame);
1145  break;
1146 
1147  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
1148  moveRotate3D(cursor_position_in_reference_frame, cursor_orientation_in_reference_frame);
1149  break;
1150 
1151  default:
1152  break;
1153  }
1154  }
1155  }
1156 }
1157 
1159 {
1160  // REMOVE ME ROS_INFO("Mouse event!");
1161  // * check if this is just a receive/lost focus event
1162  // * try to hand over the mouse event to the parent interactive marker
1163  // * otherwise, execute mouse move handling
1164 
1165  // handle receive/lose focus
1166  if (event.type == QEvent::FocusIn)
1167  {
1168  has_focus_ = true;
1169  std::set<Ogre::Pass*>::iterator it;
1172  }
1173  else if (event.type == QEvent::FocusOut)
1174  {
1175  stopDragging();
1176  has_focus_ = false;
1177  setHighlight(0.0);
1178  return;
1179  }
1180 
1181  mouse_down_ = event.left() || event.middle() || event.right();
1182 
1183  // change dragging state
1184  switch (interaction_mode_)
1185  {
1186  case visualization_msgs::InteractiveMarkerControl::BUTTON:
1187  if (event.leftUp())
1188  {
1189  Ogre::Vector3 point_rel_world;
1190  bool got_3D_point =
1191  context_->getSelectionManager()->get3DPoint(event.viewport, event.x, event.y, point_rel_world);
1192 
1193  visualization_msgs::InteractiveMarkerFeedback feedback;
1194  feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK;
1195  feedback.control_name = name_;
1196  feedback.marker_name = parent_->getName();
1197  parent_->publishFeedback(feedback, got_3D_point, point_rel_world);
1198  }
1199  break;
1200 
1201  case visualization_msgs::InteractiveMarkerControl::MENU:
1202  if (event.leftUp())
1203  {
1204  Ogre::Vector3 point_rel_world;
1205  bool got_3D_point =
1206  context_->getSelectionManager()->get3DPoint(event.viewport, event.x, event.y, point_rel_world);
1207  parent_->showMenu(event, name_, point_rel_world, got_3D_point);
1208  }
1209  break;
1210 
1211  case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
1212  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
1213  case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
1214  if (event.leftDown())
1217  visualization_msgs::InteractiveMarkerControl::VIEW_FACING);
1218  break;
1219 
1220  case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
1221  if (event.leftDown())
1222  beginMouseMovement(event, false);
1223  break;
1224 
1225  case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
1226  case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
1227  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
1228  if (event.leftDown())
1229  {
1230  // aleeper: This line was causing badness
1231  // orientation_mode_ = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
1232  beginMouseMovement(event, false);
1233  }
1234  else if (event.left() &&
1235  ((modifiers_at_drag_begin_ ^ event.modifiers) & (Qt::ShiftModifier | Qt::ControlModifier)))
1236  {
1237  // modifier buttons changed. Restart the drag.
1238  beginRelativeMouseMotion(event);
1239  }
1240  break;
1241 
1242  default:
1243  break;
1244  }
1245 
1246  if (!parent_->handleMouseEvent(event, name_))
1247  {
1248  if (event.type == QEvent::MouseMove && event.left() && mouse_dragging_)
1249  {
1251  handleMouseMovement(event);
1252  }
1253  else if (event.type == QEvent::Wheel && event.left() && mouse_dragging_)
1254  {
1255  handleMouseWheelMovement(event);
1256  }
1257  }
1258 
1259  if (event.leftDown())
1260  {
1262  }
1263  else if (event.leftUp())
1264  {
1266  stopDragging();
1267  }
1268 }
1269 
1271 {
1272  line_->setVisible(line_visible);
1273 
1275  mouse_dragging_ = true;
1276  drag_viewport_ = event.viewport;
1277 
1279  Ogre::Vector3 grab_point_in_world_frame;
1280  if (!context_->getSelectionManager()->get3DPoint(event.viewport, event.x, event.y,
1281  grab_point_in_world_frame))
1282  {
1283  // If we couldn't get a 3D point for the grab, just use the
1284  // current relative position of the control frame.
1286  }
1287  else
1288  {
1289  // If we could get a 3D point for the grab, convert it from
1290  // the world frame to the reference frame (in case those are different).
1292  reference_node_->convertWorldToLocalPosition(grab_point_in_world_frame);
1293  }
1296 
1297  QPoint absolute_mouse = QCursor::pos();
1298  mouse_relative_to_absolute_x_ = absolute_mouse.x() - event.x;
1299  mouse_relative_to_absolute_y_ = absolute_mouse.y() - event.y;
1300  beginRelativeMouseMotion(event);
1301 
1302  if (orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING && drag_viewport_)
1303  {
1305  }
1308 
1309  rotation_axis_ = control_frame_node_->getOrientation() * control_orientation_.xAxis();
1310 
1311  // Find rotation_center = 3D point closest to grab_point_ which is
1312  // on the rotation axis, relative to the reference frame.
1313  Ogre::Vector3 rotation_center_rel_ref =
1315  Ogre::Matrix4 reference_rel_control_frame;
1316  reference_rel_control_frame.makeInverseTransform(control_frame_node_->getPosition(),
1317  Ogre::Vector3::UNIT_SCALE,
1318  control_frame_node_->getOrientation());
1319  rotation_center_rel_control_ = reference_rel_control_frame * rotation_center_rel_ref;
1320  grab_point_rel_control_ = reference_rel_control_frame * grab_point_in_reference_frame_;
1321 
1322 
1323  // find mouse_z_scale_
1324  static const double DEFAULT_MOUSE_Z_SCALE = 0.001; // default to 1mm per pixel
1325  mouse_z_scale_ = DEFAULT_MOUSE_Z_SCALE;
1326 
1327  Ogre::Ray mouse_ray = getMouseRayInReferenceFrame(event, event.x, event.y);
1328  Ogre::Ray mouse_ray_10 = getMouseRayInReferenceFrame(event, event.x, event.y + 10);
1329 
1330  Ogre::Vector3 intersection_3d;
1331  Ogre::Vector3 intersection_3d_10;
1332  Ogre::Vector2 intersection_2d;
1333  float ray_t;
1334 
1335  if (intersectSomeYzPlane(mouse_ray, grab_point_in_reference_frame_,
1336  control_frame_node_->getOrientation(), intersection_3d, intersection_2d,
1337  ray_t))
1338  {
1339  if (intersectSomeYzPlane(mouse_ray_10, grab_point_in_reference_frame_,
1340  control_frame_node_->getOrientation(), intersection_3d_10, intersection_2d,
1341  ray_t))
1342  {
1343  mouse_z_scale_ = (intersection_3d_10 - intersection_3d).length() / 10.0;
1344  if (mouse_z_scale_ < std::numeric_limits<float>::min() * 100.0)
1345  mouse_z_scale_ = DEFAULT_MOUSE_Z_SCALE;
1346  }
1347  }
1348 }
1349 
1351 {
1352  Ogre::Ray mouse_ray = getMouseRayInReferenceFrame(event, event.x, event.y);
1353 
1354  bool do_rotation = false;
1355  switch (interaction_mode_)
1356  {
1357  case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
1358  moveAxis(mouse_ray, event);
1359  break;
1360 
1361  case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
1362  movePlane(mouse_ray);
1363  break;
1364 
1365  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
1366  moveRotate(mouse_ray);
1367  break;
1368 
1369  case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
1370  rotate(mouse_ray);
1371  break;
1372 
1373  case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
1374  do_rotation = true;
1375  // fall through
1376  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
1377  if (event.control())
1378  do_rotation = true;
1379  // fall through
1380  case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
1381  if (do_rotation)
1382  {
1383  if (event.shift())
1384  rotateZRelative(event);
1385  else
1386  rotateXYRelative(event);
1387  }
1388  else
1389  {
1390  if (event.shift())
1391  moveZAxisRelative(event);
1392  else
1393  moveViewPlane(mouse_ray, event);
1394  }
1395  break;
1396 
1397  default:
1398  break;
1399  }
1400 }
1401 
1403 {
1404  switch (interaction_mode_)
1405  {
1406  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
1407  case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
1408  moveZAxisWheel(event);
1409  break;
1410 
1411  default:
1412  break;
1413  }
1414 }
1415 
1416 bool InteractiveMarkerControl::intersectYzPlane(const Ogre::Ray& mouse_ray,
1417  Ogre::Vector3& intersection_3d,
1418  Ogre::Vector2& intersection_2d,
1419  float& ray_t)
1420 {
1421  return intersectSomeYzPlane(mouse_ray, control_frame_node_->getPosition(),
1422  control_frame_node_->getOrientation(), intersection_3d, intersection_2d,
1423  ray_t);
1424 }
1425 
1426 bool InteractiveMarkerControl::intersectSomeYzPlane(const Ogre::Ray& mouse_ray,
1427  const Ogre::Vector3& point_on_plane,
1428  const Ogre::Quaternion& plane_orientation,
1429  Ogre::Vector3& intersection_3d,
1430  Ogre::Vector2& intersection_2d,
1431  float& ray_t)
1432 {
1433  Ogre::Vector3 normal = plane_orientation * control_orientation_.xAxis();
1434  Ogre::Vector3 axis_1 = plane_orientation * control_orientation_.yAxis();
1435  Ogre::Vector3 axis_2 = plane_orientation * control_orientation_.zAxis();
1436 
1437  Ogre::Plane plane(normal, point_on_plane);
1438 
1439  Ogre::Vector2 origin_2d(point_on_plane.dotProduct(axis_1), point_on_plane.dotProduct(axis_2));
1440 
1441  std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(plane);
1442  if (intersection.first)
1443  {
1444  intersection_3d = mouse_ray.getPoint(intersection.second);
1445  intersection_2d =
1446  Ogre::Vector2(intersection_3d.dotProduct(axis_1), intersection_3d.dotProduct(axis_2));
1447  intersection_2d -= origin_2d;
1448 
1449  ray_t = intersection.second;
1450  return true;
1451  }
1452 
1453  ray_t = 0;
1454  return false;
1455 }
1456 
1458 {
1459  S_MaterialPtr::iterator it;
1460 
1461  for (it = materials.begin(); it != materials.end(); it++)
1462  {
1463  Ogre::MaterialPtr material = *it;
1464  Ogre::Pass* original_pass = material->getTechnique(0)->getPass(0);
1465  Ogre::Pass* pass = material->getTechnique(0)->createPass();
1466 
1467  pass->setSceneBlending(Ogre::SBT_ADD);
1468  pass->setDepthWriteEnabled(false);
1469  pass->setDepthCheckEnabled(true);
1470  pass->setLightingEnabled(true);
1471  pass->setAmbient(0, 0, 0);
1472  pass->setDiffuse(0, 0, 0, 0);
1473  pass->setSpecular(0, 0, 0, 0);
1474  pass->setCullingMode(original_pass->getCullingMode());
1475 
1476  highlight_passes_.insert(pass);
1477  }
1478 }
1479 
1480 } // namespace rviz
virtual void setStatus(const QString &message)=0
void translate(Ogre::Vector3 delta_position, const std::string &control_name)
void enableInteraction(bool enable) override
const Ogre::Quaternion & getOrientation()
QCursor makeIconCursor(QString url, bool fill_cache)
void rotate(Ogre::Quaternion delta_orientation, const std::string &control_name)
void handleMouseWheelMovement(ViewportMouseEvent &event)
void interactiveMarkerPoseChanged(Ogre::Vector3 int_marker_position, Ogre::Quaternion int_marker_orientation)
void beginMouseMovement(ViewportMouseEvent &event, bool line_visible)
void updateControlOrientationForViewFacing(Ogre::Viewport *v)
bool checkMarkerMsg(const visualization_msgs::Marker &marker, MarkerDisplay *owner)
std::set< Ogre::Pass * > highlight_passes_
void addHighlightPass(S_MaterialPtr materials)
take all the materials, add a highlight pass and store a pointer to the pass for later use ...
QCursor getDefaultCursor(bool)
void move3D(const Ogre::Vector3 &cursor_position_in_reference_frame, const Ogre::Quaternion &cursor_orientation_in_reference_frame)
bool intersectYzPlane(const Ogre::Ray &mouse_ray, Ogre::Vector3 &intersection_3d, Ogre::Vector2 &intersection_2d, float &ray_t)
compute intersection between mouse ray and y-z plane given in local coordinates
void moveViewPlane(Ogre::Ray &mouse_ray, const ViewportMouseEvent &event)
virtual void handle3DCursorEvent(ViewportMouseEvent event, const Ogre::Vector3 &cursor_3D_pos, const Ogre::Quaternion &cursor_3D_orientation)
#define NO_HIGHLIGHT_VALUE
std::vector< PointsMarkerPtr > points_markers_
#define HOVER_HIGHLIGHT_VALUE
InteractiveMarkerControl(DisplayContext *context, Ogre::SceneNode *reference_node, InteractiveMarker *parent)
Constructor.
void makeMarkers(const visualization_msgs::InteractiveMarkerControl &message)
Create marker objects from the message and add them to the internal marker arrays.
Ogre::Vector3 closestPointOnLineToPoint(const Ogre::Vector3 &line_start, const Ogre::Vector3 &line_dir, const Ogre::Vector3 &test_point)
void preFindVisibleObjects(Ogre::SceneManager *source, Ogre::SceneManager::IlluminationRenderStage irs, Ogre::Viewport *v) override
MarkerBase * createMarker(int marker_type, MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node)
const Ogre::Vector3 & getPosition()
Ogre::Vector2 project3DPointToViewportXY(const Ogre::Viewport *view, const Ogre::Vector3 &pos)
Given a viewport and a 3D position in world coordinates, project that point into the view plane...
Definition: geometry.cpp:75
void publishFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback, bool mouse_point_valid=false, const Ogre::Vector3 &mouse_point_rel_world=Ogre::Vector3(0, 0, 0))
Pure-virtual base class for objects which give Display subclasses context in which to work...
const std::string & getName()
void recordDraggingInPlaceEvent(ViewportMouseEvent &event)
bool handle3DCursorEvent(ViewportMouseEvent &event, const Ogre::Vector3 &cursor_pos, const Ogre::Quaternion &cursor_rot, const std::string &control_name)
Ogre::Ray getMouseRayInReferenceFrame(const ViewportMouseEvent &event, int x, int y)
void beginRelativeMouseMotion(const ViewportMouseEvent &event)
void setHighlight(const ControlHighlight &hl)
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void moveRotate3D(const Ogre::Vector3 &cursor_position_in_reference_frame, const Ogre::Quaternion &cursor_orientation_in_reference_frame)
virtual SelectionManager * getSelectionManager() const =0
Return a pointer to the SelectionManager.
void showMenu(ViewportMouseEvent &event, const std::string &control_name, const Ogre::Vector3 &three_d_point, bool valid_point)
void rotate3D(const Ogre::Vector3 &cursor_position_in_reference_frame, const Ogre::Quaternion &cursor_orientation_in_reference_frame)
void processMessage(const visualization_msgs::InteractiveMarkerControl &message)
Set up or update the contents of this control to match the specification in the message.
bool findClosestPoint(const Ogre::Ray &target_ray, const Ogre::Ray &mouse_ray, Ogre::Vector3 &closest_point)
std::vector< MarkerBasePtr > markers_
const std::string & getFixedFrame()
Return the current fixed frame name.
bool getRelativeMouseMotion(const ViewportMouseEvent &event, int &dx, int &dy)
void moveZAxisWheel(const ViewportMouseEvent &event)
void worldToScreen(const Ogre::Vector3 &pos_rel_reference, const Ogre::Viewport *viewport, Ogre::Vector2 &screen_pos)
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
#define ACTIVE_HIGHLIGHT_VALUE
bool intersectSomeYzPlane(const Ogre::Ray &mouse_ray, const Ogre::Vector3 &point_in_plane, const Ogre::Quaternion &plane_orientation, Ogre::Vector3 &intersection_3d, Ogre::Vector2 &intersection_2d, float &ray_t)
compute intersection between mouse ray and a y-z plane.
void moveZAxisRelative(const ViewportMouseEvent &event)
void setPose(Ogre::Vector3 position, Ogre::Quaternion orientation, const std::string &control_name)
void rotateXYRelative(const ViewportMouseEvent &event)
void handleMouseEvent(ViewportMouseEvent &event) override
void handleMouseMovement(ViewportMouseEvent &event)
void moveAxis(const Ogre::Ray &mouse_ray, const ViewportMouseEvent &event)
std::set< Ogre::MaterialPtr > S_MaterialPtr
Definition: marker_base.h:57
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
bool handleMouseEvent(ViewportMouseEvent &event, const std::string &control_name)
bool get3DPoint(Ogre::Viewport *viewport, const int x, const int y, Ogre::Vector3 &result_point)
void rotateZRelative(const ViewportMouseEvent &event)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Jan 9 2023 03:07:20