30 #include <boost/make_shared.hpp> 34 #include <OgreSceneNode.h> 35 #include <OgreSceneManager.h> 36 #include <OgreMaterialManager.h> 37 #include <OgreResourceGroupManager.h> 38 #include <OgreSubEntity.h> 40 #include <OgreRenderWindow.h> 61 , pose_changed_(false)
62 , time_since_last_feedback_(0)
64 , pose_update_requested_(false)
66 , show_visual_aids_(false)
80 boost::recursive_mutex::scoped_lock lock(
mutex_);
81 Ogre::Vector3 position( message.pose.position.x, message.pose.position.y, message.pose.position.z );
82 Ogre::Quaternion orientation( message.pose.orientation.w, message.pose.orientation.x,
83 message.pose.orientation.y, message.pose.orientation.z );
85 if ( orientation.w == 0 && orientation.x == 0 && orientation.y == 0 && orientation.z == 0 )
100 boost::recursive_mutex::scoped_lock lock(
mutex_);
103 name_ = message.name;
106 if ( message.controls.size() == 0 )
119 message.pose.position.x,
120 message.pose.position.y,
121 message.pose.position.z );
133 has_menu_ = message.menu_entries.size() > 0;
152 std::set<std::string> old_names_to_delete;
153 M_ControlPtr::const_iterator ci;
156 old_names_to_delete.insert( (*ci).first );
160 for (
unsigned i = 0; i < message.controls.size(); i++ )
162 const visualization_msgs::InteractiveMarkerControl& control_message = message.controls[ i ];
163 M_ControlPtr::iterator search_iter =
controls_.find( control_message.name );
170 control = (*search_iter).second;
176 controls_[ control_message.name ] = control;
179 control->processMessage( control_message );
183 old_names_to_delete.erase( control_message.name );
187 std::set<std::string>::iterator si;
188 for( si = old_names_to_delete.begin(); si != old_names_to_delete.end(); si++ )
195 boost::make_shared<InteractiveMarkerControl>(
context_,
205 menu_.reset(
new QMenu() );
210 for (
unsigned m=0; m < message.menu_entries.size(); m++ )
212 const visualization_msgs::MenuEntry& entry = message.menu_entries[ m ];
216 if( entry.parent_id == 0 )
223 std::map< uint32_t, MenuNode >::iterator parent_it =
menu_entries_.find( entry.parent_id );
225 ROS_ERROR(
"interactive marker menu entry %u found before its parent id %u. Ignoring.", entry.id, entry.parent_id);
229 (*parent_it).second.child_ids.push_back( entry.id );
238 std::ostringstream
s;
254 for(
size_t id_index = 0; id_index < ids.size(); id_index++ )
256 uint32_t
id = ids[ id_index ];
257 std::map< uint32_t, MenuNode >::iterator node_it =
menu_entries_.find(
id );
261 if ( node.child_ids.empty() )
265 (
int) node.entry.id );
266 connect( action, SIGNAL( triggered(
int )),
this, SLOT(
handleMenuSelect(
int )));
267 menu->addAction( action );
272 QMenu* sub_menu = menu->addMenu(
makeMenuString( node.entry.title ));
281 if ( entry.find(
"[x]" ) == 0 )
283 menu_entry = QChar( 0x2611 ) + QString::fromStdString( entry.substr( 3 ) );
285 else if ( entry.find(
"[ ]" ) == 0 )
287 menu_entry = QChar( 0x2610 ) + QString::fromStdString( entry.substr( 3 ) );
291 menu_entry = QChar( 0x3000 ) + QString::fromStdString( entry );
299 boost::recursive_mutex::scoped_lock lock(
mutex_);
300 Ogre::Vector3 reference_position;
301 Ogre::Quaternion reference_orientation;
322 std::ostringstream
s;
324 <<
" and " << fixed_frame <<
": " << error <<
" (error code: " << retval <<
")";
333 reference_position, reference_orientation ))
352 boost::recursive_mutex::scoped_lock lock(
mutex_);
359 M_ControlPtr::iterator it;
362 (*it).second->update();
378 visualization_msgs::InteractiveMarkerFeedback feedback;
379 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::KEEP_ALIVE;
387 boost::recursive_mutex::scoped_lock lock(
mutex_);
388 visualization_msgs::InteractiveMarkerFeedback feedback;
389 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
397 boost::recursive_mutex::scoped_lock lock(
mutex_);
407 setPose( position, orientation,
"" );
413 boost::recursive_mutex::scoped_lock lock(
mutex_);
422 M_ControlPtr::iterator it;
435 boost::recursive_mutex::scoped_lock lock(
mutex_);
444 boost::recursive_mutex::scoped_lock lock(
mutex_);
450 boost::recursive_mutex::scoped_lock lock(
mutex_);
451 M_ControlPtr::iterator it;
454 (*it).second->setShowVisualAids( show );
461 boost::recursive_mutex::scoped_lock lock(
mutex_);
467 boost::recursive_mutex::scoped_lock lock(
mutex_);
473 boost::recursive_mutex::scoped_lock lock(
mutex_);
480 boost::recursive_mutex::scoped_lock lock(
mutex_);
492 boost::recursive_mutex::scoped_lock lock(
mutex_);
496 Ogre::Vector3 point_rel_world = cursor_pos;
497 bool got_3D_point =
true;
499 visualization_msgs::InteractiveMarkerFeedback feedback;
500 feedback.control_name = control_name;
501 feedback.marker_name =
name_;
504 feedback.event_type = ((uint8_t)visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
507 feedback.event_type = (
event.type == QEvent::MouseButtonPress ?
508 (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN :
509 (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
522 if( event.
rightUp() &&
event.buttons_down == Qt::NoButton )
525 Ogre::Vector3 three_d_point = cursor_pos;
526 bool valid_point =
true;
528 QCursor::setPos(event.
panel->mapToGlobal(QPoint(mouse_pos.x, mouse_pos.y)));
529 showMenu( event, control_name, three_d_point, valid_point );
539 boost::recursive_mutex::scoped_lock lock(
mutex_);
543 Ogre::Vector3 point_rel_world;
547 visualization_msgs::InteractiveMarkerFeedback feedback;
548 feedback.control_name = control_name;
549 feedback.marker_name =
name_;
552 feedback.event_type = ((uint8_t)visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
555 feedback.event_type = (
event.type == QEvent::MouseButtonPress ?
556 (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN :
557 (uint8_t)visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
570 if( event.
rightUp() &&
event.buttons_down == Qt::NoButton )
573 Ogre::Vector3 three_d_point;
575 showMenu( event, control_name, three_d_point, valid_point );
590 event.panel->showContextMenu(
menu_ );
597 boost::recursive_mutex::scoped_lock lock(
mutex_);
599 std::map< uint32_t, MenuNode >::iterator it =
menu_entries_.find( menu_item_id );
603 visualization_msgs::MenuEntry& entry = it->second.entry;
605 std::string
command = entry.command;
606 uint8_t command_type = entry.command_type;
608 if ( command_type == visualization_msgs::MenuEntry::FEEDBACK )
610 visualization_msgs::InteractiveMarkerFeedback feedback;
611 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT;
612 feedback.menu_entry_id = entry.id;
616 else if ( command_type == visualization_msgs::MenuEntry::ROSRUN )
618 std::string sys_cmd =
"rosrun " + command;
623 else if ( command_type == visualization_msgs::MenuEntry::ROSLAUNCH )
625 std::string sys_cmd =
"roslaunch " + command;
635 bool mouse_point_valid,
636 const Ogre::Vector3& mouse_point_rel_world )
638 boost::recursive_mutex::scoped_lock lock(
mutex_);
640 feedback.marker_name =
name_;
658 feedback.mouse_point_valid = mouse_point_valid;
659 if( mouse_point_valid )
661 Ogre::Vector3 mouse_rel_reference =
reference_node_->convertWorldToLocalPosition( mouse_point_rel_world );
662 feedback.mouse_point.x = mouse_rel_reference.x;
663 feedback.mouse_point.y = mouse_rel_reference.y;
664 feedback.mouse_point.z = mouse_rel_reference.z;
678 feedback.pose.position.x = world_position.x;
679 feedback.pose.position.y = world_position.y;
680 feedback.pose.position.z = world_position.z;
681 feedback.pose.orientation.x = world_orientation.x;
682 feedback.pose.orientation.y = world_orientation.y;
683 feedback.pose.orientation.z = world_orientation.z;
684 feedback.pose.orientation.w = world_orientation.w;
686 feedback.mouse_point_valid = mouse_point_valid;
687 feedback.mouse_point.x = mouse_point_rel_world.x;
688 feedback.mouse_point.y = mouse_point_rel_world.y;
689 feedback.mouse_point.z = mouse_point_rel_world.z;
boost::recursive_mutex mutex_
void translate(Ogre::Vector3 delta_position, const std::string &control_name)
double time_since_last_feedback_
void setShowVisualAids(bool show)
std::map< uint32_t, MenuNode > menu_entries_
std::string last_control_name_
bool transformHasProblems(const std::string &frame, ros::Time time, std::string &error)
Check to see if a transform is known between a given frame and the fixed frame.
Ogre::Viewport * viewport
void setShowDescription(bool show)
void rotate(Ogre::Quaternion delta_orientation, const std::string &control_name)
visualization_msgs::InteractiveMarkerControl makeTitle(const visualization_msgs::InteractiveMarker &msg)
Ogre::Vector3 three_d_point_for_menu_
void populateMenu(QMenu *menu, std::vector< uint32_t > &ids)
Ogre::Vector3 requested_position_
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
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...
virtual ~InteractiveMarker()
QString makeMenuString(const std::string &entry)
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...
void handleMenuSelect(int menu_item_id)
ROSLIB_DECL std::string command(const std::string &cmd)
bool handle3DCursorEvent(ViewportMouseEvent &event, const Ogre::Vector3 &cursor_pos, const Ogre::Quaternion &cursor_rot, const std::string &control_name)
bool processMessage(const visualization_msgs::InteractiveMarker &message)
#define ROS_ASSERT_MSG(cond,...)
InteractiveMarker(Ogre::SceneNode *scene_node, DisplayContext *context)
Ogre::Quaternion requested_orientation_
InteractiveMarkerControlPtr description_control_
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
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)
tf::TransformListener * getTFClient()
Return the tf::TransformListener used to receive transform data.
const std::string & getFixedFrame()
Return the current fixed frame name.
float normalizeQuaternion(float &w, float &x, float &y, float &z)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
bool pose_update_requested_
Qt::MouseButton acting_button
void userFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
std::vector< uint32_t > top_level_menu_ids_
boost::shared_ptr< boost::thread > sys_thread_
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
virtual QString getFixedFrame() const =0
Return the fixed frame name.
#define ROS_INFO_STREAM(args)
void setPose(Ogre::Vector3 position, Ogre::Quaternion orientation, const std::string &control_name)
void statusUpdate(StatusProperty::Level level, const std::string &name, const std::string &text)
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
void set(float length, float radius)
Set the parameters on this object.
DisplayContext * context_
std::string reference_frame_
boost::shared_ptr< QMenu > menu_
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Ogre::Quaternion orientation_
void update(float wall_dt)
void setShowAxes(bool show)
void requestPoseUpdate(Ogre::Vector3 position, Ogre::Quaternion orientation)
bool handleMouseEvent(ViewportMouseEvent &event, const std::string &control_name)
bool got_3d_point_for_menu_
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
bool get3DPoint(Ogre::Viewport *viewport, const int x, const int y, Ogre::Vector3 &result_point)
void updateReferencePose()
ros::Time reference_time_
Ogre::SceneNode * reference_node_