Skip to content

Commit e664f7f

Browse files
committed
rviz_plugin_tutorials: improved drive arrows in teleop panel
1 parent 1c86a6e commit e664f7f

File tree

1 file changed

+64
-72
lines changed

1 file changed

+64
-72
lines changed

rviz_plugin_tutorials/src/drive_widget.cpp

Lines changed: 64 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -70,89 +70,81 @@ void DriveWidget::paintEvent( QPaintEvent* event )
7070
{
7171
int w = width();
7272
int h = height();
73-
74-
float lin = h/2 * linear_velocity_ / linear_max_;
75-
float ang = angular_velocity_;
73+
int size = ( w > h ) ? h : w;
7674

7775
QPen arrow;
78-
int line_width = 2 + abs( int( lin / 5.0 ));
79-
arrow.setWidth( h/20 );
76+
arrow.setWidth( size/20 );
8077
arrow.setColor( Qt::green );
8178
arrow.setCapStyle( Qt::RoundCap );
79+
arrow.setJoinStyle( Qt::RoundJoin );
8280
painter.setPen( arrow );
8381

84-
int arrowhead_x, arrowhead_y;
85-
float arrowhead_ang;
82+
int step_count = 100;
83+
QPointF left_track[ step_count ];
84+
QPointF right_track[ step_count ];
85+
86+
float half_track_width = size/4.0;
87+
88+
float cx = w/2;
89+
float cy = h/2;
90+
left_track[ 0 ].setX( cx - half_track_width );
91+
left_track[ 0 ].setY( cy );
92+
right_track[ 0 ].setX( cx + half_track_width );
93+
right_track[ 0 ].setY( cy );
94+
float angle = M_PI/2;
95+
float delta_angle = angular_velocity_ / step_count;
96+
float step_dist = linear_velocity_ * size/2 / linear_max_ / step_count;
97+
for( int step = 1; step < step_count; step++ )
98+
{
99+
angle += delta_angle / 2;
100+
float next_cx = cx + step_dist * cosf( angle );
101+
float next_cy = cy - step_dist * sinf( angle );
102+
angle += delta_angle / 2;
103+
104+
left_track[ step ].setX( next_cx + half_track_width * cosf( angle + M_PI/2 ));
105+
left_track[ step ].setY( next_cy - half_track_width * sinf( angle + M_PI/2 ));
106+
right_track[ step ].setX( next_cx + half_track_width * cosf( angle - M_PI/2 ));
107+
right_track[ step ].setY( next_cy - half_track_width * sinf( angle - M_PI/2 ));
108+
109+
cx = next_cx;
110+
cy = next_cy;
111+
}
112+
painter.drawPolyline( left_track, step_count );
113+
painter.drawPolyline( right_track, step_count );
114+
115+
int left_arrow_dir = (-step_dist + half_track_width * delta_angle > 0);
116+
int right_arrow_dir = (-step_dist - half_track_width * delta_angle > 0);
117+
118+
arrow.setJoinStyle( Qt::MiterJoin );
119+
painter.setPen( arrow );
86120

87-
if( angular_velocity_ == 0 )
121+
float head_len = size / 8.0;
122+
QPointF arrow_head[ 3 ];
123+
float x, y;
124+
if( fabsf( -step_dist + half_track_width * delta_angle ) > .01 )
88125
{
89-
painter.drawLine( w/2, h/2, w/2, h/2 - lin );
90-
arrowhead_x = w/2;
91-
arrowhead_y = h/2 - lin;
92-
if( lin < 0 )
93-
{
94-
arrowhead_ang = M_PI;
95-
}
96-
else
97-
{
98-
arrowhead_ang = 0;
99-
}
126+
x = left_track[ step_count - 1 ].x();
127+
y = left_track[ step_count - 1 ].y();
128+
arrow_head[ 0 ].setX( x + head_len * cosf( angle + 3*M_PI/4 + left_arrow_dir * M_PI ));
129+
arrow_head[ 0 ].setY( y - head_len * sinf( angle + 3*M_PI/4 + left_arrow_dir * M_PI ));
130+
arrow_head[ 1 ].setX( x );
131+
arrow_head[ 1 ].setY( y );
132+
arrow_head[ 2 ].setX( x + head_len * cosf( angle - 3*M_PI/4 + left_arrow_dir * M_PI ));
133+
arrow_head[ 2 ].setY( y - head_len * sinf( angle - 3*M_PI/4 + left_arrow_dir * M_PI ));
134+
painter.drawPolyline( arrow_head, 3 );
100135
}
101-
else
136+
if( fabsf( -step_dist - half_track_width * delta_angle ) > .01 )
102137
{
103-
int radius = abs( int( lin / ang ));
104-
int arc_pi = 180 * 16;
105-
int start_ang;
106-
int span_ang = abs( int( ang*180.0/M_PI*16 ));
107-
int x;
108-
if( ang > 0 )
109-
{
110-
if( lin > 0 )
111-
{
112-
x = w/2 - 2*radius;
113-
start_ang = 0;
114-
arrowhead_ang = ang;
115-
arrowhead_x = x + radius + radius * cosf( ang );
116-
arrowhead_y = h/2 - radius * sinf( ang );
117-
}
118-
else
119-
{
120-
x = w/2;
121-
start_ang = arc_pi;
122-
arrowhead_ang = M_PI + ang;
123-
arrowhead_x = x + radius - radius * cosf( ang );
124-
arrowhead_y = h/2 + radius * sinf( ang );
125-
}
126-
}
127-
else
128-
{
129-
if( lin > 0 )
130-
{
131-
x = w/2;
132-
start_ang = arc_pi - span_ang;
133-
arrowhead_ang = ang;
134-
arrowhead_x = x + radius - radius * cosf( -ang );
135-
arrowhead_y = h/2 - radius * sinf( -ang );
136-
}
137-
else
138-
{
139-
x = w/2 - 2*radius;
140-
start_ang = -span_ang;
141-
arrowhead_ang = M_PI + ang;
142-
arrowhead_x = x + radius + radius * cosf( -ang );
143-
arrowhead_y = h/2 + radius * sinf( -ang );
144-
}
145-
}
146-
painter.drawArc( x, h/2 - radius, 2*radius, 2*radius, start_ang, span_ang );
138+
x = right_track[ step_count - 1 ].x();
139+
y = right_track[ step_count - 1 ].y();
140+
arrow_head[ 0 ].setX( x + head_len * cosf( angle + 3*M_PI/4 + right_arrow_dir * M_PI ));
141+
arrow_head[ 0 ].setY( y - head_len * sinf( angle + 3*M_PI/4 + right_arrow_dir * M_PI ));
142+
arrow_head[ 1 ].setX( x );
143+
arrow_head[ 1 ].setY( y );
144+
arrow_head[ 2 ].setX( x + head_len * cosf( angle - 3*M_PI/4 + right_arrow_dir * M_PI ));
145+
arrow_head[ 2 ].setY( y - head_len * sinf( angle - 3*M_PI/4 + right_arrow_dir * M_PI ));
146+
painter.drawPolyline( arrow_head, 3 );
147147
}
148-
float tip_spread = M_PI/3;
149-
float head_l_ang = arrowhead_ang + M_PI * 1.5 - tip_spread / 2;
150-
float head_r_ang = arrowhead_ang + M_PI * 1.5 + tip_spread / 2;
151-
float head_size = line_width * 2;
152-
painter.drawLine( arrowhead_x + head_size * cosf( head_l_ang ), arrowhead_y - head_size * sinf( head_l_ang ),
153-
arrowhead_x, arrowhead_y );
154-
painter.drawLine( arrowhead_x, arrowhead_y,
155-
arrowhead_x + head_size * cosf( head_r_ang ), arrowhead_y - head_size * sinf( head_r_ang ));
156148
}
157149
}
158150

0 commit comments

Comments
 (0)