def main():
print("Test Started")
turtle = MyTurtlebot()
time.sleep(2)
bump_go(turtle)
turtle.stop()
print("Test Finished")
def main():
print("Bug Navigation Started")
turtle = MyTurtlebot()
time.sleep(2)
initial_position = turtle.get_estimated_pose().position
print('Initial position is: \n', initial_position)
bug_nav(turtle, timeout=None, check_connectivity=map_connectivity)
turtle.stop()
def __init__(self):
rospy.init_node("Navigator")
self.turtle = MyTurtlebot(headless=True)
# Waiting for Planner
rospy.wait_for_service("/planner/path/get")
self.get_path = rospy.ServiceProxy("/planner/path/get", GetPlan)
self.server = actionlib.SimpleActionServer("/navigator",
MoveBaseAction,
self.do_navigate, False)
self.server.start()
def __init__(self):
rospy.init_node("Explorer")
self.turtle = MyTurtlebot(headless=True)
self.do_autostop = True
# Map generator
self.map_generator = Laser2Grid(headless=True)
rospy.wait_for_service("/my_map/get")
self.map_client = rospy.ServiceProxy("/my_map/get", GetMap)
self.server = actionlib.SimpleActionServer("/explorer", ExploreAction,
self.do_explore, False)
self.server.start()
def main():
print("Test Started")
turtle = MyTurtlebot()
time.sleep(2)
global cam_info_sub
cam_info_sub = rospy.Subscriber("/camera/depth/camera_info", CameraInfo,
cam_info_cb)
pc_sub = rospy.Subscriber("/camera/depth/points", PointCloud2, pc_cb)
time.sleep(0.5)
rgb_sub = message_filters.Subscriber("/camera/rgb/image_raw", Image)
depth_sub = message_filters.Subscriber("/camera/depth/image_raw", Image)
ts = message_filters.TimeSynchronizer([rgb_sub, depth_sub], 10)
ts.registerCallback(callback)
while turtle.is_running():
turtle.set_vel(az=0.2)
pose = turtle.get_estimated_pose()
turtle_pose = (pose.position.x, pose.position.y, pose.position.z)
if mark_pose is not None:
# print(-mark_pose[2] + turtle_pose[0], mark_pose[0] + turtle_pose[1], mark_pose[2] + turtle_pose[0])
#print(mark_pose)
pass
time.sleep(RATE)
cv.destroyAllWindows()
print("Test Finished")
class Navigator:
RATE = 0.02
def __init__(self):
rospy.init_node("Navigator")
self.turtle = MyTurtlebot(headless=True)
# Waiting for Planner
rospy.wait_for_service("/planner/path/get")
self.get_path = rospy.ServiceProxy("/planner/path/get", GetPlan)
self.server = actionlib.SimpleActionServer("/navigator",
MoveBaseAction,
self.do_navigate, False)
self.server.start()
def do_navigate(self, goal):
pose = self.turtle.get_estimated_pose()
rospy.loginfo("Initial Point: [{}, {}]".format(pose.position.x,
pose.position.y))
rospy.loginfo("Goal Point: [{}, {}]".format(
goal.target_pose.pose.position.x,
goal.target_pose.pose.position.y))
path = self.get_path(PoseStamped(pose=pose), goal.target_pose, 0.001)
self.follow_path(path)
self.server.set_succeeded()
def follow_path(self, path):
for pose in path.plan.poses:
print("Going to", pose.pose.position.x, pose.pose.position.y)
self.turtle.set_pos(pose.pose.position.x, pose.pose.position.y)
self.server.publish_feedback(MoveBaseFeedback(base_position=pose))
self.turtle.stop()
class Explorer:
RATE = 0.02
def __init__(self):
rospy.init_node("Explorer")
self.turtle = MyTurtlebot(headless=True)
self.do_autostop = True
# Map generator
self.map_generator = Laser2Grid(headless=True)
rospy.wait_for_service("/my_map/get")
self.map_client = rospy.ServiceProxy("/my_map/get", GetMap)
self.server = actionlib.SimpleActionServer("/explorer", ExploreAction,
self.do_explore, False)
self.server.start()
def do_explore(self, goal):
print(goal)
if goal.goal.id == "1":
self.do_bug_nav(autostop=self.do_autostop)
elif goal.goal.id == "2":
self.do_bump_go(autostop=self.do_autostop)
else:
result = ExploreResult()
result.result.goal_id.stamp = goal.goal.stamp
result.result.goal_id.id = goal.goal.id
result.result.status = status_enum.index("REJECTED")
result.result.text = "Unknown exploration strategy"
print(result)
self.server.set_succeeded(result)
result = ExploreResult()
result.result.goal_id = goal.goal
result.result.status = status_enum.index("SUCCEEDED")
result.result.text = "Exploration completed"
self.server.set_succeeded(result)
def do_bump_go(self, timeout=None, autostop=False):
if autostop:
bump_go(self.turtle, timeout, self.map_connectivity)
else:
bump_go(self.turtle, timeout)
self.turtle.stop()
def do_bug_nav(self, timeout=None, autostop=False):
if autostop:
bug_nav(self.turtle, timeout, self.map_connectivity)
else:
bug_nav(self.turtle, timeout)
self.turtle.stop()
# this function checks connectivity in free space,
# if there is unexplored space next to it, map is not finished
# due to bad precision, some free spaces are behind walls
# possible solution would be counting number of adjacencies and if value
# is under some threshold, we suppose the map is finished
def map_connectivity(self):
resp = self.map_client()
map_gen = MyMap()
grid = map_gen.from_msg(resp.map)
finished = True
adjacency_count = 0
threshold = 1
len_x, len_y = np.shape(grid)
list_of_zeros = np.transpose(np.where(grid == 0))
print('list of possible points has dimensions: ',
np.shape(list_of_zeros))
for a in list_of_zeros:
x, y = a
if x > 0:
if grid[x - 1, y] == -1:
adjacency_count += 1
if x < len_x - 1:
if grid[x + 1, y] == -1:
adjacency_count += 1
if y > 0:
if grid[x, y - 1] == -1:
adjacency_count += 1
if y < len_y - 1:
if grid[x, y + 1] == -1:
adjacency_count += 1
if adjacency_count > threshold:
finished = False
print('map is incomplete')
return finished
if finished:
print('the total number of adjacent points to unexplored ones is ',
adjacency_count)
return finished