Visualization script

visualize.cpp

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "classes.h"
#include <string>
#include <sstream>
#include <iostream>

using namespace cv;

double resolution = 0.01;
cv::Point2d canvas_center;

cv::Point2d worldToCanvas(double x, double y) {
    return cv::Point2d(-y / resolution, -x / resolution) + canvas_center;
}

int show_canvas(emc::LaserData scan, Forces F, double setpoint_x, double setpoint_y) {

    // Create canvas
    cv::Mat canvas(500, 500, CV_8UC3, cv::Scalar(50, 50, 50));
    canvas_center = cv::Point2d(canvas.rows / 2, canvas.cols / 2);

    // Draw Pico
    cv::Scalar robot_color(0, 0, 255);
    std::vector<std::pair<double, double> > robot_points;
    robot_points.push_back(std::pair<double, double>( 0.1,  -0.2));
    robot_points.push_back(std::pair<double, double>( 0.1,  -0.1));
    robot_points.push_back(std::pair<double, double>( 0.05, -0.1));
    robot_points.push_back(std::pair<double, double>( 0.05,  0.1));
    robot_points.push_back(std::pair<double, double>( 0.1,   0.1));
    robot_points.push_back(std::pair<double, double>( 0.1,   0.2));
    robot_points.push_back(std::pair<double, double>(-0.1,   0.2));
    robot_points.push_back(std::pair<double, double>(-0.1,  -0.2));

    for(unsigned int i = 0; i < robot_points.size(); ++i) {
        unsigned int j = (i + 1) % robot_points.size();
        cv::Point2d p1 = worldToCanvas(robot_points[i].first, robot_points[i].second);
        cv::Point2d p2 = worldToCanvas(robot_points[j].first, robot_points[j].second);
        cv::line(canvas, p1, p2, robot_color, 2);
    }

    // Draw laser data
    double a = scan.angle_min;
    for(unsigned int i = 0; i < scan.ranges.size(); ++i)
    {
        double x = cos(a) * scan.ranges[i];
        double y = sin(a) * scan.ranges[i];

        cv::Point2d p = worldToCanvas(x, y);
        if (p.x >= 0 && p.y >= 0 && p.x < canvas.cols && p.y < canvas.rows)
            canvas.at<cv::Vec3b>(p) = cv::Vec3b(255, 255, 255);

        a += scan.angle_increment;
    }

    // Draw potential field
    cv::Point2d F_a_start = worldToCanvas(F.F_a.start.x, F.F_a.start.y);
    cv::Point2d F_a_end = worldToCanvas(F.F_a.end.x, F.F_a.end.y);

    cv::Point2d F_r_start = worldToCanvas(F.F_r.start.x, F.F_r.start.y);
    cv::Point2d F_r_end = worldToCanvas(F.F_r.end.x, F.F_r.end.y);

    cv::Point2d F_t_start = worldToCanvas(F.F_t.start.x, F.F_t.start.y);
    cv::Point2d F_t_end = worldToCanvas(F.F_t.end.x, F.F_t.end.y);

    line( canvas, F_t_start, F_t_end, Scalar( 255, 255, 255 ),  2, 8 );
    line( canvas, F_a_start, F_a_end, Scalar( 255, 255, 0 ),  2, 8 );
    line( canvas, F_r_start, F_r_end, Scalar( 0, 255, 0 ),  2, 8 );

    // Show setpoint
    if ( (setpoint_x != 0) || (setpoint_y != 0) ) {
        cv::Point2d setpoint = worldToCanvas(setpoint_x, setpoint_y);
        circle(canvas, setpoint, 5, Scalar( 0, 255, 255 ), 5, 8, 0);
        putText(canvas,"Setpoint", cv::Point2d(25,85), FONT_HERSHEY_SIMPLEX, 0.5, Scalar( 0, 255, 255 ));
    }

    // Show max view radius
    cv::Point2d pico_center = worldToCanvas(0, 0);
    circle(canvas, pico_center, VIEW_DIST/resolution, Scalar( 125, 125, 125 ), 2, 8, 0);

    // Convert pledge counter into string
    std::stringstream ss;
    ss << pledge;
    std::string pledge_count = ss.str();

    // Show legend
    putText(canvas,"Pico", cv::Point2d(25,25), FONT_HERSHEY_SIMPLEX, 0.5, Scalar( 0, 0, 255 ));
    putText(canvas,"F_attractive", cv::Point2d(25,40), FONT_HERSHEY_SIMPLEX, 0.5, Scalar( 255, 255, 0 ));
    putText(canvas,"F_repulsive", cv::Point2d(25,55), FONT_HERSHEY_SIMPLEX, 0.5, Scalar( 0, 255, 0 ));
    putText(canvas,"Input", cv::Point2d(25,70), FONT_HERSHEY_SIMPLEX, 0.5, Scalar( 255, 255, 255 ));
    putText(canvas,"Pledge:", cv::Point2d(400,25), FONT_HERSHEY_SIMPLEX, 0.5, Scalar( 255, 0, 255));
    putText(canvas,pledge_count, cv::Point2d(460,25), FONT_HERSHEY_SIMPLEX, 0.5, Scalar( 255, 0, 255));

    // Show canvas
    imshow("Custom visualization",canvas);
    waitKey( 3 );
    return(0);

}