class CleanupPlannerNode:
"""
ROS node that spins a FSA to control for the cleanup task
"""
def run(self):
"""
Method to initialize node and then spin the cleanup planner FSA
"""
rospy.init_node("cleanup_planner_node")
self.pioneer = pioneer_wrapper.PioneerWrapper()
rospy.Subscriber("robot_pose", Pose2D, self.robot_pose_callback)
rospy.Subscriber("goal_pose", Pose2D, self.goal_pose_callback)
rospy.Subscriber("cleanup_objects", CleanupObjectArray,
self.object_callback)
rospy.Subscriber("cleanup_zones", CleanupZoneArray,
self.cleanup_zone_callback)
self.control = CleanupControl(self)
self.control.setPrintFunction(rospy.loginfo)
rospy.on_shutdown(self.shutdown_hook)
r = rospy.Rate(30) # 30hz
while not rospy.is_shutdown():
self.control.run()
r.sleep()
def robot_pose_callback(self, msg):
self.control.robot_pose.x = msg.x
self.control.robot_pose.y = msg.y
self.control.robot_pose.theta = msg.theta
def goal_pose_callback(self, msg):
if (msg.x != self.control.user_goal_pose.x or
msg.y != self.control.user_goal_pose.y):
self.control.user_goal_pose.x = msg.x
self.control.user_goal_pose.y = msg.y
self.control.user_goal_pose.theta = msg.theta
rospy.loginfo("Updated robot goal pose")
#self.control.switchTo('go_to_user_goal')
self.control.switchTo('visit_objects')
if msg.x == -1337:
self.control.switchTo('stop_robot')
rospy.loginfo("Removed robot goal pose")
def object_callback(self, msg):
self.control.cleanup_objects = msg.objects
def cleanup_zone_callback(self, msg):
self.control.set_cleanup_zones(msg.zones)
def shutdown_hook(self):
rospy.logdebug("Storing gripper on shutdown!")
self.pioneer.store_gripper()
rospy.sleep(2.0)
def run(self):
"""
Method to initialize node and then spin the cleanup planner FSA
"""
rospy.init_node("cleanup_planner_node")
self.pioneer = pioneer_wrapper.PioneerWrapper()
rospy.Subscriber("robot_pose", Pose2D, self.robot_pose_callback)
rospy.Subscriber("goal_pose", Pose2D, self.goal_pose_callback)
rospy.Subscriber("cleanup_objects", CleanupObjectArray,
self.object_callback)
rospy.Subscriber("cleanup_zones", CleanupZoneArray,
self.cleanup_zone_callback)
self.control = CleanupControl(self)
self.control.setPrintFunction(rospy.loginfo)
rospy.on_shutdown(self.shutdown_hook)
r = rospy.Rate(30) # 30hz
while not rospy.is_shutdown():
self.control.run()
r.sleep()
