
import random
import point
import time
import stats
import plot
import math
import uav
import rospy
import tf
import violations
import controller
import zmqros
from geometry_msgs.msg import Twist


class Simulation(object):

    POSITION_NOISE = 0.1
    ORIENTATION_NOISE = 0.1

    def __init__(self, problem, risk_grid, **kwargs):
        rospy.init_node("rover", anonymous=True)
        problem.grid.set_risk_grid(risk_grid)
        self.control_noise = kwargs.get("control_noise", 0.1)

        self.init_problem_instance(problem, risk_grid, kwargs)
        self.init_visualizations(problem, risk_grid, kwargs)
        self.init_zmqros(problem)
        self.init_configurations(problem, risk_grid, kwargs)
        self.init_statistics(problem, risk_grid, kwargs)

        self.msg_type = "geometry_msgs/Twist"
        self.max_z_vel = 50  # cm/s
        self.max_o_vel = 30  # deg/s

    def init_zmqros(self, problem):
        if self.practical:
            self.swarm = zmqros.coordinator.create_swarm_from_ns(
                zmqros.get_ns_host(), zmqros.get_ns_port()
            )
        else:
            self.ns = None

    def init_problem_instance(self, problem, risk_grid, kwargs):