Add scale and color properties to Marker selection

src/rviz/default_plugin/marker_utils.cpp

@@ -84,6 +84,38 @@ MarkerBase* createMarker(int marker_type, MarkerDisplay* owner, DisplayContext*
   }
 }
 
+QString getMarkerTypeName(unsigned int type)
+{
+  switch (type) {
+  case visualization_msgs::Marker::ARROW:
+    return "Arrow";
+  case visualization_msgs::Marker::CUBE:
+    return "Cube";
+  case visualization_msgs::Marker::CUBE_LIST:
+    return "Cube List";
+  case visualization_msgs::Marker::TRIANGLE_LIST:
+    return "Triangle List";
+  case visualization_msgs::Marker::SPHERE:
+    return "Sphere";
+  case visualization_msgs::Marker::SPHERE_LIST:
+    return "Sphere List";
+  case visualization_msgs::Marker::CYLINDER:
+    return "Cylinder";
+  case visualization_msgs::Marker::LINE_STRIP:
+    return "Line Strip";
+  case visualization_msgs::Marker::LINE_LIST:
+    return "Line List";
+  case visualization_msgs::Marker::POINTS:
+    return "Points";
+  case visualization_msgs::Marker::TEXT_VIEW_FACING:
+    return "Text View Facing";
+  case visualization_msgs::Marker::MESH_RESOURCE:
+    return "Mesh";
+  default:
+    return "Unknown";
+  }
+}
+
 namespace
 {
 void addSeparatorIfRequired(std::stringstream& ss)

src/rviz/default_plugin/marker_utils.h

@@ -33,6 +33,7 @@
 
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
+#include <QString>
 
 namespace Ogre
 {
@@ -49,6 +50,9 @@ class MarkerBase;
 /** Create a marker of given type as declared in visualization_messages::Marker */
 MarkerBase* createMarker(int marker_type, MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node);
 
+/** Map marker type ID onto human-readable name */
+QString getMarkerTypeName(unsigned int type);
+
 /** Check for correctness of the marker description, issue a warning/error status in the owner if not
  *  Return false if there is a severe error meaning that this marker should be dropped */
 bool checkMarkerMsg(const visualization_msgs::Marker& marker, MarkerDisplay* owner);

src/rviz/default_plugin/markers/marker_selection_handler.cpp

@@ -33,9 +33,11 @@
 #include "rviz/default_plugin/interactive_markers/interactive_marker_control.h"
 #include "rviz/default_plugin/marker_display.h"
 #include "rviz/default_plugin/markers/marker_base.h"
+#include "rviz/default_plugin/marker_utils.h"
 #include "rviz/properties/property.h"
 #include "rviz/properties/quaternion_property.h"
 #include "rviz/properties/vector_property.h"
+#include "rviz/properties/color_property.h"
 
 #include "rviz/default_plugin/markers/marker_selection_handler.h"
 
@@ -68,9 +70,64 @@ Ogre::Quaternion MarkerSelectionHandler::getOrientation()
                            marker_->getMessage()->pose.orientation.z );
 }
 
+Ogre::Vector3 MarkerSelectionHandler::getScale()
+{
+  return Ogre::Vector3( marker_->getMessage()->scale.x,
+                        marker_->getMessage()->scale.y,
+                        marker_->getMessage()->scale.z );
+}
+
+QColor MarkerSelectionHandler::getColor()
+{
+  return QColor( (int)(marker_->getMessage()->color.r * 255),
+                 (int)(marker_->getMessage()->color.g * 255),
+                 (int)(marker_->getMessage()->color.b * 255),
+                 (int)(marker_->getMessage()->color.a * 255) );
+}
+
+namespace {
+
+VectorProperty* createScaleProperty(const visualization_msgs::Marker& marker, const Ogre::Vector3 scale, Property* parent_property)
+{
+  VectorProperty* p = new VectorProperty("Scale", scale, "", parent_property);
+  Property *x = p->childAt(0);
+  Property *y = p->childAt(1);
+  Property *z = p->childAt(2);
+  // Change scale dimension titles
+  switch (marker.type) {
+    case visualization_msgs::Marker::ARROW:
+      x->setName("Length");
+      y->setName("Width");
+      z->setName("Height");
+      break;
+    case visualization_msgs::Marker::CYLINDER:
+      z->setName("Height");
+      break;
+    case visualization_msgs::Marker::LINE_STRIP:
+    case visualization_msgs::Marker::LINE_LIST:
+      x->setName("Thickness");
+      y->hide();
+      z->hide();
+      break;
+    case visualization_msgs::Marker::POINTS:
+      x->setName("Width");
+      y->setName("Height");
+      z->hide();
+      break;
+    case visualization_msgs::Marker::TEXT_VIEW_FACING:
+      x->hide();
+      y->hide();
+      z->setName("Size");
+      break;
+  }
+  return p;
+}
+
+}
+
 void MarkerSelectionHandler::createProperties( const Picked& obj, Property* parent_property )
 {
-  Property* group = new Property( "Marker " + marker_id_, QVariant(), "", parent_property );
+  Property* group = new Property( "Marker " + marker_id_, getMarkerTypeName(marker_->getMessage()->type), "", parent_property );
   properties_.push_back( group );
 
   position_property_ = new VectorProperty( "Position", getPosition(), "", group );
@@ -79,13 +136,21 @@ void MarkerSelectionHandler::createProperties( const Picked& obj, Property* pare
   orientation_property_ = new QuaternionProperty( "Orientation", getOrientation(), "", group );
   orientation_property_->setReadOnly( true );
 
+  scale_property_ = createScaleProperty( *marker_->getMessage(), getScale(), group );
+  scale_property_->setReadOnly( true );
+
+  color_property_ = new ColorProperty( "Color", getColor(), "", group );
+  color_property_->setReadOnly( true );
+
   group->expand();
 }
 
 void MarkerSelectionHandler::updateProperties()
 {
   position_property_->setVector( getPosition() );
   orientation_property_->setQuaternion( getOrientation() );
+  scale_property_->setVector( getScale() );
+  color_property_->setColor( getColor() );
 }
 
 } // end namespace rviz

src/rviz/default_plugin/markers/marker_selection_handler.h

@@ -39,6 +39,7 @@ class InteractiveMarkerControl;
 class MarkerBase;
 class QuaternionProperty;
 class VectorProperty;
+class ColorProperty;
 typedef std::pair<std::string, int32_t> MarkerID;
 
 class MarkerSelectionHandler: public SelectionHandler
@@ -49,6 +50,8 @@ class MarkerSelectionHandler: public SelectionHandler
 
   Ogre::Vector3 getPosition();
   Ogre::Quaternion getOrientation();
+  Ogre::Vector3 getScale();
+  QColor getColor();
 
   virtual void createProperties( const Picked& obj, Property* parent_property );
   virtual void updateProperties();
@@ -58,6 +61,8 @@ class MarkerSelectionHandler: public SelectionHandler
   QString marker_id_;
   VectorProperty* position_property_;
   QuaternionProperty* orientation_property_;
+  VectorProperty* scale_property_;
+  ColorProperty* color_property_;
 };
 
 } // end namespace rviz