def main():
R=RuncommandFlag()
pub=R.Init_node()
rate = rospy.Rate(2)
while not rospy.is_shutdown():
msg=code_flags()
msg.object_detect_id=1
# msg.object_ibvs_id=2
# msg.desire_detect_id=0
# msg.desire_ibvs_id=4
pub.publish(msg)
rate.sleep()
def main():
try:
# 初始化ros节点
rospy.init_node("cv_bridge_test")
rospy.loginfo("Starting cv_bridge_test node")
k = DetectTile()
code_flag_sub = Codeflags()
sub = rospy.Subscriber("/pick_place_tile_vision/object_detect_id",
UInt8, code_flag_sub.callback_object_detect_id)
sub = rospy.Subscriber("/pick_place_tile_vision/desire_detect_id",
UInt8, code_flag_sub.callback_desire_detect_id)
"""
we use flag to choose different state
0,Use for one feature IBVS
1,For detecting object tile
2,Use for detecting two feature with IBVS
3,Use for two features IBVS
"""
flag_data = code_flags()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
try:
if len(code_flag_sub.object_detect_id_buf) != 0:
# print "code_flag_sub.object_detect_id_buf[-1]",code_flag_sub.object_detect_id_buf[-1]
if code_flag_sub.object_detect_id_buf[-1] == 1:
k.process_rgb_object_image(k.rgb_image)
# cen=k.process_rgb_image(k.rgb_image)
# print "cenpixel\n",cen
# time.sleep(1)
else:
print "please wait open object vision flag----"
if len(code_flag_sub.desire_detect_id_buf) != 0:
if code_flag_sub.desire_detect_id_buf[-1] == 1:
k.process_rgb_desire_image(k.rgb_image)
# cen=k.process_rgb_image(k.rgb_image)
# print "cenpixel\n",cen
# print "haha"
# time.sleep(1)
else:
print "please wait open desire vision flag----"
rate.sleep()
except:
print "no sucking tile----"
# rospy.spin()
except KeyboardInterrupt:
print "Shutting down cv_bridge_test node."
cv2.destroyAllWindows()
#!/usr/bin/env python
import rospy
from tilling_robot.msg import code_flags
import yaml
rospy.init_node("code_msg_test")
pub = rospy.Publisher('/pick_place_tile_vision/pick_vision',
code_flags,
queue_size=10)
rate = rospy.Rate(2)
while not rospy.is_shutdown():
msg = code_flags()
msg.object_detect_id = 1
# msg.object_ibvs_id=2
# msg.desire_detect_id=0
# msg.desire_ibvs_id=4
pub.publish(msg)
rate.sleep()