def _features_callback(self,data):
if self._initialized and self._running:
time_now = rospy.get_time()
if (time_now - self._last_update_time) >= self._feedback_period:
self._last_update_time = time_now
cmd_current = CmdCurrent()
cmd_current.channel = self._x_channel
cmd_current.current = self._x_current_factor*data.head_x_abs_mm
self._cmd_light_current_pub.publish(cmd_current)
cmd_current.channel = self._y_channel
cmd_current.current = self._y_current_factor*data.head_y_abs_mm
self._cmd_light_current_pub.publish(cmd_current)
def _start_callback(self,stimuli):
self._stopped = False
self._begin = True
# rospy.loginfo('begin here')
self._stochastic_stimulus_channel = stimuli.stochastic_stimulus_channel
self._stochastic_stimulus_current = stimuli.stochastic_current
self._stochastic_left_prob = stimuli.stochastic_left_prob
self._stochastic_right_prob = stimuli.stochastic_right_prob
self._stochastic_background_channel = stimuli.stochastic_background_channel
self._stochastic_background_current = stimuli.stochastic_background_current
cmd_current = CmdCurrent()
cmd_current.channel = self._stochastic_background_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
rospy.loginfo('stochastic sleeping for {0} secs before starting'.format(stimuli.wait.to_sec()))
rospy.sleep(stimuli.wait)
# check to see if stopped while waiting
if not self._stopped:
rospy.loginfo('head_stochastic starting with stochastic_stimulus_current = {0}, stochastic_left_prob = {1}, stochastic_right_prob = {2}'.format(stimuli.stochastic_current,stimuli.stochastic_left_prob,stimuli.stochastic_right_prob))
self._running = True
def _features_callback(self,data):
if self._initialized and self._running:
time_now = rospy.get_time()
if (time_now - self._last_update_time) >= self._feedback_period:
self._last_update_time = time_now
if self.firstframe == True:
self.x0 = data.x_neck
self.y0 = data.y_neck
rospy.loginfo("center in mm %2.2f, %2.2f", self.x0, self.y0)
self.firstframe = False
if self.firstframe == False:
cmd_current = CmdCurrent()
cmd_current.channel = self._x_channel
distance_mm = sqrt(pow((data.x_neck - self.x0),2) + pow((data.y_neck - self.y0),2))
rospy.loginfo("distance in mm %2.2f", distance_mm)
if distance_mm < self._dead_zone :
gradient = 0
else :
gradient = self._radial_gradient
cmd_current.current = int(round(gradient*distance_mm))
rospy.loginfo("current is %2.2f",cmd_current.current)
rospy.loginfo("current is %d",cmd_current.current)
self._cmd_light_current_pub.publish(cmd_current)
def _behavior_callback(self,data):
if self._initialized and self._running:
time_now = rospy.get_time()
if (time_now - self._last_update_time) >= self._feedback_period:
self._last_update_time = time_now
cmd_current = CmdCurrent()
cmd_current.channel = self._stochastic_background_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
# rospy.loginfo('first index %d',self.firstindex)
if (((self.index - self.firstindex + 1 + 500) % 1500) <= 10):
cmd_current = CmdCurrent()
cmd_current.channel = self._stochastic_background_channel
cmd_current.current = self._stochastic_background_current
# rospy.loginfo('blue %d',cmd_current.current)
self._cmd_light_current_pub.publish(cmd_current)
rospy.loginfo("BLUE: %d", cmd_current.current)
self.bend_state = 0
# if(data.b_run == true)
elif (((self.index - self.firstindex + 1 + 500) % 1500) > 10 and ((self.index - self.firstindex + 1 + 500) % 1500) < 750 ):
# rospy.loginfo("RED: %d", (self.index - self.firstindex + 1 + 500) % 1500)
if data.b_bend and data.b_left == False and data.b_right == False:
self.bend_state = 0
elif data.b_bend and data.b_left and (self.s_filtered < 0.8):
if self.bend_state == 0 or self.bend_state == 1:
cmd_current = CmdCurrent()
cmd_current.channel = self._stochastic_stimulus_channel
self.bernoulli_left = random.random()
self.left_current = self._stochastic_stimulus_current*(self.bernoulli_left < self._stochastic_left_prob)
cmd_current.current = self.left_current
self._cmd_light_current_pub.publish(cmd_current)
self.bend_state = -1
rospy.loginfo('Bernoulli left: %f, Current Left: %d',self.bernoulli_left, self.left_current)
elif self.bend_state == -1:
cmd_current = CmdCurrent()
cmd_current.channel = self._stochastic_stimulus_channel
cmd_current.current = self.left_current
self._cmd_light_current_pub.publish(cmd_current)
rospy.loginfo('Current Left: %d',self.left_current)
elif data.b_bend and data.b_right and (self.s_filtered < 0.8):
if self.bend_state == 0 or self.bend_state == -1:
cmd_current = CmdCurrent()
cmd_current.channel = self._stochastic_stimulus_channel
self.bernoulli_right = random.random()
self.right_current = self._stochastic_stimulus_current*(self.bernoulli_right < self._stochastic_right_prob)
cmd_current.current = self.right_current
self.bend_state = 1
self._cmd_light_current_pub.publish(cmd_current)
rospy.loginfo('Bernoulli Right: %f, Current Right: %d',self.bernoulli_right, self.right_current)
elif self.bend_state == 1:
cmd_current = CmdCurrent()
cmd_current.channel = self._stochastic_stimulus_channel
cmd_current.current = self.right_current
self._cmd_light_current_pub.publish(cmd_current)
rospy.loginfo('Current Right: %d',self.right_current)
else:
# rospy.loginfo("STOPPED: %d", (self.index - self.firstindex + 1 + 500) % 1500)
cmd_current = CmdCurrent()
cmd_current.channel = self._stochastic_stimulus_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
def _behavior_callback(self,data):
if self._initialized and self._running:
time_now = rospy.get_time()
if (time_now - self._last_update_time) >= self._feedback_period:
self._last_update_time = time_now
# if data.b_cast and data.b_left:
if data.b_left:
cmd_current = CmdCurrent()
cmd_current.channel = self._cast_left_channel
cmd_current.current = self._cast_left_current
self._cmd_light_current_pub.publish(cmd_current)
if self._cast_right_channel != self._cast_left_channel:
cmd_current.channel = self._cast_right_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
# elif data.b_cast and data.b_right:
elif data.b_right:
cmd_current = CmdCurrent()
if self._cast_left_channel != self._cast_right_channel:
cmd_current.channel = self._cast_left_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
cmd_current.channel = self._cast_right_channel
cmd_current.current = self._cast_right_current
self._cmd_light_current_pub.publish(cmd_current)
else:
cmd_current = CmdCurrent()
cmd_current.channel = self._cast_left_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
if self._cast_right_channel != self._cast_left_channel:
cmd_current.channel = self._cast_right_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
def _behavior_callback(self,data):
if self._initialized and self._running:
time_now = rospy.get_time()
if (time_now - self._last_update_time) >= self._feedback_period:
self._last_update_time = time_now
cmd_current = CmdCurrent()
cmd_current.channel = self._cast_background_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
# rospy.loginfo('first index %d',self.firstindex)
if (((self.index - self.firstindex + 1 + 500) % 1500) <= 4 ):
cmd_current = CmdCurrent()
cmd_current.channel = self._cast_background_channel
cmd_current.current = self._cast_background_current
# rospy.loginfo('blue %d',cmd_current.current)
self._cmd_light_current_pub.publish(cmd_current)
# rospy.loginfo("BLUE: %d", cmd_current.current)
# if(data.b_run == true)
elif (((self.index - self.firstindex + 1 + 500) % 1500) > 4 and ((self.index - self.firstindex + 1 + 500) % 1500) < 500 ):
# rospy.loginfo("RED: %d", (self.index - self.firstindex + 1 + 500) % 1500)
if data.b_bend & data.b_left and (self.s_filtered < 0.8):
cmd_current = CmdCurrent()
cmd_current.channel = self._cast_left_channel
cmd_current.current = self._cast_left_current
self._cmd_light_current_pub.publish(cmd_current)
if self._cast_right_channel != self._cast_left_channel:
cmd_current.channel = self._cast_right_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
elif data.b_bend & data.b_right and (self.s_filtered < 0.8):
cmd_current = CmdCurrent()
if self._cast_left_channel != self._cast_right_channel:
cmd_current.channel = self._cast_left_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
cmd_current.channel = self._cast_right_channel
cmd_current.current = self._cast_right_current
self._cmd_light_current_pub.publish(cmd_current)
else:
cmd_current = CmdCurrent()
cmd_current.channel = self._cast_left_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
if self._cast_right_channel != self._cast_left_channel:
cmd_current.channel = self._cast_right_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
else:
# rospy.loginfo("STOPPED: %d", (self.index - self.firstindex + 1 + 500) % 1500)
cmd_current = CmdCurrent()
cmd_current.channel = self._cast_left_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
if self._cast_right_channel != self._cast_left_channel:
cmd_current.channel = self._cast_right_channel
cmd_current.current = 0
self._cmd_light_current_pub.publish(cmd_current)
