Write a Proposition Node - VerifiableRobotics/LTL_stack GitHub Wiki
For this ROS node, you need one Publisher sending Bool messages to the topic: <example_name>/inputs/<input_name>
.
<example_name>
is the name of your example and <input_name>
is the name of the input proposition.
Below is an input node example.
#! /usr/bin/env python
import argparse
import rospy
import std_msgs.msg
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Get AprilTag")
parser.add_argument('node_name', type=str, help='Specify name of ros node')
parser.add_argument('node_publish_topic', type=str, help='Specify name of publishing topic to the controller')
# parse argument
args, unknown = parser.parse_known_args()
rospy.init_node(args.node_name)
# Set up publisher to an input topic
# e.g: arg.node_publish_topic = "example_name/inputs/input_name"
pub = rospy.Publisher(args.node_publish_topic, std_msgs.msg.Bool, queue_size=10)
rate = rospy.Rate(10) # set publish rate
while not rospy.is_shutdown():
#FILL IN information to process
# publish to the input topic
pub.publish(True)
rate.sleep()
You can start the input node above by first copying the code to a file:input_node.py
. Then use the command python input_node.py <node_name> <topic_name>
. Replace <node_name>
and <topic_name>
with yours.
For this ROS node, you need one Subscriber retrieving Bool messages from the topic: <example_name>/outputs/<output_name>
.
<example_name>
is the name of your example and <output_name>
is the name of the output proposition.
Below is an output node example.
#! /usr/bin/env python
import argparse
import rospy
import std_msgs.msg
class OutputProcess(object):
def __init__(self):
self.controller_request_bool = False
def callback(self, data):
# save latest info
self.controller_request_bool = data.data
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Set velocity of robot.")
parser.add_argument('node_name', type=str, help='Specify name of ros node')
parser.add_argument('node_subscribe_topic', type=str, help='Specify controller topic to respond to')
# get arguments
args, unknown = parser.parse_known_args()
rospy.init_node(args.node_name)
rate = rospy.Rate(10) # set publish rate
a = OutputProcess()
# subsribe to an output proposition topic
# e.g: arg.node_subscribe_topic = "example_name/outputs/output_name"
rospy.Subscriber(args.node_subscribe_topic, std_msgs.msg.Bool, callback=a.callback)
while not rospy.is_shutdown():
if a.controller_request_bool:
# FILL IN response to the valuation of the output
pass
rate.sleep()
You can start the input node above by first copying the code to a file:output_node.py
. Then use the command python output_node.py <node_name> <topic_name>
. Replace <node_name>
and <topic_name>
with yours.