def run_program(show_map):
map_size = [100, 100]
init_pose = [map_size[0] / 2.0, map_size[1] / 2.0, 0]
#carmen handlers
robot = myRobot(map_size, init_pose, show_map).__disown__()
pyCarmen.front_laser(robot)
#other handlers not covered in carmen
#robot_extras = myTklibHandler()
#pyTklib.subscribe_spline_free_request_message(robot_extras);
#pyTklib.subscribe_spline_free_response_message(robot_extras);
i = 0
while (1):
#pyTklib.carmen_publish_spline_free_request_message(pyTklib.SplineC([0, 0, 0], [10, 10, 1], 10.0, 10.0));
pyTklib.carmen_ipc_sleep(0.05)
pyCarmen.carmen_ipc_sleep(0.05)
def __init__(self, filename, show_gui=True):
#load the logfile and get the relevant EKF
#policies, problem, myEKF = load_problem_with_EKF(filename)
trajopt = carmen_util_load_pck(filename)
self.myEKF = trajopt.get_ekf(point(0, 0, 0))
#open up a robot and subscribe to front_laser messages
Robot.__init__(self)
fl = pyCarmen.front_laser(self)
#initialize pyTklib message passing to allow
# ekf messages to be published
pyTklib.carmen_ipc_initialize(1, ["python"])
#initialize relevant state
self.prev_odo = None
self.prev_timestamp = maxint
self.last_message_timestamp = maxint
self.dt = 0.1
self.show_gui = show_gui
if (show_gui):
self.init_plot()
#self.prev_time = time()
if __name__ == "__main__":
# Create an callback
robot = myRobot().__disown__()
# Create callers
# the exact syntax of these might change a bit
if (not len(argv) == 2):
print "usage: python viewlaser {frontlaser, rearlaser, laser5}"
if (argv[1] == "frontlaser"):
print "viewing front laser"
fl = pyCarmen.front_laser(robot)
elif (argv[1] == "rearlaser"):
print "viewing rear laser"
rl = pyCarmen.rear_laser(robot)
elif (argv[1] == "laser5"):
print "viewing laser 5"
laser = pyCarmen.laser5(robot)
# Dispatch and don't return
robot.start()
robot.connect()
x, y, theta = self.robot.get_odometry()
axis([x-10, x+10, y-10, y+10])
draw()
def set_destinations(self, relative_dests):
odometry = self.robot.get_odometry();
R = get_rotation_matrix2D(odometry[2])
X, Y = dot(R, relative_dests)+transpose([odometry[0:2]])
dest_plt.set_data(X, Y)
if __name__ == "__main__":
#create a robot and subscribe to odometry messages
myrobot = visualizer_robot()
odo = pyCarmen.front_laser(myrobot)
#run the planner
ct = visualizer_tklib(myrobot)
pyTklib.subscribe_trajopt_destinations_message(ct)
pyTklib.subscribe_trajopt_curr_spline_message(ct)
#class subscribe_ekf_message{
#ct.start()
while(1):
pyTklib.carmen_ipc_sleep(0.05);
pyCarmen.carmen_ipc_sleep(0.05);
sleep(0.05)
def __init__(self):
Robot.__init__(self)
fl = pyCarmen.front_laser(self)
self.range_plt, = plot([], [], 'gx')
self.my_corner_plt, = plot([], [], 'ro')
if __name__ == "__main__":
# Create an callback
robot = myRobot().__disown__()
# Create callers
# the exact syntax of these might change a bit
if(not len(argv) == 2):
print "usage: python viewlaser {frontlaser, rearlaser, laser5}"
if(argv[1] == "frontlaser"):
print "viewing front laser"
fl = pyCarmen.front_laser(robot)
elif(argv[1] == "rearlaser"):
print "viewing rear laser"
rl = pyCarmen.rear_laser(robot)
elif(argv[1] == "laser5"):
print "viewing laser 5"
laser = pyCarmen.laser5(robot)
# Dispatch and don't return
robot.start()
robot.connect()