Ejemplo n.º 1
0
    def __init__(self):
        # basic config
        self.pose = np.array([0.1, 0.1, 0.])
        self.obsv = []

        # ts = message_filters.ApproximateTimeSynchronizer([self.odom_sub, self.scan_sub], 10, 0.01)
        # ts.registerCallback(self.callback)

        self.bearings = np.linspace(0, 2 * pi, 360)
        self.start_time = time.time()

        # ergodic control config
        self.size = 4.0
        self.explr_space = Box(np.array([0.0, 0.0]),
                               np.array([self.size, self.size]),
                               dtype=np.float64)
        self.basis = Basis(explr_space=self.explr_space, num_basis=10)
        # self.t_dist = TargetDist()
        self.t_dist = TargetDist(means=[[1.0, 1.0], [3.0, 3.0]],
                                 cov=0.1,
                                 size=self.size)
        self.phik = convert_phi2phik(self.basis, self.t_dist.grid_vals,
                                     self.t_dist.grid, self.size)
        self.weights = {'R': np.diag([10, 1])}
        self.model = TurtlebotDyn(size=self.size, dt=0.1)
        self.erg_ctrl = RTErgodicControl(self.model,
                                         self.t_dist,
                                         weights=self.weights,
                                         horizon=80,
                                         num_basis=10,
                                         batch_size=500)

        self.obstacles = np.array([[1., 2.], [2., 1.], [2., 2.], [3., 1.],
                                   [1., 3.], [2., 3.], [3., 2.]])
        self.erg_ctrl.barr.update_obstacles(self.obstacles)

        self.erg_ctrl.phik = self.phik
        self.log = {'traj': [], 'ctrls': [], 'ctrl_seq': [], 'count': 0}

        # self.odom_sub = message_filters.Subscriber('/odom', Odometry)#, self.odom_callback)
        # self.scan_sub = message_filters.Subscriber('/scan', LaserScan)#, self.scan_callback)
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        self.scan_sub = rospy.Subscriber('/scan', LaserScan,
                                         self.scan_callback)
        self.ctrl_sub = rospy.Subscriber('/ctrl_flag', Bool,
                                         self.ctrl_callback)
        self.obsv_pub = rospy.Publisher('/landmarks', Marker, queue_size=1)
        self.ctrl_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
Ejemplo n.º 2
0
    def __init__(self):
        # basic config
        self.pose = np.array([0.1, 0.1, 0.])
        self.obsv = []

        self.bearings = np.linspace(0, 2*pi, 360)
        self.start_time = time.time()

        # ergodic control config
        self.size = 4.0
        self.explr_space = Box(np.array([0.0,0.0]), np.array([self.size,self.size]), dtype=np.float64)
        self.basis = Basis(explr_space=self.explr_space, num_basis=10)
        # self.t_dist = TargetDist()
        self.t_dist = TargetDist(means=[[1.0,1.0],[3.0,3.0]], cov=0.1, size=self.size)
        self.phik = convert_phi2phik(self.basis, self.t_dist.grid_vals, self.t_dist.grid, self.size)
        self.weights = {'R': np.diag([10, 1])}
        self.model = TurtlebotDyn(size=self.size, dt=0.1)
        self.erg_ctrl = RTErgodicControl(self.model, self.t_dist, weights=self.weights, horizon=80, num_basis=10, batch_size=500)

        self.obstacles = np.array([[1.,2.], [2.,1.], [2.,2.], [3.,1.], [1.,3.], [2.,3.], [3.,2.]])
        self.erg_ctrl.barr.update_obstacles(self.obstacles)

        self.erg_ctrl.phik = self.phik
        self.log = {'traj':[], 'ctrls':[], 'ctrl_seq':[], 'count':0}

        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
        self.ctrl_sub = rospy.Subscriber('/ctrl_flag', Bool, self.ctrl_callback)
        self.obsv_pub = rospy.Publisher('/landmarks', Marker, queue_size=1)
        self.ctrl_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.path_pub = rospy.Publisher('/test_path', Path, queue_size=1)
        self.map_pub = rospy.Publisher('/landmarks_map', Marker, queue_size=1)

        self.path_msg = Path()
        self.odom_header = None

        #######
        # for test only
        self.old_obsv = []
        self.curr_obsv = []
        self.lm_table = []
Ejemplo n.º 3
0
    def __init__(self):
        # basic config
        self.pose = np.array([0.1, 0.1, 0.])
        self.obsv = []

        self.bearings = np.linspace(0, 2 * pi, 360)
        self.start_time = time.time()

        # ergodic control config
        self.size = 4.0
        self.explr_space = Box(np.array([0.0, 0.0]),
                               np.array([self.size, self.size]),
                               dtype=np.float64)
        self.basis = Basis(explr_space=self.explr_space, num_basis=10)
        # self.t_dist = TargetDist()
        self.t_dist = TargetDist(means=[[1.0, 1.0], [3.0, 3.0]],
                                 cov=0.1,
                                 size=self.size)
        self.weights = {'R': np.diag([10, 1])}
        self.model = TurtlebotDyn(size=self.size, dt=0.1)
        self.erg_ctrl = RTErgodicControl(self.model,
                                         self.t_dist,
                                         weights=self.weights,
                                         horizon=80,
                                         num_basis=10,
                                         batch_size=500)

        self.obstacles = np.array([[1., 2.], [2., 1.], [2., 2.], [3., 1.],
                                   [1., 3.], [2., 3.], [3., 2.]])
        self.erg_ctrl.barr.update_obstacles(self.obstacles)

        self.phik = convert_phi2phik(self.basis, self.t_dist.grid_vals,
                                     self.t_dist.grid, self.size)
        self.erg_ctrl.phik = self.phik
        self.log = {'traj': [], 'ctrls': [], 'ctrl_seq': [], 'count': 0}

        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        self.scan_sub = rospy.Subscriber('/scan', LaserScan,
                                         self.scan_callback)
        self.ctrl_sub = rospy.Subscriber('/ctrl_flag', Bool,
                                         self.ctrl_callback)
        self.obsv_pub = rospy.Publisher('/landmarks', Marker, queue_size=1)
        self.ctrl_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.path_pub = rospy.Publisher('/test_path', Path, queue_size=1)
        self.map_pub = rospy.Publisher('/landmarks_map', Marker, queue_size=1)
        self.ekf_pub = rospy.Publisher('/ekf_odom', Odometry, queue_size=1)

        self.path_msg = Path()
        self.odom_header = None

        #######
        # for test only
        self.old_obsv = []
        self.curr_obsv = []
        self.lm_table = []

        cube_list = Marker()
        cube_list.header.frame_id = 'odom'
        cube_list.header.stamp = rospy.Time.now()
        cube_list.ns = 'landmark_map'
        cube_list.action = Marker.ADD
        cube_list.pose.orientation.w = 1.0
        cube_list.id = 0
        cube_list.type = Marker.CUBE_LIST

        cube_list.scale.x = 0.05
        cube_list.scale.y = 0.05
        cube_list.scale.z = 0.5
        cube_list.color.b = 1.0
        cube_list.color.a = 1.0
        self.cube_list = cube_list

        self.real_landmarks = np.array([
            [2.34, 2.13],  # id: 0
            [0.60, 0.60],  # id: 1
            [0.60, 3.40],  # id: 2
            [3.40, 0.60],  # id: 3
            [3.40, 3.40]
        ])  # id: 4
        self.init_pose = np.zeros(3)
        self.raw_odom_traj = []
        self.lm_id = []

        #######
        # ekf
        self.ekf_mean = np.array([0.0, 0.0, 0.0])
        self.ekf_cov = np.diag([1e-09 for _ in range(3)])
        self.ekf_R = np.diag([0.01, 0.01, 0.01])
        self.ekf_Q = np.diag([0.03, 0.03])
        self.init_flag = False
Ejemplo n.º 4
0
ax1_2.set_aspect('equal')
ax1_2.set_xlim(0, 1)
ax1_2.set_ylim(0, 1)
ax1_2.contourf(*xy, phi.reshape(50, 50))
# plt.pause(0.1)
plt.close()

############################################
# test ergodic controller
from turtlebot_dyn import TurtlebotDyn
from integrator_se2 import IntegratorSE2
from ergodic_control import RTErgodicControl
from utils import convert_ck2dist

# configure simulation
env = TurtlebotDyn()
model = TurtlebotDyn()
weights = {'R': np.diag([10, 1])}
erg_ctrl = RTErgodicControl(model,
                            t_dist,
                            weights=weights,
                            horizon=80,
                            num_basis=5,
                            batch_size=-1)
erg_ctrl.phik = convert_phi2phik(erg_ctrl.basis, t_dist.grid_vals, t_dist.grid)
tf = 1200

# start simulation
log = {'traj': [], 'ctrls': [], 'ctrl_seq': []}
state = env.reset()
for t in range(tf):
Ejemplo n.º 5
0
# plot the reconstructed phi
ax1_2 = fig1.add_subplot(122)
ax1_2.set_aspect('equal')
ax1_2.contourf(*xy, phi.reshape(50, 50), levels=20)
plt.show()
plt.close()

############################################
# test ergodic controller
from turtlebot_dyn import TurtlebotDyn
from ergodic_control import RTErgodicControl
from utils import convert_ck2dist
from time import time

# configure simulation
env = TurtlebotDyn(size=size)
model = TurtlebotDyn(size=size)
weights = {'R': np.diag([10, 1])}
erg_ctrl = RTErgodicControl(model,
                            t_dist,
                            weights=weights,
                            horizon=80,
                            num_basis=5,
                            batch_size=500)
erg_ctrl.phik = convert_phi2phik(erg_ctrl.basis, t_dist.grid_vals, t_dist.grid)
tf = 1200

# ros configuration
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
Ejemplo n.º 6
0
ax1_2.set_aspect('equal')
ax1_2.set_xlim(0, 1)
ax1_2.set_ylim(0, 1)
ax1_2.contourf(*xy, phi.reshape(50, 50))
# plt.pause(0.1)
plt.close()

############################################
# test ergodic controller
from turtlebot_dyn import TurtlebotDyn
from integrator_se2 import IntegratorSE2
from ergodic_control import RTErgodicControl
from utils import convert_ck2dist

# configure simulation
env = TurtlebotDyn()
model = TurtlebotDyn()
weights = {'R': np.diag([10, 1])}
erg_ctrl = RTErgodicControl(model,
                            t_dist,
                            weights=weights,
                            horizon=80,
                            num_basis=5,
                            batch_size=-1)
erg_ctrl.phik = convert_phi2phik(erg_ctrl.basis, t_dist.grid_vals, t_dist.grid)
tf = 1200

# ros configuration
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist