def fusion(self): #self._maskjoints[41] = 0 # designate means _sum = 2.0 self._covs = [ float(self._temp[0]) / _sum, float(self._temp[1]) / _sum, float(self._temp[2]) / _sum, float(self._temp[3]) / _sum ] # take means self._jointmeans = list(self._covs[0]*self.joint_idle+ self._covs[1]*self.joint_reflex +\ self._covs[2]*self.joint_slave + self._covs[3]*self.joint_auto) # get mask for i in range(len(self._jointmeans)): self._jointmeans[i] = self._maskjoints[i] * self._jointmeans[i] self._payload = list(np.add(self._default, self._jointmeans)) # compare if the value exceed maxmin self.compare_bounds(self._payload) # make message tform.make_message(self._pub_msg, 0, 'fusion', 0, self._payload) # update state message self._state_msg.data = self._covs return None
def start(self): loop_rate = rospy.Rate(20) while not rospy.is_shutdown(): tform.make_message(self.msg, 1, 'gait', 1, self._payload) self.pub.publish(self.msg) print self._payload loop_rate.sleep()
def start(self): rospy.loginfo("DEMO") loop_rate = rospy.Rate(50) while not rospy.is_shutdown(): "make message" tform.make_message(self._pub_msg, 0, 'demo', 0, self._payload) "publish message" self.pub.publish(self._pub_msg) loop_rate.sleep()
def start(self): loop_rate = rospy.Rate(_RATE) while not rospy.is_shutdown(): self.react() self._payload_neck = [0, 0, 0, self._neck, 0] tform.make_message(self._pub_msg, 4, 'neck', 2, self._payload_neck) self.pub.publish(self._pub_msg) #print(self._payload_neck) print self._direction loop_rate.sleep()
def start(self): rospy.loginfo("In attesa") while not rospy.is_shutdown(): # make message self._payload = [self.neck_pos[0], self.neck_pos[1], self.neck_pos[2], self.neck_pos[3] + self.eye_width, self.neck_pos[4] + self.eye_height] tform.make_message(self._pub_msg, 0, 'neck', 0, self._payload) # publish self.pub.publish(self._pub_msg) #self.pub.publish(message) self.loop_rate.sleep()
def start(self): rospy.loginfo("In attesa") while not rospy.is_shutdown(): # make message self._payload = [200, 200, self.speed_r, self.speed_l, 600] tform.make_message(self._pub_msg, 4, 'wheel', 5, self._payload) # publish self.pub.publish(self._pub_msg) #self.pub.publish(message) self.loop_rate.sleep()
def speech_cb(self, msg): if msg.data == 'hello': self._content = 'こんにちは' filename = 'hello' elif msg.data == 'agree': self._content = 'そうですね。' filename = 'agree' elif msg.data == 'goodbye': self._content = 'さようならー' filename = 'goodbye' elif msg.data == 'selfintro1': self._content = 'はじめまして。ー。ー。なまえは、いぶきです。いきる、っていういみがあります。ー。としわ、じっさい。' filename = 'selfintro1' elif msg.data == 'selfintro2': self._content = 'すきなことは、いろんなばしょに、いくこと。ー。ー。ー。やまや、うみ、たくさんのどうぶつや、むしたち、。ー。いろんなものをみて、。ー。さわって、。ー。かんじる。ー。いろんなばしょで、いっしょに、あそんでくれるともだちがほしいです。' filename = 'selfintro2' elif msg.data == 'selfintro3': self._content = 'どうぞよろしくおねがいします。' filename = 'selfintro3' elif msg.data == 'thankyou': self._content = 'ありがとうございます' filename = 'thankyou' # because we only want contents be carried out once, so do commu tts here WAVNAME = self.getTTS() # get wave name self._data = self.load_presets(filename) print(self._data[4]) run_event = threading.Event() run_event.set() # send_w = threading.Thread(target = self.tcplink, args = (self.sock, self.addr, WAVNAME, run_eventw)) move_t = threading.Thread(target=self.player_i, args=(WAVNAME, run_event)) # send_w.start() move_t.start() time.sleep(0.19) # threshold # make the message, and send the message to slave operation for index in range(0, len(self._data[4])): self.make_carlos_message(self._data, index) tform.make_message(self._pub_msg, 3, 'slave', 3, self._payload) print(self._payload) self.pub.publish(self._pub_msg) time.sleep(INTERVAL)
def start(self): rospy.loginfo("silva_REFLEX") loop_rate = rospy.Rate(_RATE) while not rospy.is_shutdown(): # set message from callback self.set_msg_from_pos() # make the message tform.make_message(self._pub_msg, 2, 'reflex', 0, self._payload) # publish the message self.pub.publish(self._pub_msg) loop_rate.sleep()
def start(self): rospy.loginfo("silva_AUTO") # publish the /joint_local/auto message loop_rate = rospy.Rate(_RATE) while not rospy.is_shutdown(): # set message from callback self.set_msg_from_pos() # make the message tform.make_message(self._pub_msg, 4, 'auto', 0, self._payload) # publish the message self.pub.publish(self._pub_msg) # print type(self._payload) loop_rate.sleep()
def start(self): rospy.loginfo("silva_SLAVE") loop_rate = rospy.Rate(_RATE) while not rospy.is_shutdown(): # set message from callback self.set_msg_from_pos() # make the message tform.make_message(self._pub_msg, 3, 'slave', 0, self._payload) # publish the message self.pub.publish(self._pub_msg) # debug # print type(self._rel) loop_rate.sleep()
def start(self): rospy.loginfo("HANDIDLE") loop_rate = rospy.Rate(_RATE) # # signal flag for running threads # run_event = threading.Event() # run_event.set() # # # thread that changes blinks # move_blink = threading.Thread(target = self.blink, args = \ # (2, run_event)) # # move_blink.start() while not rospy.is_shutdown(): self._count += 1 # print self._count # do rythem self.rythem_d(self._count) #print self._physical #print self._eyelid # make message self.make_message() self.make_message_a() # tform method use hand r, I want to see difference tform.make_message(self._pub_msg_r, 1, 'handr', 9, self._payload) tform.make_message(self._pub_msg_ra, 4, 'handr', 9, self._payload_r) # publish self.pub.publish(self._pub_msg) self.pub_a.publish(self._pub_msg_a) self.pub.publish(self._pub_msg_r) self.pub_a.publish(self._pub_msg_a) if self._count > 15 * _RATE: #every 10 seconds self._count = 0 self._bias = int(3 * _RATE * rd.rand()) #bias within 2 seconds loop_rate.sleep()
def start(self): rospy.loginfo("Joystick") loop_rate = rospy.Rate(_RATE) while not rospy.is_shutdown(): # make payload self._payload[0] = 0 self._payload[1] = 0 self._payload[2] = int(self._speed_right * 350) # right self._payload[3] = int(self._speed_left * 350) # left if self._break == 1: self._payload[4] = 0 else: self._payload[4] = 100 # break print("left", self._payload) tform.make_message(self._pub_msg, 3, 'wheel', 4, self._payload) self.pub.publish(self._pub_msg) loop_rate.sleep()
def start(self): loop_rate = rospy.Rate(_RATE) while not rospy.is_shutdown(): self._payload_neck = self._neck self._payload_headc = self._headc self._payload_hip = self._hip tform.make_message(self._pub_neck, 4, 'neck', 2, self._payload_neck) self.pub.publish(self._pub_neck) tform.make_message(self._pub_headc, 4, 'headc', 2, self._payload_headc) self.pub.publish(self._pub_headc) tform.make_message(self._pub_hip, 4, 'hip', 2, self._payload_hip) self.pub.publish(self._pub_hip) #print(self._payload_neck) print self._payload_hip loop_rate.sleep()
def pub_pos(self, msg): self.counter += 1 if self.counter % 10 != 0: return print(rospy.Time.now()) count = msg.scan_time / msg.time_increment upper_angle1 = 218.5 upper_angle2 = 180 upper_angle3 = 270 upper_angle4 = 135 lower_angle1 = 180 lower_angle2 = 141 lower_angle3 = 225 lower_angle4 = 90 s_range = 0.40 l_range = 1.5 l_range2 = 0.80 thr1 = 0.20 thr2 = 0.20 thr3 = 0.10 thr4 = 0.35 thr5 = 0.35 ibegin = 0 iend = 0 num = int(0) total = float(0) total_angle = int(0) near_point = int(0) minimum_list_x = [] minimum_list_y = [] pair_minimum_x = [] pair_minimum_y = [] for i in range(int(count)): if s_range < msg.ranges[i] < l_range2: angle = msg.angle_increment * i * 180.0 / 3.14159265358979 if (lower_angle1 < angle < upper_angle1) or ( lower_angle2 < angle <= upper_angle2) or ( lower_angle3 < angle <= upper_angle3) or ( lower_angle4 < angle <= upper_angle4): if (i > 1) and (i < int(count) - 1): if (msg.ranges[i - 1] - msg.ranges[i] > thr1): ibegin = i iend = i self.pos_r1 = msg.ranges[i] self.pos_theta1 = angle self.pos_x1 = -1 * self.pos_r1 * math.cos( self.pos_theta1 * 3.14159265358979 / 180.0) self.pos_y1 = -1 * self.pos_r1 * math.sin( self.pos_theta1 * 3.14159265358979 / 180.0) if msg.ranges[i + 1] - msg.ranges[i] > thr2: iend = i self.pos_r2 = msg.ranges[i] self.pos_theta2 = angle self.pos_x2 = -1 * self.pos_r2 * math.cos( self.pos_theta2 * 3.14159265358979 / 180.0) self.pos_y2 = -1 * self.pos_r2 * math.sin( self.pos_theta2 * 3.14159265358979 / 180.0) dist = ((self.pos_x2 - self.pos_x1)**2 + (self.pos_y2 - self.pos_y1)**2)**(1.0 / 2.0) if (thr3 < dist < thr4): minimum_list_x.append( (self.pos_x2 + self.pos_x1) / 2.0) minimum_list_y.append( (self.pos_y2 + self.pos_y1) / 2.0) elif l_range2 < msg.ranges[i] < l_range: angle = msg.angle_increment * i * 180.0 / 3.14159265358979 if (lower_angle1 < angle < upper_angle1) or ( lower_angle2 < angle <= upper_angle2): if (i > 1) and (i < int(count) - 1): if (msg.ranges[i - 1] - msg.ranges[i] > thr1): ibegin2 = i iend2 = i self.pos_r1 = msg.ranges[i] self.pos_theta1 = angle self.pos_x1 = -1 * self.pos_r1 * math.cos( self.pos_theta1 * 3.14159265358979 / 180.0) self.pos_y1 = -1 * self.pos_r1 * math.sin( self.pos_theta1 * 3.14159265358979 / 180.0) if msg.ranges[i + 1] - msg.ranges[i] > thr2: iend2 = i self.pos_r2 = msg.ranges[i] self.pos_theta2 = angle self.pos_x2 = -1 * self.pos_r2 * math.cos( self.pos_theta2 * 3.14159265358979 / 180.0) self.pos_y2 = -1 * self.pos_r2 * math.sin( self.pos_theta2 * 3.14159265358979 / 180.0) dist = ((self.pos_x2 - self.pos_x1)**2 + (self.pos_y2 - self.pos_y1)**2)**(1.0 / 2.0) if (thr3 < dist < thr4): minimum_list_x.append( (self.pos_x2 + self.pos_x1) / 2.0) minimum_list_y.append( (self.pos_y2 + self.pos_y1) / 2.0) if len(minimum_list_x) >= 2: for i in range(len(minimum_list_x) - 1): if ((minimum_list_x[i] - minimum_list_x[i + 1])**2 + (minimum_list_y[i] - minimum_list_y[i + 1])**2)**( 1.0 / 2.0) < thr5: pair_minimum_x.append( (minimum_list_x[i] + minimum_list_x[i + 1]) / 2.0) pair_minimum_y.append( (minimum_list_y[i] + minimum_list_y[i + 1]) / 2.0) minimum_pos = PoseStamped() minimum_pos.header.stamp = rospy.Time.now() minimum_pos.header.frame_id = 'laser' minimum_pos.pose.position.x = pair_minimum_x[0] minimum_pos.pose.position.y = pair_minimum_y[0] if pair_minimum_y[0] < 0: minimum_pos.pose.position.y = pair_minimum_y[0] + 0.4 else: minimum_pos.pose.position.y = pair_minimum_y[0] - 0.4 minimum_pos.pose.position.z = 0.0 q = quaternion_from_euler(0, 0, 0) minimum_pos.pose.orientation = Quaternion(*q) self.minimum_pub.publish(minimum_pos) angle_temp = math.atan2(pair_minimum_y[0], pair_minimum_x[0]) if angle_temp > 0: angle_send = int( ((-angle_temp + math.pi) / math.pi) * 180) else: angle_send = int( ((-angle_temp - math.pi) / math.pi) * 180) self.direction_pub.publish(angle_send) self._payload_headc[2] = -self.eye_gaze self._payload_headc[3] = -self.eye_gaze tform.make_message(self._pub_headc, 4, 'headc', 2, self._payload_headc) self.pub.publish(self._pub_headc) # print 'x:',minimum_list_x # print 'y:',minimum_list_y elif len(minimum_list_x) > 0: minimum_pos = PoseStamped() minimum_pos.header.stamp = rospy.Time.now() minimum_pos.header.frame_id = 'laser' minimum_pos.pose.position.x = minimum_list_x[0] minimum_pos.pose.position.y = minimum_list_y[0] # if minimum_list_y[0] < 0: # minimum_pos.pose.position.y = minimum_list_y[0] + 0.4 # else: # minimum_pos.pose.position.y = minimum_list_y[0] - 0.4 minimum_pos.pose.position.z = 0.0 q = quaternion_from_euler(0, 0, 0) minimum_pos.pose.orientation = Quaternion(*q) self.minimum_pub.publish(minimum_pos) angle_temp = math.atan2(minimum_list_y[0], minimum_list_x[0]) angle_send = 180 if angle_temp > 0: angle_send = int(((-angle_temp + math.pi) / math.pi) * 180) else: angle_send = int(((-angle_temp - math.pi) / math.pi) * 180) self.direction_pub.publish(angle_send) self._headc[2] = -self.eye_gaze self._headc[3] = -self.eye_gaze tform.make_message(self._pub_headc, 4, 'headc', 2, self._payload_headc) self.pub.publish(self._pub_headc) else: minimum_pos = PoseStamped() minimum_pos.header.stamp = rospy.Time.now() minimum_pos.header.frame_id = 'laser' minimum_pos.pose.position.x = 0 minimum_pos.pose.position.y = 0 minimum_pos.pose.position.z = 0.0 q = quaternion_from_euler(0, 0, 0) minimum_pos.pose.orientation = Quaternion(*q) self.minimum_pub.publish(minimum_pos)
position, addr = fb_client.recvfrom(4096) payload_p = [] payload_c = [] # split the position and currents pac = position.split(',') p_pos = pac[0] p_cur = pac[1] for idx in range(0, 5): payload_p.append(int(p_pos[idx * 5:(idx + 1) * 5])) for idx in range(0, 5): payload_c.append(int(p_cur[idx * 5:(idx + 1) * 5]) * 2 - 10000) # make message tform.make_message(pub_msg_p, 2, dev_name, 3, payload_p) tform.make_message(pub_msg_c, 2, dev_name, 4, payload_c) #print payload_p #print payload_c print position # publish pub_p.publish(pub_msg_p) pub_c.publish(pub_msg_c) rate.sleep() # If the value cannot be obtain, try again until get the value __version = "1.1.0"
(rtclient,cur_client, 'h', run_event, joint)) move_t.start() #--------------------------------------------------------------------------- while not rospy.is_shutdown(): "generate one time message" otm = tform.merge(joint._payload) print otm #--------------------------------------------------------------------------- try: "UDP send launch" motorsock.sendto(otm, (ip[dev_name], port[dev_name])) except socket.error as error: if error.errno == errno.ENETUNREACH: rospy.WARN("connection to mbed lost.") else: raise if dev_name == 'wheel': tform.make_message(pub_msg, 2, dev_name, 2, joint._payload_w) pub.publish(pub_msg) #--------------------------------------------------------------------------- rate.sleep() if dev_name == 'wheel': move_t.join() __version = "1.1.0"
"generate one time message" otm = tform.merge(joint._payload) #--------------------------------------------------------------------------- try: "UDP send launch" motorsock.sendto(otm, (ip[dev_name], port[dev_name])) except socket.error as error: if error.errno == errno.ENETUNREACH: rospy.WARN("connection to mbed lost.") else: raise if dev_name == 'wheel': pub_msg = make_message(2, dev_name, 2, joint._payload_w) pub.publish(pub_msg) # make some message tform.make_message(joint._pub_msg_p, 2, dev_name, 3, joint._payload_p) tform.make_message(joint._pub_msg_c, 2, dev_name, 4, joint._payload_c) pub_p.publish(joint._pub_msg_p) pub_c.publish(joint._pub_msg_c) #--------------------------------------------------------------------------- rate.sleep() move_t.join() __version = "1.0.1"