def moving_to_block_side(self): msg = robot_path_t() msg.utime = int(1000000000 * time.time()) msg.path_length = 4 p1 = pose_xyt_t() p2 = pose_xyt_t() p3 = pose_xyt_t() p4 = pose_xyt_t() p1.x = 0.0 p1.y = 0.0 p1.theta = 0.0 p2.x = 0.0 p2.y = -0.4 p2.theta = 0.0 p3.x = 0.3 p3.y = -0.4 p3.theta = 0.0 p4.x = 0.3 p4.y = -0.265 p4.theta = 0.0 msg.path = [p1, p2, p3, p4] self.lc.publish("CONTROLLER_PATH", msg.encode()) self.set_current_state("idle")
def __init__(self, mbot_arm, planner): self.mbot_arm = mbot_arm self.tp = planner self.tags = [] self.status_message = "State: Idle" self.current_state = "idle" self.next_state = "idle" self.lc = lcm.LCM() self.camera_extrinsics = np.zeros((4, 4), dtype=float) self.subscription = self.lc.subscribe(ARM_COMMAND_CH, self.mbot_arm_cmd_handler) self.subscription = self.lc.subscribe(NEXT_BLOCK_REQUEST_CH, self.block_request_handler) self.subscription = self.lc.subscribe(BOT_BLOCK_STATUS_CH, self.mbot_block_status_handler) self.subscription = self.lc.subscribe(SLAM_POSE_CH, self.slam_pose_handler) self.subscription = self.lc.subscribe(PATH_STATUS_CHANNEL, self.path_complete_handler) #for move towards and grabbing self.arm_status = mbot_arm_status_t() self.arm_block = mbot_arm_block_t() #for interaction with explorer self.arm_block_list = [] #mbot_arm_block_t() self.arm_block_ids = [] #int self.cur_block = mbot_arm_block_t( ) #keep track of current block (move towards to) self.mbot_picking_status = mbot_picking_status_t( ) #update together with explorer self.cur_pose = pose_xyt_t() #subscribe to slam pose #initialization for block searching task self.robot_path = robot_path_t() self.robot_path.path = [pose_xyt_t(), pose_xyt_t()] self.robot_path.path_length = 2 self.robot_path.path_type = 5 self.path_completed = True self.complete_counter = -1 self.new_image = False self.initialized = 0 self.block_held = 0 self.block_counter = -1 self.cur_block.tag_id = -1 self.mbot_picking_status.status = self.mbot_picking_status.READY_FOR_NEXT self.cur_pose.x = 0 self.cur_pose.y = 0 self.cur_pose.theta = 0 self.finished_tag_ids = [] self.block_pre_state = DETECT_NONE self.block_cur_state = DETECT_NONE self.lost_tag_count = 0 self.lost_tag_max_count = 50
def __init__(self): # initialize LCM self.lc = lcm.LCM() self.robot_pose = pose_xyt_t() self.robot_path = robot_path_t() self.subscription = self.lc.subscribe(ODOMETRY_CHANNEL, self.robot_pose_handler) self.waypoints = 3 * [ [0.0, 1, PI], [1, 0.0, PI], [1, 1, PI], [1, 0, PI], [0, 1, PI], [0, 0, PI] ] #[[0.0,0.5,PI],[0.5,0.0,PI],[0.5,0.5,PI],[0.5,0,PI],[0,0.5,PI],[0,0,PI]] #[[0.5,1,PI],[-0.5,2,PI],[0.5,3,PI],[-0.5,4,PI],[0,1,PI],[0,0,PI]]# self.stop_front_thresh = None self.nav_dists = None self.nav_angles = None self.x = 0 self.y = 0 self.theta = 0 self.yaw = None self.scale = 10 / length_paper self.bot_publish() self.vision_image = np.zeros((img_width, img_length, 3), dtype=np.float) self.img_view_threshold = np.zeros((img_width, img_length, 3), dtype=np.float) self.worldmap = np.zeros((100, 100, 3), dtype=np.float) self.function_sanity_flag = True self.robot_pose_thread_function() self.end_function()
def moving_back(self): msg = robot_path_t() msg.utime = int(100000000 * time.time()) msg.path_length = 2 p1 = pose_xyt_t() p2 = pose_xyt_t() p1.x = 0.0 p1.y = 0.0 p1.theta = 0.0 p2.x = 0.15 p2.y = 0.0 p2.theta = 0.0 msg.path = [p1, p2] self.lc.publish("CONTROLLER_PATH", msg.encode()) self.set_current_state("idle")
def broadcast_goal(self, goal_pose): """Broadcast goal_pose on channel GOAL_POSE""" msg = pose_xyt_t() msg.utime = int(time.time() * 1000000) msg.x = goal_pose[0] msg.y = goal_pose[1] msg.theta = goal_pose[2] self.sm.lc.publish("GOAL_POSE", msg.encode())
def broadcast_spin_goal(self, goal_pose, only_one_pose=False): """ Broadcase robot_path_t on channel CONTROLLER_PATH""" msg = robot_path_t() msg.utime = int(time.time() * 1000000) start_pose = pose_xyt_t() start_pose.x = self.current_pose[0] start_pose.y = self.current_pose[1] start_pose.theta = self.current_pose[2] end_pose = pose_xyt_t() end_pose.x = goal_pose[0] end_pose.y = goal_pose[1] end_pose.theta = goal_pose[2] if not only_one_pose: msg.path = [start_pose, end_pose] else: msg.path = [start_pose] msg.path_length = len(msg.path) self.sm.lc.publish("CONTROLLER_PATH", msg.encode())
def bot_publish(self): self.robot_path.path_length = len(self.waypoints) angle_check = 1 self.robot_path.path = [] if angle_check == 1: for i in range(self.robot_path.path_length): pose_temp = pose_xyt_t() pose_temp.utime = int(time.time() * 1e6) pose_temp.x = self.waypoints[i][0] pose_temp.y = self.waypoints[i][1] pose_temp.theta = self.waypoints[i][2] #print "radians:",cmd.position_radians # print "pose x,y,stopfrontthresh:",pose_temp.x,pose_temp.y,self.stop_front_thresh,"\n" self.robot_path.path.append(pose_temp) # print "Path x",self.robot_path.path[0].x,"\n" self.robot_path.utime = int(time.time() * 1e6) self.lc.publish(ROBOT_PATH_T_CHANNEL,self.robot_path.encode()) else: print "Check not passed.\n"
def __init__(self): # initialize LCM self.lc = lcm.LCM() self.robot_pose = pose_xyt_t() self.robot_path = robot_path_t() self.subscription = self.lc.subscribe(ODOMETRY_CHANNEL, self.robot_pose_handler) self.waypoints = []#3*[[0.0,1,PI],[1,0.0,PI],[1,1,PI],[1,0,PI],[0,1,PI],[0,0,PI]]#[[0.0,0.5,PI],[0.5,0.0,PI],[0.5,0.5,PI],[0.5,0,PI],[0,0.5,PI],[0,0,PI]] self.stop_front_thresh = None self.nav_dists = None self.nav_angles = None self.x = 0 self.y = 0 self.theta = 0 self.yaw = None self.scale = 10/length_paper # self.bot_publish() self.vision_image = np.zeros((img_width, img_length, 3), dtype=np.float) self.img_view_threshold = np.zeros((img_width, img_length, 3), dtype=np.float) self.worldmap = np.zeros((100, 100, 3), dtype=np.float) # left for picamera self.img = None self.camera = None self.rawCapture = None self.M = None self.dst_size = None self.function_sanity_flag = True #self.robot_pose_thread_function() self.robot_pose_thread = threading.Thread(target=self.robot_pose_thread_function) self.robot_pose_thread.start() self.robot_perception_thread_function() self.end_function()
def publish_mbot_command(self, state, goal_pose, obstacles): """ Publishes mbot command. """ msg = mbot_command_t() msg.utime = int(time.time() * 1e6) msg.state = state if state == mbot_command_t.STATE_STOPPED: pass elif state == mbot_command_t.STATE_MOVING: msg.goal_pose.x, msg.goal_pose.y, msg.goal_pose.theta = 1, 1, 1 msg.num_obstacles = len(obstacles) for i in range(msg.num_obstacles): obs_pose = pose_xyt_t() obs_pose.utime = int(time.time() * 1e6) obs_pose.x, obs_pose.y, obs_pose.theta = obstacles[i] msg.obstacle_poses.append(obs_pose) else: raise NameError('Unknown mbot commanded state') self.lc.publish("MBOT_COMMAND", msg.encode())