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())