cd ~/Shahao/navigation_research source devel/setup.bash roslaunch navigation_research controller_project.launch
cd ~/Shahao/Following_Leader/leader_pose_estimate python t_subgoal_node.py
cd ~/Shahao/Following_Leader python local_planner_node.py
cd ~/Shahao/navigation_research source devel/setup.bash roslaunch turtlebot_teleop keyboard_teleop.launch