frame_marker.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Mario Prats */
00036 
00037 #include <frame_marker.h>
00038 #include <moveit/robot_interaction/interactive_marker_helpers.h>
00039 
00040 #include <interactive_markers/tools.h>
00041 
00042 #include <eigen_conversions/eigen_msg.h>
00043 
00044 #include <boost/math/constants/constants.hpp>
00045 
00046 namespace benchmark_tool
00047 {
00048 const float GripperMarker::GOAL_NOT_TESTED_COLOR[4] = { 0.75, 0.75, 0.75, 1.0 };
00049 const float GripperMarker::GOAL_PROCESSING_COLOR[4] = { 0.9, 0.9, 0.9, 1.0 };
00050 const float GripperMarker::GOAL_NOT_REACHABLE_COLOR[4] = { 1.0, 0.0, 0.0, 1.0 };
00051 const float GripperMarker::GOAL_REACHABLE_COLOR[4] = { 0.0, 1.0, 0.0, 1.0 };
00052 const float GripperMarker::GOAL_COLLISION_COLOR[4] = { 1.0, 1.0, 0.0, 1.0 };
00053 
00054 FrameMarker::FrameMarker(Ogre::SceneNode* parent_node, rviz::DisplayContext* context, const std::string& name,
00055                          const std::string& frame_id, const geometry_msgs::Pose& pose, double scale,
00056                          const std_msgs::ColorRGBA& color, bool is_selected, bool visible_x, bool visible_y,
00057                          bool visible_z)
00058   : parent_node_(parent_node)
00059   , context_(context)
00060   , selected_(is_selected)
00061   , visible_x_(visible_x)
00062   , visible_y_(visible_y)
00063   , visible_z_(visible_z)
00064   , color_(color)
00065   , receiver_(NULL)
00066   , receiver_method_(NULL)
00067 {
00068   buildFrom(name, frame_id, pose, scale, color);
00069 }
00070 
00071 FrameMarker::FrameMarker(Ogre::SceneNode* parent_node, rviz::DisplayContext* context, const std::string& name,
00072                          const std::string& frame_id, const geometry_msgs::Pose& pose, double scale,
00073                          const float color[4], bool is_selected, bool visible_x, bool visible_y, bool visible_z)
00074   : parent_node_(parent_node)
00075   , context_(context)
00076   , selected_(is_selected)
00077   , visible_x_(visible_x)
00078   , visible_y_(visible_y)
00079   , visible_z_(visible_z)
00080   , receiver_(NULL)
00081   , receiver_method_(NULL)
00082 {
00083   color_.r = color[0];
00084   color_.g = color[1];
00085   color_.b = color[2];
00086   color_.a = color[3];
00087   buildFrom(name, frame_id, pose, scale, color_);
00088 }
00089 
00090 void FrameMarker::hide(void)
00091 {
00092   if (isVisible())
00093   {
00094     position_ = imarker->getPosition();
00095     orientation_ = imarker->getOrientation();
00096     imarker.reset();
00097   }
00098 }
00099 
00100 void FrameMarker::show(Ogre::SceneNode* scene_node, rviz::DisplayContext* context)
00101 {
00102   if (!isVisible())
00103   {
00104     rebuild();
00105   }
00106 }
00107 
00108 void FrameMarker::showDescription(const std::string& description)
00109 {
00110   imarker_msg.description = description;
00111 
00112   if (isVisible())
00113   {
00114     Ogre::Vector3 position = imarker->getPosition();
00115     Ogre::Quaternion orientation = imarker->getOrientation();
00116     updateMarker();
00117     imarker->setPose(position, orientation, "");
00118   }
00119   imarker->setShowDescription(true);
00120 }
00121 
00122 void FrameMarker::hideDescription()
00123 {
00124   imarker->setShowDescription(false);
00125 }
00126 
00127 void FrameMarker::getPosition(geometry_msgs::Point& position)
00128 {
00129   if (imarker)
00130   {
00131     position.x = imarker->getPosition().x;
00132     position.y = imarker->getPosition().y;
00133     position.z = imarker->getPosition().z;
00134   }
00135   else
00136   {
00137     position.x = position_.x;
00138     position.y = position_.y;
00139     position.z = position_.z;
00140   }
00141 }
00142 
00143 void FrameMarker::getOrientation(geometry_msgs::Quaternion& orientation)
00144 {
00145   if (imarker)
00146   {
00147     orientation.x = imarker->getOrientation().x;
00148     orientation.y = imarker->getOrientation().y;
00149     orientation.z = imarker->getOrientation().z;
00150     orientation.w = imarker->getOrientation().w;
00151   }
00152   else
00153   {
00154     orientation.x = orientation_.x;
00155     orientation.y = orientation_.y;
00156     orientation.z = orientation_.z;
00157     orientation.w = orientation_.w;
00158   }
00159 }
00160 
00161 void FrameMarker::getPose(Eigen::Affine3d& pose)
00162 {
00163   pose = Eigen::Affine3d(Eigen::Quaterniond(imarker->getOrientation().w, imarker->getOrientation().x,
00164                                             imarker->getOrientation().y, imarker->getOrientation().z));
00165   pose.translation() = Eigen::Vector3d(imarker->getPosition().x, imarker->getPosition().y, imarker->getPosition().z);
00166 }
00167 
00168 void FrameMarker::setPose(Eigen::Affine3d& pose)
00169 {
00170   Eigen::Quaterniond q(pose.rotation());
00171   imarker->setPose(Ogre::Vector3(pose.translation().x(), pose.translation().y(), pose.translation().z()),
00172                    Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()), "");
00173 }
00174 
00175 void FrameMarker::setColor(float r, float g, float b, float a)
00176 {
00177   // Update marker color
00178   color_.r = r;
00179   color_.g = g;
00180   color_.b = b;
00181   color_.a = a;
00182 
00183   for (unsigned int c = 0; c < imarker_msg.controls.size(); ++c)
00184   {
00185     for (unsigned int m = 0; m < imarker_msg.controls[c].markers.size(); ++m)
00186     {
00187       if (imarker_msg.controls[c].markers[m].type == visualization_msgs::Marker::MESH_RESOURCE ||
00188           imarker_msg.controls[c].markers[m].type == visualization_msgs::Marker::SPHERE)
00189       {
00190         imarker_msg.controls[c].markers[m].color.r = r;
00191         imarker_msg.controls[c].markers[m].color.g = g;
00192         imarker_msg.controls[c].markers[m].color.b = b;
00193         imarker_msg.controls[c].markers[m].color.a = a;
00194       }
00195     }
00196   }
00197 
00198   if (isVisible())
00199   {
00200     Ogre::Vector3 position = imarker->getPosition();
00201     Ogre::Quaternion orientation = imarker->getOrientation();
00202     updateMarker();
00203     imarker->setPose(position, orientation, "");
00204   }
00205 }
00206 
00207 void FrameMarker::select(void)
00208 {
00209   selected_ = true;
00210   rebuild();
00211 }
00212 
00213 void FrameMarker::unselect(void)
00214 {
00215   selected_ = false;
00216   rebuild();
00217 }
00218 
00219 void FrameMarker::rebuild()
00220 {
00221   geometry_msgs::Pose pose;
00222   getPosition(pose.position);
00223   getOrientation(pose.orientation);
00224   buildFrom(imarker_msg.name, imarker_msg.header.frame_id, pose, imarker_msg.scale, color_);
00225 }
00226 
00227 void FrameMarker::buildFrom(const std::string& name, const std::string& frame_id, const geometry_msgs::Pose& pose,
00228                             double scale, const std_msgs::ColorRGBA& color)
00229 {
00230   color_ = color;
00231 
00232   visualization_msgs::InteractiveMarker int_marker;
00233   if (isSelected())
00234   {
00235     geometry_msgs::PoseStamped pose_stamped;
00236     pose_stamped.header.frame_id = frame_id;
00237     pose_stamped.pose = pose;
00238     int_marker = robot_interaction::make6DOFMarker(name, pose_stamped, scale);
00239   }
00240   else
00241   {
00242     int_marker.scale = scale;
00243     int_marker.name = name;
00244     int_marker.pose = pose;
00245   }
00246   int_marker.header.frame_id = frame_id;
00247   int_marker.header.stamp = ros::Time::now();
00248 
00249   int_marker.menu_entries = menu_entries_;
00250 
00251   visualization_msgs::InteractiveMarkerControl m_control;
00252   m_control.always_visible = true;
00253   m_control.interaction_mode = m_control.BUTTON;
00254 
00255   // Display a frame marker with an sphere in the origin
00256   visualization_msgs::Marker m;
00257   m.type = visualization_msgs::Marker::SPHERE;
00258   m.scale.x = 0.1 * scale;
00259   m.scale.y = 0.1 * scale;
00260   m.scale.z = 0.1 * scale;
00261   m.ns = "goal_pose_marker";
00262   m.action = visualization_msgs::Marker::ADD;
00263   m.color = color;
00264   m_control.markers.push_back(m);
00265 
00266   m.type = visualization_msgs::Marker::ARROW;
00267   m.scale.x = 0.3 * scale;
00268   m.scale.y = 0.1 * m.scale.x;
00269   m.scale.z = 0.1 * m.scale.x;
00270   m.ns = "goal_pose_marker";
00271   m.action = visualization_msgs::Marker::ADD;
00272 
00273   // X axis
00274   if (visible_x_)
00275   {
00276     m.color.r = 1.0f;
00277     m.color.g = 0.0f;
00278     m.color.b = 0.0f;
00279     m.color.a = 1.0f;
00280     m_control.markers.push_back(m);
00281   }
00282 
00283   // Y axis
00284   if (visible_y_)
00285   {
00286     tf::Quaternion imq;
00287     imq = tf::createQuaternionFromRPY(0, 0, boost::math::constants::pi<double>() / 2.0);
00288     tf::quaternionTFToMsg(imq, m.pose.orientation);
00289     m.color.r = 0.0f;
00290     m.color.g = 1.0f;
00291     m.color.b = 0.0f;
00292     m.color.a = 1.0f;
00293     m_control.markers.push_back(m);
00294   }
00295 
00296   // Z axis
00297   if (visible_z_)
00298   {
00299     tf::Quaternion imq;
00300     imq = tf::createQuaternionFromRPY(0, -boost::math::constants::pi<double>() / 2.0, 0);
00301     tf::quaternionTFToMsg(imq, m.pose.orientation);
00302     m.color.r = 0.0f;
00303     m.color.g = 0.0f;
00304     m.color.b = 1.0f;
00305     m.color.a = 1.0f;
00306     m_control.markers.push_back(m);
00307   }
00308   int_marker.controls.push_back(m_control);
00309 
00310   imarker.reset(new rviz::InteractiveMarker(parent_node_, context_));
00311   interactive_markers::autoComplete(int_marker);
00312   imarker->processMessage(int_marker);
00313   imarker->setShowVisualAids(true);
00314   imarker->setShowAxes(false);
00315   imarker->setShowDescription(false);
00316 
00317   // reconnect if it was previously connected
00318   if (receiver_ && receiver_method_)
00319   {
00320     connect(receiver_, receiver_method_);
00321   }
00322 
00323   imarker_msg = int_marker;
00324 }
00325 
00326 GripperMarker::GripperMarker(const robot_state::RobotState& robot_state, Ogre::SceneNode* parent_node,
00327                              rviz::DisplayContext* context, const std::string& name, const std::string& frame_id,
00328                              const robot_interaction::RobotInteraction::EndEffector& eef,
00329                              const geometry_msgs::Pose& pose, double scale, const GripperMarkerState& state,
00330                              bool is_selected, bool visible_x, bool visible_y, bool visible_z)
00331   : FrameMarker(parent_node, context, name, frame_id, pose, scale, stateToColor(state), is_selected, visible_x,
00332                 visible_y, visible_z)
00333   , eef_(eef)
00334   , display_gripper_mesh_(false)
00335   , state_(state)
00336 {
00337   robot_state_ = &robot_state;
00338 }
00339 
00340 void GripperMarker::select(bool display_gripper_mesh)
00341 {
00342   display_gripper_mesh_ = display_gripper_mesh;
00343   FrameMarker::select();
00344 }
00345 
00346 void GripperMarker::unselect(bool display_gripper_mesh)
00347 {
00348   display_gripper_mesh_ = display_gripper_mesh;
00349   FrameMarker::unselect();
00350 }
00351 
00352 void GripperMarker::buildFrom(const std::string& name, const std::string& frame_id, const geometry_msgs::Pose& pose,
00353                               double scale, const std_msgs::ColorRGBA& color)
00354 {
00355   color_ = color;
00356 
00357   visualization_msgs::InteractiveMarker int_marker;
00358   if (isSelected())
00359   {
00360     geometry_msgs::PoseStamped pose_stamped;
00361     pose_stamped.pose = pose;
00362     int_marker = robot_interaction::make6DOFMarker(name, pose_stamped, scale);
00363   }
00364   else
00365   {
00366     int_marker.scale = scale;
00367     int_marker.name = name;
00368     int_marker.pose = pose;
00369   }
00370   int_marker.header.frame_id = frame_id;
00371   int_marker.header.stamp = ros::Time::now();
00372 
00373   int_marker.menu_entries = menu_entries_;
00374 
00375   visualization_msgs::InteractiveMarkerControl m_control;
00376   m_control.always_visible = true;
00377   m_control.interaction_mode = m_control.BUTTON;
00378 
00379   if (display_gripper_mesh_)
00380   {
00381     // If selected and gripper_mesh enabled, display the actual end effector mesh
00382     const robot_state::JointModelGroup* joint_model_group = robot_state_->getJointModelGroup(eef_.eef_group);
00383     const std::vector<std::string>& link_names = joint_model_group->getLinkModelNames();
00384 
00385     std_msgs::ColorRGBA marker_color;
00386     marker_color = color;
00387     visualization_msgs::MarkerArray marker_array;
00388     robot_state_->getRobotMarkers(marker_array, link_names, marker_color, "goal_pose_marker", ros::Duration());
00389 
00390     for (std::size_t i = 0; i < marker_array.markers.size(); ++i)
00391     {
00392       marker_array.markers[i].header = int_marker.header;
00393       m_control.markers.push_back(marker_array.markers[i]);
00394     }
00395 
00396     Eigen::Affine3d tip_pose = robot_state_->getGlobalLinkTransform(eef_.parent_link);
00397     tf::poseEigenToMsg(tip_pose, int_marker.pose);
00398   }
00399   else
00400   {
00401     // Display a frame marker with an sphere in the origin
00402     visualization_msgs::Marker m;
00403     m.type = visualization_msgs::Marker::SPHERE;
00404     m.scale.x = 0.1 * scale;
00405     m.scale.y = 0.1 * scale;
00406     m.scale.z = 0.1 * scale;
00407     m.ns = "goal_pose_marker";
00408     m.action = visualization_msgs::Marker::ADD;
00409     m.color = color;
00410     m_control.markers.push_back(m);
00411 
00412     m.type = visualization_msgs::Marker::ARROW;
00413     m.scale.x = 0.3 * scale;
00414     m.scale.y = 0.1 * m.scale.x;
00415     m.scale.z = 0.1 * m.scale.x;
00416     m.ns = "goal_pose_marker";
00417     m.action = visualization_msgs::Marker::ADD;
00418 
00419     // X axis
00420     if (visible_x_)
00421     {
00422       m.color.r = 1.0f;
00423       m.color.g = 0.0f;
00424       m.color.b = 0.0f;
00425       m.color.a = 1.0f;
00426       m_control.markers.push_back(m);
00427     }
00428 
00429     // Y axis
00430     if (visible_y_)
00431     {
00432       tf::Quaternion imq;
00433       imq = tf::createQuaternionFromRPY(0, 0, boost::math::constants::pi<double>() / 2.0);
00434       tf::quaternionTFToMsg(imq, m.pose.orientation);
00435       m.color.r = 0.0f;
00436       m.color.g = 1.0f;
00437       m.color.b = 0.0f;
00438       m.color.a = 1.0f;
00439       m_control.markers.push_back(m);
00440     }
00441 
00442     // Z axis
00443     if (visible_z_)
00444     {
00445       tf::Quaternion imq;
00446       imq = tf::createQuaternionFromRPY(0, -boost::math::constants::pi<double>() / 2.0, 0);
00447       tf::quaternionTFToMsg(imq, m.pose.orientation);
00448       m.color.r = 0.0f;
00449       m.color.g = 0.0f;
00450       m.color.b = 1.0f;
00451       m.color.a = 1.0f;
00452       m_control.markers.push_back(m);
00453     }
00454   }
00455 
00456   int_marker.controls.push_back(m_control);
00457 
00458   imarker.reset(new rviz::InteractiveMarker(parent_node_, context_));
00459   interactive_markers::autoComplete(int_marker);
00460   imarker->processMessage(int_marker);
00461   imarker->setShowAxes(false);
00462   imarker->setShowVisualAids(true);
00463   imarker->setShowDescription(false);
00464   imarker->setPose(Ogre::Vector3(pose.position.x, pose.position.y, pose.position.z),
00465                    Ogre::Quaternion(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z),
00466                    "");
00467 
00468   // reconnect if it was previously connected
00469   if (receiver_ && receiver_method_)
00470   {
00471     connect(receiver_, receiver_method_);
00472   }
00473 
00474   imarker_msg = int_marker;
00475 }
00476 
00477 const float* GripperMarker::stateToColor(const GripperMarkerState& state)
00478 {
00479   const float* color = 0;
00480   if (state == NOT_TESTED)
00481     color = GOAL_NOT_TESTED_COLOR;
00482   else if (state == PROCESSING)
00483     color = GOAL_PROCESSING_COLOR;
00484   else if (state == REACHABLE)
00485     color = GOAL_REACHABLE_COLOR;
00486   else if (state == NOT_REACHABLE)
00487     color = GOAL_NOT_REACHABLE_COLOR;
00488   else if (state == IN_COLLISION)
00489     color = GOAL_COLLISION_COLOR;
00490   else
00491     color = GOAL_NOT_TESTED_COLOR;
00492 
00493   return color;
00494 }
00495 }


benchmarks_gui
Author(s): Mario Prats
autogenerated on Mon Jul 24 2017 02:22:21