43 from naoqi_bridge_msgs.msg import(
52 NODE_NAME =
"nao_leds" 57 NaoqiNode.__init__( self, self.
NODE_NAME )
63 random.seed( rospy.Time.now().to_nsec() )
78 self.actionlib.start()
82 int(request.color.r*255) << 16 |
83 int(request.color.g*255) << 8 |
84 int(request.color.b*255)
90 request.fade_duration.to_sec()
96 third_of_duration = request.blink_duration / 3.0
100 bg_msg.led_name =
"FaceLeds" 101 bg_msg.color = request.bg_color
102 bg_msg.fade_duration = third_of_duration
105 blink_msg = copy.deepcopy( bg_msg )
108 feedback = BlinkFeedback()
109 result = BlinkResult()
110 result.still_blinking =
False 115 if not request.colors:
117 reason =
"No colors to blink were specified" 118 elif request.blink_duration.to_sec() <= 0.0:
120 reason =
"Blink duration cannot be 0" 121 elif request.blink_rate_mean <= 0.0
or request.blink_rate_sd <= 0.0:
123 reason =
"Invalid parameter for blink rate" 126 rospy.logwarn(
"Bad Blink request: {}".format(reason))
127 self.actionlib.set_aborted(result, reason)
131 sleep_mean = request.blink_rate_mean
132 sleep_sd = request.blink_rate_sd
133 max_sleep_time = sleep_mean + 3* sleep_sd
136 while (
not self.actionlib.is_preempt_requested()
and 137 not rospy.is_shutdown() ) :
140 blink_msg.color = random.choice( request.colors )
145 rospy.sleep( third_of_duration )
150 feedback.last_color = blink_msg.color
151 self.actionlib.publish_feedback( feedback )
154 sleep_time = random.gauss( sleep_mean, sleep_sd )
158 elif sleep_time > max_sleep_time :
159 sleep_time = max_sleep_time
161 rospy.sleep( sleep_time )
164 if self.actionlib.is_new_goal_available() :
165 result.still_blinking =
True 168 self.actionlib.set_preempted( result )
170 if __name__ ==
'__main__':
172 rospy.loginfo( node.NODE_NAME +
" running..." )
174 rospy.loginfo( node.NODE_NAME +
" stopped." )
def fade_rgb(self, request)
def run_blink(self, request)
def get_proxy(self, name, warn=True)