This combines the bug_nav node with the goto_processing_plant scout behavior.
To test:
Start sim
New terminal - roslaunch ./launch/scoot.launch "search:=search" "mode:=manual"
New terminal - rosrun navigation bug_nav.py
New terminal - ROS_NAMESPACE="/scout_1" rosrun scoot repl.py
In the repl:
In [1]: from geometry_msgs.msg import Point
In [2]: home_pose = Point()
In [3]: home_pose.x = 20.0
In [4]: home_pose.y = 20.0
In [5]: scoot.home_pose = home_pose
In [6]: scout.goto_processing_plant.main(scoot)
Quick walk-through of code:
goto_processing_plant.py gets the home_pose from scoot (this will be set once we find the processing plant)
executes go_to.py's main function, which sets up the Publisher (/scout_1/waypoints) and the Subscriber (/scout_1/bug_nav_status)
calls the goto function in go_to.py with the home_pose.x, home_pose.y
goto publishes this point on the waypoints topic for bug nav to pick up; it also checks whether the bug_nav_status topic has published the message "Arrived!", if it has, it returns True; if it publishes anything else, it returns False
goto_processing_plant saves the result of the goto function, stops the rover, brakes the rover, and if the result is True, it sys.exit(0), if False, it sys.exit(1)
Also, I added the structure for round 3 behaviors in Task.py and I added a subscriber to the obstacles topic in bug_nav. We need to be able to interrupt bug_nav if we see the processing plant, for example, in case our odometry is very off.
This combines the bug_nav node with the goto_processing_plant scout behavior.
To test:
In the repl: In [1]: from geometry_msgs.msg import Point In [2]: home_pose = Point() In [3]: home_pose.x = 20.0 In [4]: home_pose.y = 20.0 In [5]: scoot.home_pose = home_pose In [6]: scout.goto_processing_plant.main(scoot)
Quick walk-through of code:
Also, I added the structure for round 3 behaviors in Task.py and I added a subscriber to the obstacles topic in bug_nav. We need to be able to interrupt bug_nav if we see the processing plant, for example, in case our odometry is very off.