63 lines
1.6 KiB
Python
Executable File
63 lines
1.6 KiB
Python
Executable File
#!/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 Point
|
|
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 = Point()
|
|
# Init x, y, & z coordinates
|
|
self.Point.x = 5
|
|
self.Point.y = 0
|
|
self.Point.z = 3.5 # need to check what the xd was previously and save the height
|
|
|
|
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', Point, 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.x, self.Point.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
|
|
|