#!/usr/bin/env python2.7

### Cesar Rodriguez Dec 2021
### Generate step input in x or y direction
import rospy,  tf
import time
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Bool

class Main:
	# class method used to initialize variables once when the program starts
	def __init__(self):

		# variable(s)
		self.Point 	= PoseStamped()
		# Init x, y, & z coordinates
		self.Point.pose.position.x	= 1
		self.Point.pose.position.y	= 0
		self.Point.pose.position.z	= 4.0
		
		self.bool = False

		# subscriber(s)
		rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) 

		# publisher(s), with specified topic, message type, and queue_size
		self.pub_step = rospy.Publisher('/reference/waypoints', PoseStamped, queue_size = 5) 	 	

		# rate(s)
		pub_rate = 1 # rate for the publisher method, specified in Hz

		# timer(s), used to control method loop frequencies as defined by the rate(s)
		self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.pub)

	# Callbacks
	def wait_cb(self,data):
		self.bool = data

	# Publish messages
	def pub(self,pub_timer):
		if self.bool == False: 		
			rospy.loginfo('Waiting...')
		else:
			self.Point.header.stamp = rospy.Time.now()
			
			# Published desired msgs
			self.pub_step.publish(self.Point)

			rospy.loginfo("Sending [Point x] %d [Point y] %d",
										 self.Point.pose.position.x, self.Point.pose.position.y) 	

if __name__=="__main__":

	# Initiate ROS node
	rospy.init_node('step',anonymous=True)
	try:
		Main() # create class object
		rospy.spin() # loop until shutdown signal

	except rospy.ROSInterruptException:
	        pass