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)
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 _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 _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)