// Update a actarray device void actarray_update(actarray_t *actarray) { int ii; // Update the device subscription if (rtk_menuitem_ischecked(actarray->subscribe_item)) { if (!actarray->proxy->info.subscribed) { if (playerc_actarray_subscribe(actarray->proxy, PLAYER_OPEN_MODE) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); playerc_actarray_get_geom(actarray->proxy); } } else { if (actarray->proxy->info.subscribed) if (playerc_actarray_unsubscribe(actarray->proxy) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); } rtk_menuitem_check(actarray->subscribe_item, actarray->proxy->info.subscribed); // Draw in the actarray scan if it has been changed. if (actarray->proxy->info.subscribed) { if (actarray->proxy->info.datatime != actarray->datatime) actarray_draw(actarray); actarray->datatime = actarray->proxy->info.datatime; } else { for (ii = 0; ii < actarray->fig_count; ++ii) { rtk_fig_show(actarray->actuator_fig[ii], 0); } } // Move the actarray if (actarray->proxy->info.subscribed && rtk_menuitem_ischecked(actarray->command_item)) { for (ii = 0; ii < actarray->fig_count; ++ii) { rtk_fig_show(actarray->actuator_fig_cmd[ii], 1); } actarray_move(actarray); } else { for (ii = 0; ii < actarray->fig_count; ++ii) { rtk_fig_show(actarray->actuator_fig_cmd[ii], 0); } } }
// Dont draw the ir scan void ir_nodraw(ir_t *ir) { int i; for (i = 0; i < ir->proxy->data.ranges_count; i++) rtk_fig_show(ir->scan_fig[i], 0); }
// Draw the ir scan void ir_draw(ir_t *ir) { int i; double dr, da; double points[3][2]; for (i = 0; i < ir->proxy->data.ranges_count; i++) { rtk_fig_show(ir->scan_fig[i], 1); rtk_fig_clear(ir->scan_fig[i]); // Draw in the ir itself rtk_fig_color_rgb32(ir->scan_fig[i], COLOR_IR); rtk_fig_rectangle(ir->scan_fig[i], 0, 0, 0, 0.01, 0.05, 0); // Draw in the range scan rtk_fig_color_rgb32(ir->scan_fig[i], COLOR_IR_SCAN); dr = ((double)ir->proxy->data.ranges[i]); da = 20 * M_PI / 180 / 2; points[0][0] = 0; points[0][1] = 0; points[1][0] = dr * cos(-da); points[1][1] = dr * sin(-da); points[2][0] = dr * cos(+da); points[2][1] = dr * sin(+da); rtk_fig_polygon(ir->scan_fig[i], 0, 0, 0, 3, points, 1); } }
// Draw the actarray scan void actarray_draw(actarray_t *actarray) { double value; double min, max; double ax, ay, bx, by; double fx, fd; int ii; actarray_allocate(actarray, actarray->proxy->actuators_count); for(ii = 0; ii < actarray->proxy->actuators_count; ++ii) { value = actarray->proxy->actuators_data[ii].position; min = -1; max = 1; if (actarray->proxy->actuators_geom && actarray->proxy->actuators_geom_count == actarray->proxy->actuators_count) { min = actarray->proxy->actuators_geom[ii].min; max = actarray->proxy->actuators_geom[ii].max; } // now limit and scale the value to the actuator bar if (value > max) value = max; if (value < min) value = min; value = 2*(value-min)/(max-min) -1; rtk_fig_t * fig = actarray->actuator_fig[ii]; rtk_fig_show(fig, 1); rtk_fig_clear(fig); rtk_fig_origin(actarray->actuator_fig[ii], ARRAY_SPACING*ii +ARRAY_X_OFFSET, 0, 0); rtk_fig_color_rgb32(fig, COLOR_ACTARRAY_DATA); rtk_fig_line(fig, 0, -1, 0, 1); rtk_fig_ellipse(actarray->actuator_fig[ii], 0, value, 0, 0.2, 0.2, 1); } }
// Draw the dio scan void dio_draw(dio_t *dio) { int i; char ntext[64], str[1024]; uint32_t digin = dio->proxy->digin; uint32_t count = dio->proxy->count; rtk_fig_show(dio->fig, 1); rtk_fig_clear(dio->fig); str[0] = '\0'; if (count > 0) { for (i = count-1; i >= 0 ; i--) { snprintf(str, sizeof(str), "%s%i", str, (digin & (1 << i)) > 0); if (3==(count-1-i)%4) snprintf(str, sizeof(str), "%s ", str); } } rtk_fig_color_rgb32(dio->fig, COLOR_DIO); rtk_fig_text(dio->fig, +1, +0.5, 0, str); return; }
// Update a dio device void dio_update(dio_t *dio) { // Update the device subscription if (rtk_menuitem_ischecked(dio->subscribe_item)) { if (!dio->proxy->info.subscribed) if (playerc_dio_subscribe(dio->proxy, PLAYER_OPEN_MODE) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); } else { if (dio->proxy->info.subscribed) if (playerc_dio_unsubscribe(dio->proxy) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); } rtk_menuitem_check(dio->subscribe_item, dio->proxy->info.subscribed); if (dio->proxy->info.subscribed) { // Draw in the dio scan if it has been changed. if (dio->proxy->info.datatime != dio->datatime) { dio_draw(dio); dio->datatime = dio->proxy->info.datatime; } } else { // Dont draw the dio. rtk_fig_show(dio->fig, 0); } }
// Draw the map scan void map_draw(map_t *map) { int x,y; double scale = map->proxy->resolution; rtk_fig_show(map->fig, 1); rtk_fig_clear(map->fig); puts( "map draw" ); rtk_fig_color_rgb32(map->fig, 0xFF0000 ); rtk_fig_rectangle(map->fig, 0,0,0, map->proxy->width * scale, map->proxy->height * scale, 0 ); // TODO - combine contiguous cells to minimize the number of // rectangles we have to draw - performance is pretty nasty right // now on big maps. for( x=0; x<map->proxy->width; x++ ) for( y=0; y<map->proxy->height; y++ ) { switch( map->proxy->cells[ x + y * map->proxy->width ] ) { case -1: // empty: draw nothing break; case 0: // unknown: draw grey square rtk_fig_color_rgb32(map->fig, 0x808080 ); rtk_fig_rectangle(map->fig, (x - map->proxy->width/2.0) * scale + scale/2.0, (y - map->proxy->height/2.0) * scale + scale/2.0, 0, scale, scale, 1); break; case +1: // occupied: draw black square rtk_fig_color_rgb32(map->fig, 0x0 ); rtk_fig_rectangle(map->fig, (x - map->proxy->width/2.0) * scale + scale/2.0, (y - map->proxy->height/2.0) * scale + scale/2.0, 0, scale, scale, 1); break; default: puts( "Warning: invalid occupancy value." ); break; } } return; }
// Draw the ptz scan void ptz_draw(ptz_t *ptz) { double ox, oy, d; double ax, ay, bx, by; double fx, fd; // Camera field of view in x-direction (radians) fx = ptz->proxy->zoom; fd = 0.5 / tan(fx/2); rtk_fig_show(ptz->data_fig, 1); rtk_fig_clear(ptz->data_fig); rtk_fig_show(ptz->data_fig_tilt, 1); rtk_fig_clear(ptz->data_fig_tilt); rtk_fig_color_rgb32(ptz->data_fig, COLOR_PTZ_DATA); ox = 100 * cos(ptz->proxy->pan); oy = 100 * sin(ptz->proxy->pan); rtk_fig_line(ptz->data_fig, 0, 0, ox, oy); ox = 100 * cos(ptz->proxy->pan + fx / 2); oy = 100 * sin(ptz->proxy->pan + fx / 2); rtk_fig_line(ptz->data_fig, 0, 0, ox, oy); ox = 100 * cos(ptz->proxy->pan - fx / 2); oy = 100 * sin(ptz->proxy->pan - fx / 2); rtk_fig_line(ptz->data_fig, 0, 0, ox, oy); // Draw in the zoom bar (2 m in length) d = sqrt(fd * fd + 0.5 * 0.5); ax = d * cos(ptz->proxy->pan + fx / 2); ay = d * sin(ptz->proxy->pan + fx / 2); bx = d * cos(ptz->proxy->pan - fx / 2); by = d * sin(ptz->proxy->pan - fx / 2); rtk_fig_line(ptz->data_fig, ax, ay, bx, by); rtk_fig_color_rgb32(ptz->data_fig_tilt, COLOR_PTZ_DATA_TILT); ox = 100 * cos(ptz->proxy->tilt); oy = 100 * sin(ptz->proxy->tilt); rtk_fig_line(ptz->data_fig_tilt, 0, 0, ox, oy); }
// Update a map device void map_update(map_t *map) { // Update the device subscription if (rtk_menuitem_ischecked(map->subscribe_item)) { if (!map->proxy->info.subscribed) { if (playerc_map_subscribe(map->proxy, PLAYER_OPEN_MODE) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); // download a map if (playerc_map_get_map( map->proxy ) >= 0) map_draw( map ); } } else { if (map->proxy->info.subscribed) if (playerc_map_unsubscribe(map->proxy) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); } rtk_menuitem_check(map->subscribe_item, map->proxy->info.subscribed); if (map->proxy->info.subscribed) { if(rtk_menuitem_ischecked(map->continuous_item)) { static struct timeval old_time = {0,0}; struct timeval time; double time_diff = 0.0; gettimeofday(&time, NULL); // i don't use (map->proxy->info.datatime != map->datatime)) // because some drivers return strange datatimes // and the map may change to often for the current map format // in playerv.h you can adjust MAP_UPDATE_TIME (default 1 sec) time_diff = (double)(time.tv_sec - old_time.tv_sec) + (double)(time.tv_usec - old_time.tv_usec)/1000000; if (time_diff > MAP_UPDATE_TIME) { playerc_map_get_map(map->proxy); map_draw(map); old_time = time; } } } else { // Dont draw the map. rtk_fig_show(map->fig, 0); } }
// Update a laser device void laser_update(laser_t *laser) { // Update the device subscription if (rtk_menuitem_ischecked(laser->subscribe_item)) { if (!laser->proxy->info.subscribed) { if (playerc_laser_subscribe(laser->proxy, PLAYER_OPEN_MODE) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); // Get the laser geometry if (playerc_laser_get_geom(laser->proxy) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); rtk_fig_origin(laser->scan_fig, laser->proxy->pose[0], laser->proxy->pose[1], laser->proxy->pose[2]); } } else { if (laser->proxy->info.subscribed) if (playerc_laser_unsubscribe(laser->proxy) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); } rtk_menuitem_check(laser->subscribe_item, laser->proxy->info.subscribed); // Making config changes here causes X to go nuts. Don't know why - BPG // Update the configuration stuff if (laser->proxy->info.subscribed) laser_update_config(laser); if (laser->proxy->info.subscribed) { // Draw in the laser scan if it has been changed. if (laser->proxy->info.datatime != laser->datatime) { laser_draw(laser); laser->datatime = laser->proxy->info.datatime; } } else { // Dont draw the laser. rtk_fig_show(laser->scan_fig, 0); } }
// Draw the map scan void vectormap_draw(vectormap_t *map) { unsigned ii, jj; uint32_t colour = 0xFF0000; double xCenter, yCenter; const uint8_t * feature; size_t feature_count; rtk_fig_show(map->fig, 1); rtk_fig_clear(map->fig); // draw map data for (ii = 0; ii < map->proxy->layers_count; ++ii) { // get a different colour for each layer the quick way, will duplicate after 6 layers colour = colour >> 4 & colour << 24; rtk_fig_color_rgb32(map->fig, colour); // render the features for (jj = 0; jj < map->proxy->layers_data[ii]->features_count; ++jj) { feature = playerc_vectormap_get_feature_data( map->proxy, ii, jj ); feature_count = playerc_vectormap_get_feature_data_count( map->proxy, ii, jj ); if ((feature) && (feature_count > 0)) player_wkb_process_wkb(map->proxy->wkbprocessor, feature, feature_count, (playerwkbcallback_t)(rtk_fig_line), map->fig); } } // draw map extent rtk_fig_color_rgb32( map->fig, 0xFF0000 ); xCenter = map->proxy->extent.x1 - (map->proxy->extent.x1 - map->proxy->extent.x0)/2; yCenter = map->proxy->extent.y1 - (map->proxy->extent.y1 - map->proxy->extent.y0)/2; rtk_fig_rectangle( map->fig, xCenter, yCenter, 0, map->proxy->extent.x1 - map->proxy->extent.x0, map->proxy->extent.y1 - map->proxy->extent.y0, 0 ); return; }
// Draw the aio scan void aio_draw(aio_t *aio) { int i; char ntext[64], text[1024]; rtk_fig_show(aio->fig, 1); rtk_fig_clear(aio->fig); text[0] = 0; for (i = 0; i < aio->proxy->voltages_count; i++) { snprintf(ntext, sizeof(ntext), "%i: %0.3f\n", i, aio->proxy->voltages[i]); strcat(text, ntext); } // Draw in the aio reading // TODO: get text origin from somewhere rtk_fig_color_rgb32(aio->fig, COLOR_AIO); rtk_fig_text(aio->fig, +1, 0, 0, text); return; }
// Update a ptz device void ptz_update(ptz_t *ptz) { // Update the device subscription if (rtk_menuitem_ischecked(ptz->subscribe_item)) { if (!ptz->proxy->info.subscribed) if (playerc_ptz_subscribe(ptz->proxy, PLAYER_OPEN_MODE) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); } else { if (ptz->proxy->info.subscribed) if (playerc_ptz_unsubscribe(ptz->proxy) != 0) PRINT_ERR1("libplayerc error: %s", playerc_error_str()); } rtk_menuitem_check(ptz->subscribe_item, ptz->proxy->info.subscribed); // Draw in the ptz scan if it has been changed. if (ptz->proxy->info.subscribed) { if (ptz->proxy->info.datatime != ptz->datatime) ptz_draw(ptz); ptz->datatime = ptz->proxy->info.datatime; } else { rtk_fig_show(ptz->data_fig, 0); rtk_fig_show(ptz->data_fig_tilt, 0); } // Move the ptz if (ptz->proxy->info.subscribed && rtk_menuitem_ischecked(ptz->command_item)) { rtk_fig_show(ptz->cmd_fig, 1); rtk_fig_show(ptz->cmd_fig_tilt, 1); ptz_move(ptz); } else { rtk_fig_show(ptz->cmd_fig, 0); rtk_fig_show(ptz->cmd_fig_tilt, 0); } }
// Draw the map scan void map_draw(map_t *map) { double scale = map->proxy->resolution; double color; char val; char* cellsDrawn=(char*)calloc(map->proxy->width*map->proxy->height,sizeof(char)); int mapWidth=map->proxy->width; int mapHeight=map->proxy->height; int x,y; double ox,oy; double rectangleWidth,rectangleHeight; int startx, starty; int endx,endy; rtk_fig_show(map->fig, 1); rtk_fig_clear(map->fig); puts( "map draw" ); rtk_fig_color(map->fig, 0.5, 0.5, 0.5 ); rtk_fig_rectangle(map->fig, 0,0,0, mapWidth * scale, mapHeight* scale, 1 ); for( y=0; y<mapHeight; y++ ) for( x=0; x<mapWidth; x++) { if(cellsDrawn[x+y*mapWidth]==TRUE) continue; startx=x; starty=y; endy=mapHeight-1; val = map->proxy->cells[ x + y * mapWidth ]; if(val==0) continue; while(x < mapWidth) { int yy = y; while(yy+1 < mapHeight) { if(map->proxy->cells[x+(yy+1)*mapWidth]==val) yy++; else break; } if( yy < endy ) endy=yy; if(x+1<mapWidth && map->proxy->cells[(x+1)+y*mapWidth]==val && cellsDrawn[(x+1)+y*mapWidth]==FALSE) x++; else break; } endx=x; map_mark_cells(cellsDrawn,mapWidth,mapHeight,startx,starty,endx,endy); rectangleWidth=(endx-startx+1)*scale; rectangleHeight=(endy-starty+1)*scale; ox=((startx-mapWidth/2.0f)*scale+rectangleWidth/2.0f); oy=((starty-mapHeight/2.0f)*scale+rectangleHeight/2.0f); color = (double)val/map->proxy->data_range; // scale to[-1,1] color *= -1.0; //flip sign for coloring occupied to black color = (color + 1.0)/2.0; // scale to [0,1] rtk_fig_color(map->fig, color, color, color ); rtk_fig_rectangle(map->fig, ox, oy, 0, rectangleWidth, rectangleHeight,1); } free(cellsDrawn); return; }
// Draw the ranger scan void ranger_draw(ranger_t *ranger) { int ii = 0, jj = 0; int point_count; double point1[2], point2[2]; double (*points)[2]; double current_angle = 0.0f, temp = 0.0f; unsigned int ranges_per_sensor = 0; // Drawing type depends on the selected sensor type // Singular sensors (e.g. sonar sensors): // Draw a cone for the first range scan of each sensor // Non-singular sensors (e.g. laser scanner): // Draw the edge of the scan and empty space // Calculate the number of ranges per sensor if (ranger->proxy->sensor_count == 0) ranges_per_sensor = ranger->proxy->ranges_count; else ranges_per_sensor = ranger->proxy->ranges_count / ranger->proxy->sensor_count; if (rtk_menuitem_ischecked(ranger->device_item)) { // Draw sonar-like points = calloc(3, sizeof(double)*2); temp = 20.0f * M_PI / 180.0f / 2.0f; for (ii = 0; ii < ranger->proxy->sensor_count; ii++) { rtk_fig_show(ranger->scan_fig[ii], 1); rtk_fig_clear(ranger->scan_fig[ii]); rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_SONAR_SCAN); // Draw a cone for the first range for each sensor // Assume the range is straight ahead (ignore min_angle and resolution properties) points[0][0] = 0.0f; points[0][1] = 0.0f; points[1][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(-temp); points[1][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(-temp); points[2][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(temp); points[2][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(temp); rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, 3, points, 1); // Draw the sensor itself rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER); rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0); } free(points); points=NULL; } else { // Draw laser-like if (rtk_menuitem_ischecked(ranger->style_item)) { // Draw each sensor in turn points = calloc(ranger->proxy->sensor_count, sizeof(double)*2); for (ii = 0; ii < ranger->proxy->sensor_count; ii++) { rtk_fig_show(ranger->scan_fig[ii], 1); rtk_fig_clear(ranger->scan_fig[ii]); // Draw empty space points[0][0] = ranger->proxy->sensor_poses[ii].px; points[0][1] = ranger->proxy->sensor_poses[ii].py; point_count = 1; current_angle = ranger->start_angle; // Loop over the ranges for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++) { range_to_point(ranger, jj, ii, current_angle, points[point_count]); // Move round to the angle of the next range current_angle += ranger->resolution; point_count++; } rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_EMP); rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, point_count, points, 1); // Draw occupied space rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC); current_angle = ranger->start_angle; for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++) { range_to_point(ranger, jj, ii, current_angle - ranger->resolution, point1); range_to_point(ranger, jj, ii, current_angle + ranger->resolution, point2); rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0], point2[1]); current_angle += ranger->resolution; } } free(points); points = NULL; } else { // Draw a range scan for each individual sensor in the device for (ii = 0; ii < ranger->proxy->sensor_count; ii++) { rtk_fig_show(ranger->scan_fig[ii], 1); rtk_fig_clear(ranger->scan_fig[ii]); rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC); current_angle = ranger->start_angle; // Get the first point range_to_point(ranger, ii * ranges_per_sensor, ii, ranger->start_angle, point1); // Loop over the rest of the ranges for (jj = ii * ranges_per_sensor + 1; jj < (ii + 1) * ranges_per_sensor; jj++) { range_to_point(ranger, jj, ii, current_angle, point2); // Draw a line from point 1 (previous point) to point 2 (current point) rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0], point2[1]); point1[0] = point2[0]; point1[1] = point2[1]; // Move round to the angle of the next range current_angle += ranger->resolution; } } } // For each sensor... for (ii = 0; ii < ranger->proxy->sensor_count; ii++) { if (rtk_menuitem_ischecked(ranger->intns_item)) { // Draw an intensity scan if (ranger->proxy->intensities_count > 0) { current_angle = ranger->start_angle; for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++) { if (ranger->proxy->intensities[0] != 0) { range_to_point(ranger, jj, ii, current_angle, point1); rtk_fig_rectangle(ranger->scan_fig[ii], point1[0], point1[1], 0, 0.05, 0.05, 1); } current_angle += ranger->resolution; } } } // Draw the sensor itself rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER); rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0); } } }
// Draw the laser scan void laser_draw(laser_t *laser) { int i; int style; double ax, ay, bx, by; double r, b, res; int point_count; double (*points)[2]; rtk_fig_show(laser->scan_fig, 1); rtk_fig_clear(laser->scan_fig); if (!rtk_menuitem_ischecked(laser->style_item)) { rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC); // Draw in the range scan for (i = 0; i < laser->proxy->scan_count; i++) { bx = laser->proxy->point[i].px; by = laser->proxy->point[i].py; if (i == 0) { ax = bx; ay = by; } rtk_fig_line(laser->scan_fig, ax, ay, bx, by); ax = bx; ay = by; } } else { res = laser->proxy->scan_res / 2; // Draw in the range scan (empty space) points = calloc(laser->proxy->scan_count,sizeof(double)*2); for (i = 0; i < laser->proxy->scan_count; i++) { r = laser->proxy->scan[i][0]; b = laser->proxy->scan[i][1]; points[i][0] = r * cos(b - res); points[i][1] = r * sin(b - res); } rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_EMP); rtk_fig_polygon(laser->scan_fig, 0, 0, 0, laser->proxy->scan_count, points, 1); free(points); points = NULL; // Draw in the range scan (occupied space) rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC); for (i = 0; i < laser->proxy->scan_count; i++) { r = laser->proxy->scan[i][0]; b = laser->proxy->scan[i][1]; ax = r * cos(b - res); ay = r * sin(b - res); bx = r * cos(b + res); by = r * sin(b + res); rtk_fig_line(laser->scan_fig, ax, ay, bx, by); } } // Draw in the intensity scan for (i = 0; i < laser->proxy->scan_count; i++) { if (laser->proxy->intensity[i] == 0) continue; ax = laser->proxy->point[i].px; ay = laser->proxy->point[i].py; rtk_fig_rectangle(laser->scan_fig, ax, ay, 0, 0.05, 0.05, 1); } // Draw in the laser itself rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER); rtk_fig_rectangle(laser->scan_fig, 0, 0, 0, laser->proxy->size[0], laser->proxy->size[1], 0); return; }