def __init__(self): self.evadeSet = False self.controller = XBox360() self.bridge = CvBridge() self.throttle = 0 self.grid_img = None ##self.throttleLock = Lock() print "evasion" rospy.Subscriber("/lidargrid", Image, self.gridCB, queue_size=1) rospy.Subscriber("/cmd_vel", TwistStamped , self.twistCB , queue_size = 1) rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5) self.pub_img = rospy.Publisher("/steering_img", Image, queue_size = 1) self.pub_twist = rospy.Publisher("/lidar_twist", TwistStamped, queue_size=1) #self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1) rospy.init_node ('lidar_cmd',anonymous=True) rospy.spin()
def __init__(self): print "dataRecorder" self.record = False self.twist = None self.twistLock = Lock() self.bridge = CvBridge() self.globaltime = None self.controller = XBox360() ##rospy.Subscriber("/camera/depth/points" , PointCloud2, self.ptcloudCB) rospy.Subscriber("/camera/depth/image_rect_raw_drop", Image, self.depthCB) rospy.Subscriber("/camera/rgb/image_rect_color_drop", Image, self.streamCB) #rospy.Subscriber("/camera/rgb/image_rect_mono_drop", Image, self.streamCB) ##for black and white images see run.launch and look at the drop fps nodes near the bottom rospy.Subscriber("/lidargrid", Image, self.lidargridCB) rospy.Subscriber("/cmd_vel", TwistStamped, self.cmd_velCB) rospy.Subscriber("/joy", Joy ,self.joyCB) self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1) rospy.init_node('dataRecorder',anonymous=True) rospy.spin()
def __init__(self): rospy.loginfo("runner.__init__") # ---------------------------- self.sess = tf.InteractiveSession() self.model = cnn_cccccfffff() saver = tf.train.Saver() saver.restore(self.sess, "/home/ubuntu/catkin_ws/src/car/scripts/model.ckpt") rospy.loginfo("runner.__init__: model restored") # ---------------------------- self.bridge = CvBridge() self.netEnable = False self.controller = XBox360() rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.runCB) rospy.Subscriber("/joy", Joy, self.joyCB) self.pub_twist = rospy.Publisher("/neural_twist", TwistStamped, queue_size=1) #self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1) rospy.init_node('neural_cmd', anonymous=True) rospy.spin()
def __init__(self): self.throttleInitialized = False self.joy_timeout = 2.0 self.lid_timeout = 0.30 self.cnn_timeout = 0.1 self.joy_time = time.time() self.lid_time = time.time() self.cnn_time = time.time() self.controller = XBox360() self.joy_cmd = TwistStamped() self.lid_cmd = TwistStamped() self.cnn_cmd = TwistStamped() self.cruiseControl = False self.cruiseThrottle = 0.5 self.steeringAngle = 0.5 self.throttle = 0.5 self.trim = 0.0 ##self.throttleLock = Lock() print "cmd_control" rospy.Subscriber("/imu", Imu, self.imuCB, queue_size=5) rospy.Subscriber("/lidar_twist", TwistStamped, self.lidarTwistCB, queue_size=5) rospy.Subscriber("/neural_twist", TwistStamped, self.neuralTwistCB, queue_size=5) rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5) self.vel_pub = rospy.Publisher("/cmd_vel", TwistStamped, queue_size=1) #self.state_pub = rospy.Publisher("/joint_states", JointState, queue_size=1) self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1) rospy.init_node('cmd_control', anonymous=True) rate = rospy.Rate(66) while not rospy.is_shutdown(): self.cmdRouter() rate.sleep()