From f46514f0ff96c4d89763a27a34eb00dac99251fe Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Wed, 27 Sep 2023 22:36:09 +0900 Subject: [PATCH] [Beetle] create a new scritp for the assembly flag publication --- .../beetle/script/test/assembly_flag_pub.py | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100755 robots/beetle/script/test/assembly_flag_pub.py diff --git a/robots/beetle/script/test/assembly_flag_pub.py b/robots/beetle/script/test/assembly_flag_pub.py new file mode 100755 index 000000000..9fd27c196 --- /dev/null +++ b/robots/beetle/script/test/assembly_flag_pub.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python + +import rospy +import time +from diagnostic_msgs.msg import KeyValue +from std_msgs.msg import Bool + +if __name__=="__main__": + rospy.init_node("assembly_flag_pub") + r = rospy.Rate(1) + flag_pub = rospy.Publisher('/beetle1/assembly_flag', KeyValue, queue_size = 1) + docking_pub = rospy.Publisher('/beetle1/docking_cmd', Bool, queue_size = 1) + + flag_msg = KeyValue() + docking_msg = Bool() + time.sleep(0.5) + + flag_msg.key = '1' + flag_msg.value = '1' + flag_pub.publish(flag_msg) + time.sleep(0.5) + + flag_msg.key = '2' + flag_msg.value = '1' + flag_pub.publish(flag_msg) + time.sleep(0.5) + + docking_msg.data = True + docking_pub.publish(docking_msg) +