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