rviz_plugin_tutorials: drive_widget documentation

Author: hershwg

rviz_plugin_tutorials/src/drive_widget.cpp

@@ -42,13 +42,18 @@ DriveWidget::DriveWidget( QWidget* parent )
   : QWidget( parent )
   , linear_velocity_( 0 )
   , angular_velocity_( 0 )
-  , linear_max_( 10 )
-  , angular_max_( 2 )
+  , linear_scale_( 10 )
+  , angular_scale_( 2 )
 {
 }
 
+// This paintEvent() is complex because of the drawing of the two
+// wheel arc-arrows.
 void DriveWidget::paintEvent( QPaintEvent* event )
 {
+  // The background color and crosshair lines are drawn differently
+  // depending on whether this widget is enabled or not.  This gives a
+  // nice visual indication of whether the control is "live".
   QColor background;
   QColor crosshair;
   if( isEnabled() )
@@ -61,6 +66,10 @@ void DriveWidget::paintEvent( QPaintEvent* event )
     background = Qt::lightGray;
     crosshair = Qt::darkGray;
   }
+
+  // The main visual is a square, centered in the widget's area.  Here
+  // we compute the size of the square and the horizontal and vertical
+  // offsets of it.
   int w = width();
   int h = height();
   int size = (( w > h ) ? h : w) - 1;
@@ -70,10 +79,17 @@ void DriveWidget::paintEvent( QPaintEvent* event )
   QPainter painter( this );
   painter.setBrush( background );
   painter.setPen( crosshair );
+
+  // Draw the background square.
   painter.drawRect( QRect( hpad, vpad, size, size ));
+
+  // Draw a cross-hair inside the square.
   painter.drawLine( hpad, height() / 2, hpad + size, height() / 2 );
   painter.drawLine( width() / 2, vpad, width() / 2, vpad + size );
 
+  // If the widget is enabled and the velocities are not zero, draw
+  // some sweet green arrows showing possible paths that the wheels of
+  // a diff-drive robot would take if it stayed at these velocities.
   if( isEnabled() && (angular_velocity_ != 0 || linear_velocity_ != 0 ))
   {
     QPen arrow;
@@ -83,6 +99,10 @@ void DriveWidget::paintEvent( QPaintEvent* event )
     arrow.setJoinStyle( Qt::RoundJoin );
     painter.setPen( arrow );
 
+    // This code steps along a central arc defined by the linear and
+    // angular velocites.  At each step, it computes where the left
+    // and right wheels would be and collecting the resulting points
+    // in the left_track and right_track arrays.
     int step_count = 100;
     QPointF left_track[ step_count ];
     QPointF right_track[ step_count ];
@@ -97,7 +117,7 @@ void DriveWidget::paintEvent( QPaintEvent* event )
     right_track[ 0 ].setY( cy );
     float angle = M_PI/2;
     float delta_angle = angular_velocity_ / step_count;
-    float step_dist = linear_velocity_ * size/2 / linear_max_ / step_count;
+    float step_dist = linear_velocity_ * size/2 / linear_scale_ / step_count;
     for( int step = 1; step < step_count; step++ )
     {
       angle += delta_angle / 2;
@@ -113,15 +133,23 @@ void DriveWidget::paintEvent( QPaintEvent* event )
       cx = next_cx;
       cy = next_cy;
     }
+    // Now the track arrays are filled, so stroke each with a fat green line.
     painter.drawPolyline( left_track, step_count );
     painter.drawPolyline( right_track, step_count );
 
+    // Here we decide which direction each arrowhead will point
+    // (forward or backward).  This works by comparing the arc length
+    // travelled by the center in one step (step_dist) with the arc
+    // length travelled by the wheel (half_track_width * delta_angle).
     int left_arrow_dir = (-step_dist + half_track_width * delta_angle > 0);
     int right_arrow_dir = (-step_dist - half_track_width * delta_angle > 0);
 
+    // Use MiterJoin for the arrowheads so we get a nice sharp point.
     arrow.setJoinStyle( Qt::MiterJoin );
     painter.setPen( arrow );
 
+    // Compute and draw polylines for each arrowhead.  This code could
+    // probably be more elegant.
     float head_len = size / 8.0;
     QPointF arrow_head[ 3 ];
     float x, y;
@@ -152,6 +180,9 @@ void DriveWidget::paintEvent( QPaintEvent* event )
   }
 }
 
+// Every mouse move event received here sends a velocity because Qt
+// only sends us mouse move events if there was previously a
+// mouse-down event while in the widget.
 void DriveWidget::mouseMoveEvent( QMouseEvent* event )
 {
   sendVelocitiesFromMouse( event->x(), event->y(), width(), height() );
@@ -162,19 +193,27 @@ void DriveWidget::mousePressEvent( QMouseEvent* event )
   sendVelocitiesFromMouse( event->x(), event->y(), width(), height() );
 }
 
+// When the mouse leaves the widget but the button is still held down,
+// we don't get the leaveEvent().  However, when the mouse drags out
+// of the widget and then other buttons are pressed (or possibly other
+// window-manager things happen), we will get a leaveEvent() but not a
+// mouseReleaseEvent().  Without this you can have a robot stuck "on"
+// without the user controlling it.
 void DriveWidget::leaveEvent( QEvent* event )
 {
   stop();
 }
 
+// Compute and emit linear and angular velocities based on Y and X
+// mouse positions relative to the central square.
 void DriveWidget::sendVelocitiesFromMouse( int x, int y, int width, int height )
 {  
   int size = (( width > height ) ? height : width );
   int hpad = ( width - size ) / 2;
   int vpad = ( height - size ) / 2;
 
-  linear_velocity_ = (1.0 - float( y - vpad ) / float( size / 2 )) * linear_max_;
-  angular_velocity_ = (1.0 - float( x - hpad ) / float( size / 2 )) * angular_max_;
+  linear_velocity_ = (1.0 - float( y - vpad ) / float( size / 2 )) * linear_scale_;
+  angular_velocity_ = (1.0 - float( x - hpad ) / float( size / 2 )) * angular_scale_;
   update();
   Q_EMIT outputVelocity( linear_velocity_, angular_velocity_ );
 }

rviz_plugin_tutorials/src/drive_widget.h

@@ -34,31 +34,46 @@
 namespace rviz_plugin_tutorials
 {
 
+// DriveWidget implements a control which translates mouse Y values
+// into linear velocities and mouse X values into angular velocities.
 class DriveWidget: public QWidget
 {
 Q_OBJECT
 public:
+  // This class is not instantiated by pluginlib::ClassLoader, so the
+  // constructor has no restrictions.
   DriveWidget( QWidget* parent = 0 );
 
+  // We override QWidget::paintEvent() to do custom painting.
   virtual void paintEvent( QPaintEvent* event );
+
+  // We override the mouse events and leaveEvent() to keep track of
+  // what the mouse is doing.
   virtual void mouseMoveEvent( QMouseEvent* event );
   virtual void mousePressEvent( QMouseEvent* event );
   virtual void mouseReleaseEvent( QMouseEvent* event );
   virtual void leaveEvent( QEvent* event );
 
+  // Override sizeHint() to give the layout managers some idea of a
+  // good size for this.
   virtual QSize sizeHint() const { return QSize( 150, 150 ); }
 
 Q_SIGNALS:
+  // We emit outputVelocity() whenever it changes.
   void outputVelocity( float linear, float angular );
 
 protected:
+  // mouseMoveEvent() and mousePressEvent() need the same math to
+  // figure the velocities, so I put that in here.
   void sendVelocitiesFromMouse( int x, int y, int width, int height );
+
+  // Emit zero velocity.
   void stop();
 
   float linear_velocity_; // In m/s
   float angular_velocity_; // In radians/s
-  float linear_max_;
-  float angular_max_;
+  float linear_scale_; // In m/s
+  float angular_scale_; // In radians/s
 };
 
 } // end namespace rviz_plugin_tutorials