def navigator_run(is_goal, curr_goal, curr_pose):
#curr_goal, curr_pose = myargs
print "carmen"
robot = RobotCallback().__disown__()
print "loc"
loc = pyCarmen.global_pose(robot)
#robot.initialize_pose(5.3, 11.5, 0)
#robot.set_goal(15, 12)
#robot.command_go()
while True:
pyCarmen.carmen_ipc_sleep(0.1)
#print is_goal
if (is_goal.value == 1):
print "setting goal", curr_goal[0], curr_goal[1]
robot.set_goal(curr_goal[0], curr_goal[1])
robot.command_go()
is_goal.value = 0
print "G",
if robot.has_data:
#print "pos", robot.position
#print "orient", math.degrees(robot.orientation)
#print "my command:", curr_command
print ".",
#print robot.position
curr_pose[0] = robot.position[0]
curr_pose[1] = robot.position[1]
curr_pose[2] = robot.orientation
sys.stdout.flush()
def processCarmen(self):
pyCarmen.carmen_ipc_sleep(0.1)
if self.robot.has_data:
self.curr_pose[0] = self.robot.position[0]
self.curr_pose[1] = self.robot.position[1]
self.yaw = self.robot.orientation
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 connect(self):
while (1):
pyCarmen.carmen_ipc_sleep(0.05)
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)
fl = pyCarmen.front_laser(self)
self.range_plt, = plot([], [], 'gx')
self.my_corner_plt, = plot([], [], 'ro')
def callback(self, the_type, msg):
#print msg.keys()
#msg["robot_pose"]
#msg["range"]
pts = rtheta_to_xy(point(0, 0, 0), msg["range"])
corners = corner_extractor(pts, 5, 0.2)
pts = array(pts)
X, Y = pts
print pts
self.range_plt.set_data(X, Y)
X = pts[0].take(corners)
Y = pts[1].take(corners)
self.my_corner_plt.set_data(X, Y)
axis([-5, 15, -10, 10])
draw()
if __name__ == "__main__":
myrobot = test_robot()
while (1):
pyCarmen.carmen_ipc_sleep(0.01)
def connect(self): while(1): pyCarmen.carmen_ipc_sleep(0.05)