def property_changed(interface, changed, invalidated, path): iface = interface[interface.rfind(".") + 1:] for name, value in changed.iteritems(): val = str(value) #print(name + "=" + val) if name == 'Connected': if val == "1": print("ON") message = String() message.data = "gpio,w,on" send(message) elif val == "0": print("OFF") message = String() message.data = "gpio,w,off" send(message) advertise() else: pass elif name == 'Alias' or name == 'Name': print("ON") message = String() message.data = "gpio,w,on" send(message) else: pass
def execute(self, userdata): #self.ON_pub.publish(Empty()) rospy.sleep(120) my_string=String() my_string.data="Il est l'heure de se lever. Debout les feignants." self.french_pub.publish(my_string) rospy.sleep(480) self.ON2_pub.publish(Empty()) rospy.sleep(120) my_string.data="Chauffage de la salle de bain en cours." self.french_pub.publish(my_string) rospy.sleep(0.01) self.heatON_pub.publish(Empty()) rospy.sleep(0.01) self.music_pub.publish(Empty()) rospy.sleep(60) self.weather_pub.publish(Empty()) rospy.sleep(240) self.heatOFF_pub.publish(Empty()) return 'succeeded'
def execute(self, ud): tts_msg = String() tts_msg.data = 'Do you want me to start guiding you back to the arena?' self.tts_pub.publish(tts_msg) self.question_asked = True return_param = '' while True: self.mutex.acquire() if self.confirmation_received: if self.start_guiding: tts_msg.data = "I will guide you back to the arena." self.tts_pub.publish(tts_msg) try: self.people_follower_srv.call(request=peopleFollowerRequest.RELEASE_TARGET) except rospy.ServiceException: pass return_param = 'start_guiding' else: return_param = 'do_not_guide' self.mutex.release() break self.mutex.release() return return_param
def execute(self, ud): tts_msg = String() tts_msg.data = 'Do you want me to continue following you?' self.tts_pub.publish(tts_msg) self.question_asked = True return_param = '' while True: self.mutex.acquire() if self.confirmation_received: if self.continue_following: tts_msg.data = "I will continue following you." self.tts_pub.publish(tts_msg) try: self.people_follower_srv.call(request=peopleFollowerRequest.START_FOLLOWING) except rospy.ServiceException: pass return_param = 'continue_following' else: tts_msg.data = "I will stop following you." self.tts_pub.publish(tts_msg) return_param = 'ask_guiding' self.mutex.release() break self.mutex.release() return return_param
def decision_function(self): last_three = self.engagement_history[-3:] mean = sum(last_three)/3 # Let at least do 3 trials in 5 min with a bad mean #if mean < 10 and self.current_time > 300 and np.size(self.engagement_history) > 3: # if self.current_time > 36000000: # self.engagement_history = self.engagement_history[0:-3] # msg = String() # if self.activity_turn: # msg.data = "drawing_nao" # #self.activity_turn = False # else: # pass # #msg.data = "joke_nao" # #self.activity_turn = True # self.pub_activity.publish(msg) msg = String() if self.nb_repetitions == 14: msg.data = "drawing_nao" #self.activity_turn = False self.pub_activity.publish(msg) if self.nb_repetitions == 7: msg.data = "drawing_nao" #self.activity_turn = False self.pub_activity.publish(msg)
def run(self): self.speechRecogProxy.setLanguage("English") wordList = ["come", "go", "stand", "sit", "relax", "stiffen"] self.speechRecogProxy.setWordListAsVocabulary(wordList) self.speechRecogProxy.subscribe("ros") self.dataNamesList = ["WordRecognized"] self.recognizedWordPub = rospy.Publisher("recognized_word", String) self.textToSpeechPub = rospy.Publisher("speech", String) r = rospy.Rate(5) prev_word = str() while not rospy.is_shutdown(): try: memoryData = self.memoryProxy.getListData(self.dataNamesList) except RuntimeError,e: rospy.logerr("Error accessing ALMemory: %s", e) rospy.signal_shutdown("No NaoQI available anymore") if (memoryData[0][1] >= 0.2) and (memoryData[0][0] != prev_word): msg = String() msg.data = memoryData[0][0] self.recognizedWordPub.publish(msg) msg = String() msg.data = "Oh kay." self.textToSpeechPub.publish(msg) prev_word = memoryData[0][0] # reset if memoryData[0][0] == '': prev_word = '' r.sleep()
def connect_with_arduino(): global send_str port = rospy.get_param('steer_motor_port',"/dev/ttyUSB0") pub = rospy.Publisher('motor_controller_input', String, queue_size=10) pub_str = String() print port try: ser = serial.Serial(port,9600) ser.setDTR(False) time.sleep(1) ser.setDTR(True) print "start connection with steer_motor" except: print "cannot start connection with steer_motor" sys.exit() time.sleep(2) rate = rospy.Rate(100) count = 0 while count < 4: count += 1 send_zero_to_steer_motor(ser) rate.sleep() print "Did You Turn On a Motor Switch??" wait_count = 0 while not wait_count > 200: ser.write("EN\n") ser.write("NP\n") while ser.inWaiting() > 0: if ser.read() == "p": ser.write("EN\n") send_devision_to_steer_motor(ser) pub_str.data = send_str[:-1] pub.publish(pub_str) rate.sleep() wait_count = wait_count + 1 print "Control Start" while not rospy.is_shutdown(): if enable_motor == True: ser.write("NP\n") while ser.inWaiting() > 0: if ser.read() == "p": ser.write("EN\n") send_devision_to_steer_motor(ser) pub_str.data = send_str[:-1] pub.publish(pub_str) else: ser.write("DI\n") rate.sleep() ser.close()
def publishSimulatedFeedback(bestShape_index, shapeType_index, doGroupwiseComparison): feedback = String(); if(doGroupwiseComparison): feedback.data = str(shapeType_index) + '_' + str(bestShape_index); else: names = ('old', 'new'); feedback.data = names[bestShape_index]; onFeedbackReceived(feedback);
def cmdCallback(data): p = String() p.data = ">SVLX:"+str(int(data.linear.x * rate / .00004871 / 1000 / 2)) #Each raw v-velocity tick = 48.71 um per (time interval between messages) pub_cmd.publish(p) p.data = ">SVAZ:"+str(int(data.angular.z * rate / .00019482 / 1000/ 2)) #Each raw w-velocity tick = 194.82 urad per (time interval between messages) pub_cmd.publish(p)
def callback_sub_msg(self, msg): try: symbol = String() symbol.data = chr(msg.data) except ValueError: symbol.data = "Symbol not exist!" self.pub.publish(symbol)
def _on_approach_pressed(self): str = String() str.data = "getposition" self._publishersys.publish(str) d = rospy.Duration(0.2,0) rospy.sleep(d) str.data = "approach_victim" self._publishersys.publish(str) self._widget.victim_label.setText('Approach Victim..')
def execute(self, userdata): my_string = String() my_string.data = "Start charging." self.charge_pub.publish(Empty()) self.string_pub.publish(my_string) rospy.loginfo("Charging... Sleep 2 hours") rospy.sleep(2.1*60.0*60.0) my_string.data = "Charge done." self.string_pub.publish(my_string) return 'succeeded'
def _on_mode_pressed(self): str = String() if RobotSteering.mode : self._widget.mode_push_button.setText('Auto_Mode') RobotSteering.mode = False str.data = "teleop_mode" else : self._widget.mode_push_button.setText('Teleop_Mode') RobotSteering.mode = True str.data = "auto_mode" self._publishersys.publish(str)
def _on_sensor_pressed(self): str = String() if RobotSteering.modesensor : self._widget.sensor_button.setText('Stop_Sensor') RobotSteering.modesensor = False str.data = "start_sensor" else : self._widget.sensor_button.setText('Start_Sensor') RobotSteering.modesensor = True str.data = "stop_sensor" self._publishersys.publish(str)
def demo(): rospy.init_node('Case2016_Demo') pub = rospy.Publisher('CModelRobotOutput', outputMsg.CModel_robot_output) pub_arm = rospy.Publisher('/ur_driver/URScript', String) rospy.Subscriber("/hands", Hands, h_call) rospy.Subscriber("/faces", Faces, f_call) rospy.Subscriber("/bluetooth_teleop/joy", Joy, joy_call) command = outputMsg.CModel_robot_output(); pub.publish(command) #Reset command.rACT = 1 command.rGTO = 1 command.rSP = 255 command.rFR = 75 pub.publish(command) #Activate # Arm Control val = String() arm_array = [-3.84, -2.1, 1.22, 0.61, 1.57, 0] while not rospy.is_shutdown(): if safe == 1: #Autonomous mode print "Auto" command.rPR = state pub.publish(command) global target arm_array[0] = arm_array[0] + target[0]*0.001 arm_array[2] = arm_array[2] + target[1]*0.001 target = [0,0] val.data = "movej(" + str(arm_array) +", a=0.1, v=0.1)" pub_arm.publish(val) print val.data if safe == 2: #Manual mode print "Manual" scale = 0.1 if joy.buttons[12] == 1: command.rPR = 0 if joy.buttons[14] == 1: command.rPR = 255 if joy.buttons[11] == 1: arm_array = home[:] pub.publish(command) arm_array[0] = arm_array[0] + joy.axes[0]*scale #base angle value of arm, left analog stick arm_array[1] = arm_array[1] + joy.axes[1]*scale #shoulder value of arm, left analog stick arm_array[2] = arm_array[2] + joy.axes[2]*scale #elbow value of arm, right analog stick arm_array[3] = arm_array[3] + joy.axes[3]*scale #wrist1 value of arm, right analog stick #arm_array[4] = arm_array[4] + joy.axes[3]*scale #wrist2 value of arm, right analog stick #arm_array[5] = arm_array[5] + joy.axes[3]*scale #wrist3 value of arm, right analog stick val.data = "movej(" + str(arm_array) +", a=0.5, v=1.0)" pub_arm.publish(val) print val.data rospy.sleep(0.8)
def output(): global out global select pub = rospy.Publisher('psoc_cmd',String) while not rospy.is_shutdown(): p = String() if select is LEFT: p.data = ">FLPC:"+str(out) elif select is RIGHT: p.data = ">FRPC:"+str(out) pub.publish(p) time.sleep(.1)
def output(): global leftOut global rightOut pub = rospy.Publisher('psoc_cmd',String) while not rospy.is_shutdown(): p = String() p.data = ">SPLM:"+str(leftOut) pub.publish(p) p = String() p.data = ">SPRM:"+str(rightOut) pub.publish(p) time.sleep(.1)
def find_color(self, passed_im, label_color, mask): im = passed_im.copy() if self.isTesting: self.image = im contours = cv2.findContours(mask, cv2.cv.CV_RETR_TREE, cv2.cv.CV_CHAIN_APPROX_SIMPLE)[0] approx_contours = [] for c in contours: area = cv2.contourArea(c) if area < 500: continue perim = cv2.arcLength(c, True) approx = cv2.approxPolyDP(c, .02*perim, True) if len(approx) == 4: # rectangle approx_contours.append(approx) rect_msg = String() rect_msg.data = "{} rectangle".format(label_color) self.pub_blobs.publish(rect_msg) moments = cv2.moments(c) center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00'])) im = passed_im.copy() cv2.putText(im, "{} rectangle".format(label_color), center, cv2.FONT_HERSHEY_PLAIN, 2, (100, 255, 100)) cv2.drawContours(im, [approx], -1, (100, 255, 100), 2) cv2.imwrite("/home/racecar/challenge_photos1/{}rectangle{}.png".format(label_color, int(time.clock()*1000)), im) print "{}square".format(label_color) elif abs(len(approx)-12) <= 1: # cross approx_contours.append(approx) cross_msg = String() cross_msg.data = "{} cross".format(label_color) self.pub_blobs.publish(cross_msg) moments = cv2.moments(c) center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00'])) im = passed_im.copy() cv2.putText(im, "{} cross".format(label_color), center, cv2.FONT_HERSHEY_PLAIN, 2, (100, 255, 100)) cv2.drawContours(im, [approx], -1, (100, 255, 100), 2) cv2.imwrite("/home/racecar/challenge_photos1/{}cross{}.png".format(label_color, int(time.clock()*1000)), im) print "{}cross".format(label_color) elif abs(len(approx)-8) <= 2: # circle approx_contours.append(approx) circ_msg = String() circ_msg.data = "{} circle".format(label_color) self.pub_blobs.publish(circ_msg) moments = cv2.moments(c) center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00'])) im = passed_im.copy() cv2.putText(im, "{} circle".format(label_color), center, cv2.FONT_HERSHEY_PLAIN, 2, (100, 255, 100)) cv2.drawContours(im, [approx], -1, (100, 255, 100), 2) cv2.imwrite("/home/racecar/challenge_photos1/{}circle{}.png".format(label_color, int(time.clock()*1000)), im) print "{}circle".format(label_color) if self.isTesting: cv2.drawContours(self.image, approx_contours, -1, (100, 255, 100), 2)
def visCallback(data): if((data.data=="1")): ser.write("sta") pub_msg = String() pub_msg.data = "0" global pub pub.publish(pub_msg) time.sleep(2*0.80) ser.write("sto") time.sleep(1) pub_msg = String() pub_msg.data = "1" pub.publish(pub_msg) time.sleep(0.2)
def onChangeHumanTarget(msg): global last_human_target if "_/" in str(msg.data): current_human_target = str(msg.data).split("_/")[1] if current_human_target != last_human_target: target = String() target.data = current_human_target pub_human_target.publish(target) # it also defines an action of the human: human_action = "looks_"+current_human_target action = String() action.data = human_action pub_human_action.publish(action) last_human_target = current_human_target
def talker(): pub = rospy.Publisher('sub_dialog',protocol_dialogue, queue_size=10) rospy.init_node('tf_transforms') rate = rospy.Rate(1) tef = protocol_dialogue() stre = String() stre2= String() stre.data = "human" stre2.data = "Go right of that tree05" tef.agent = stre tef.command = stre2 rospy.loginfo(tef) pub.publish(tef) rate.sleep()
def _on_ignor_pressed(self): str = String() str.data = "ignor" self._publishersys.publish(str) d = rospy.Duration(0.5,0) rospy.sleep(d) str.data = "go" self._publishersys.publish(str) #self._widget.groupBox.setEnabled(False) self._widget.comfirm_button.setEnabled(True) self._widget.approach_victim_button.setEnabled(True) self._widget.ignor_button.setEnabled(True) self._widget.current_z_angular_label.setEnabled(False) self._widget.label.setEnabled(False) self._widget.victim_label.setText('Searching...')
def sendCommandToRobot(self, data): splitData = data.split(":") targetRobot = splitData[0] command = splitData[1][1:] if targetRobot == "All": for (robot_name, publisher) in self._robotDict.iteritems(): msg = StringMsg() msg.data = command publisher.publish(msg) else: publisher = self._robotDict[targetRobot] msg = StringMsg() msg.data = command publisher.publish(msg)
def __update_status_to_wp(self, status, address): ''' update task control status to waypoint_user_pub params: status: task status: -1 canceled 0 added to tasks queue, red light on 1 executing 3 completed, red light off address: current task's address ''' # address = task.address # msg = String() # status = str(address) + ':' + str(status) # msg.data = status # self.__feedback_pub.publish(msg) # pub task status to rosbridge_driver if status == 0: light_status = 1023 elif status == 3: light_status = 0 else: return msg = String() msg.data = 'light:' + str(address) + ',' + str(light_status) self.__feedback_pub.publish(msg)
def execute(self, userdata): tosay = String() myint = Int32() tosay.data = "Bonne nuit." self.french_pub.publish(tosay) rospy.sleep(0.01) myint.data = 1 self.brightness1_pub.publish(myint) rospy.sleep(0.01) myint.data = 6 self.brightness2_pub.publish(myint) rospy.sleep(0.01) myint.data = 6 self.brightness3_pub.publish(myint) rospy.sleep(0.3) # Shutdown everything self.OFF3_pub.publish(Empty()) rospy.sleep(0.01) self.OFF2_pub.publish(Empty()) rospy.sleep(0.01) self.OFF1_pub.publish(Empty()) rospy.sleep(0.01) self.heatOFF_pub.publish(Empty()) rospy.sleep(0.01) return 'succeeded'
def callback_thread(data,time): global res global test_var global thread1 if checker == "true": if data.data != "SWITCH": result = data.data if result == "COMEBACK": result="COME BACK" elif result == "TAKEPICTURE": result = "TAKE PICTURE" elif result == "SCANFOREST": result = "SCAN FOREST" elif result == "SCANAREA": result = "SCAN AREA" elif result == "TAKEOFF": result="TAKE OFF" elif result == "NEXTTO": result="NEXT" print "hello" window.insert(INSERT,'Genius: ','hcolor') window.insert(END,result.upper()+'\n','hnbcolor') string = String() string.data = result.upper() result = result.lower() res = result thread1 = res thread.start_new_thread(sleeping_time, (res,5,)) thread.start_new_thread(compare_thread, (res,1,))
def test_multiple_subscribers(self): topic = "/test_subscribe_receive_json" msg_type = "std_msgs/String" client1 = "client_test_subscribe_receive_json_1" client2 = "client_test_subscribe_receive_json_2" msg = String() msg.data = "dsajfadsufasdjf" pub = rospy.Publisher(topic, String) multi = MultiSubscriber(topic, msg_type) received = {"msg1": None, "msg2": None} def cb1(msg): received["msg1"] = msg def cb2(msg): received["msg2"] = msg multi.subscribe(client1, cb1) multi.subscribe(client2, cb2) sleep(0.5) pub.publish(msg) sleep(0.5) self.assertEqual(msg.data, received["msg1"]["data"]) self.assertEqual(msg.data, received["msg2"]["data"])
def read_and_pub_yaml(pub, path): if not rospy.is_shutdown(): msg = String() with open(path, 'r') as myfile: msg.data=myfile.read() rospy.loginfo("publishing goal: %s", path) pub.publish(msg)
def final_result(self, hyp, uttid): """ Insert the final result. """ rospy.loginfo("final_result publish!!!!") msg = String() msg.data = str(hyp.lower()) rospy.loginfo(msg.data) self.pub.publish(msg)
def execute(self, userdata): global jo_status global carole_status jo_status = 1 carole_status = 1 tosay = String() myint = Int32() tosay.data = "Il est l'heure d'aller dormir." self.french_pub.publish(tosay) rospy.sleep(0.01) self.ON3_pub.publish(Empty()) rospy.sleep(0.01) self.ON2_pub.publish(Empty()) rospy.sleep(0.01) self.ON1_pub.publish(Empty()) rospy.sleep(0.01) myint.data = 175 self.color1_pub.publish(myint) rospy.sleep(0.01) self.color3_pub.publish(myint) rospy.sleep(0.01) myint.data = 2 self.brightness1_pub.publish(myint) rospy.sleep(0.01) myint.data = 10 self.brightness2_pub.publish(myint) rospy.sleep(0.01) myint.data = 9 self.brightness3_pub.publish(myint) rospy.sleep(0.01) return 'succeeded'
def pr2_mover(object_list): # TODO: Initialize variables dict_list = [] centroids = [] # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') # TODO: Parse parameters into individual variables dict_dropbox = {} for p in dropbox_param: dict_dropbox[p['name']] = p['position'] # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list for obj in object_list_param: # TODO: Get the PointCloud for a given object and obtain it's centroid object_name = String() object_name.data = obj['name'] print(object_name) pick_pose = Pose() pick_pose.position.x = 0 pick_pose.position.y = 0 pick_pose.position.z = 0 #set orientation to 0 pick_pose.orientation.x = 0 pick_pose.orientation.y = 0 pick_pose.orientation.z = 0 pick_pose.orientation.w = 0 #set place pose orientation to 0 place_pose = Pose() place_pose.orientation.x = 0 place_pose.orientation.y = 0 place_pose.orientation.z = 0 place_pose.orientation.w = 0 # TODO: Create 'place_pose' for the object for detected_object in object_list: if detected_object.label == object_name.data: print("set pose for " + object_name.data) points_arr = ros_to_pcl(detected_object.cloud).to_array() pick_pose_np = np.mean(points_arr, axis=0)[:3] pick_pose.position.x = np.asscalar(pick_pose_np[0]) pick_pose.position.y = np.asscalar(pick_pose_np[1]) pick_pose.position.z = np.asscalar(pick_pose_np[2]) break # TODO: Assign the arm to be used for pick_place arm_name = String() if obj['group'] == 'red': arm_name.data = 'left' elif obj['group'] == 'green': arm_name.data = 'right' else: print "assign the arm error" # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format test_scene_num = Int32() test_scene_num.data = 3 place_pose.position.x = dict_dropbox[arm_name.data][0] place_pose.position.y = dict_dropbox[arm_name.data][1] place_pose.position.z = dict_dropbox[arm_name.data][2] dict_list.append( make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)) # Wait for 'pick_place_routine' service to come up #rospy.wait_for_service('pick_place_routine') #print("pick_place") #try: # pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request # resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) # print ("Response: ",resp.success) #except rospy.ServiceException, e: # print "Service call failed: %s"%e # TODO: Output your request parameters into output yaml file yaml_filename = "output_" + str(test_scene_num.data) + ".yaml" send_to_yaml(yaml_filename, dict_list)
def pr2_mover(object_list): print('start mover function') global max_success_count # TODO: Initialize variables test_scene_num = Int32() arm_name = String() object_name = String() object_group = String() pick_pose = Pose() place_pose = Pose() dropbox_dict = {} dict_list = [] pick_pose_point = Point() place_pose_point = Point() success_count = 0 # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') # Get scene number from launch file test_scene_num.data = rospy.get_param('/test_scene_num') # print("test_scene_num = %d"% test_scene_num.data) # Get dropbox parameters dropbox_param = rospy.get_param('/dropbox') # TODO: Parse parameters into individual variables for dropbox in dropbox_param: dropboxdata = dropbox_data(dropbox['position'], dropbox['name']) dropbox_dict[dropbox['group']] = dropboxdata # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list for obj in object_list_param: group = obj['group'] name = obj['name'] pos = object_list.get(name) # TODO: Get the PointCloud for a given object and obtain it's centroid if pos is not None: object_name.data = name pick_pose_point.x = np.asscalar(pos[0]) pick_pose_point.y = np.asscalar(pos[1]) pick_pose_point.z = np.asscalar(pos[2]) pick_pose.position = pick_pose_point # TODO: Create 'place_pose' for the object dropboxdata = dropbox_dict[group] place_pose_point.x = dropboxdata.pos[0] place_pose_point.y = dropboxdata.pos[1] place_pose_point.z = dropboxdata.pos[2] place_pose.position = place_pose_point # TODO: Assign the arm to be used for pick_place arm_name.data = dropboxdata.arm print( "Scene %d, picking up found %s object, using %s arm, and placing it in the %s bin." % (test_scene_num.data, object_name.data, arm_name.data, group)) # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) success_count = success_count + 1 else: print("Label: %s not found" % name) # Wait for 'pick_place_routine' service to come up # rospy.wait_for_service('pick_place_routine') # try: # pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request #resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) # resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose) # print("Response: ",resp.success) # except rospy.ServiceException, e: # print("Service call failed: %s"%e) # TODO: Output your request parameters into output yaml file if max_success_count < success_count: yaml_filename = "./output/output_" + str(test_scene_num.data) + ".yaml" print("output file name = %s" % yaml_filename) send_to_yaml(yaml_filename, dict_list) max_success_count = success_count print("Success picking up object number = %d" % success_count) return
def sendMove(self, string): st = String() st.data = string self.pub.publish(st)
def commandPub(self, s): cmdMsg = String() cmdMsg.data = s self.cmdPub.publish(cmdMsg)
def pr2_mover(object_list): # TODO: Initialize variables # resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) TEST_SCENE_NUM = Int32() OBJECT_NAME = String() WHICH_ARM = String() PICK_POSE = Pose() PLACE_POSE = Pose() # TODO: Get/Read parameters pick_list = [] # rospy.get_param return a dictionary include {'name' : , 'group': } object_list_param = rospy.get_param('/object_list') # TODO: Parse parameters into individual variables for i in range(0, len(object_list_param)): object_name = object_list_param[i]['name'] object_group = object_list_param[i]['group'] pick_list.append((object_name, object_group)) # TODO: Rotate PR2 in place to capture side tables for the collision map print('rotate PR2 in palce to capture side tables for the collision map') box_param = rospy.get_param('/dropbox') # red box in left side red_box_pos = box_param[0]['position'] # green box in right side green_box_pos = box_param[1]['position'] dict_list = [] for item, group in pick_list: TEST_SCENE_NUM.data = 3 centroids = find_centroid(object_list, item) OBJECT_NAME.data = item if group == 'green': WHICH_ARM.data = 'right' PLACE_POSE.position.x = green_box_pos[0] PLACE_POSE.position.y = green_box_pos[1] PLACE_POSE.position.z = green_box_pos[2] else: WHICH_ARM.data = 'left' PLACE_POSE.position.x = red_box_pos[0] PLACE_POSE.position.y = red_box_pos[1] PLACE_POSE.position.z = red_box_pos[2] if centroids is not None: PICK_POSE.position.x = np.asscalar(centroids[0]) PICK_POSE.position.y = np.asscalar(centroids[1]) PICK_POSE.position.z = np.asscalar(centroids[2]) else: PICK_POSE.position.x = 0 PICK_POSE.position.y = 0 PICK_POSE.position.z = 0 print('make yaml_dict') yaml_dict = make_yaml_dict(TEST_SCENE_NUM, WHICH_ARM, OBJECT_NAME, PICK_POSE, PLACE_POSE) dict_list.append(yaml_dict) print('send to yaml') send_to_yaml("output_lm3.yaml", dict_list) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) print("Response: ", resp.success) print("response get pick_place_routine and response success") except rospy.ServiceException, e: print('rospy serviece Exception') print "Service call failed: %s" % e
def FindKnobs(PointList): global Xaverage, Yaverage, Zaverage, CPoints, Xval, Yval, Zval ResetCalcValues() #Finding Handles Trimmed = [] for x in PointList: distance = pow(x[0]**2+x[1]**2+x[2]**2, 0.5) if distance < 1.0: Trimmed.append(x) if False: #For Verbosity print O.color.GREEN + "Point Trimmed:", len(Trimmed), O.color.END print O.color.GREEN + "Point Count: ", len(PointList) ,O.color.END for x in Trimmed: temp1 = np.greater_equal(x[3], Redcolor[0]) temp2 = np.less_equal(x[3], Redcolor[1]) temp3 = np.array_equal([True, True, True], temp1) and np.array_equal([True, True, True], temp2) if temp3: Zaverage[0].append(x[2]) #Yaverage[0].append(x[1]) #Xaverage[0].append(x[0]) temp1 = np.greater_equal(x[3], Bluecolor[0]) temp2 = np.less_equal(x[3], Bluecolor[1]) temp3 = np.array_equal([True, True, True], temp1) and np.array_equal([True, True, True], temp2) if temp3: Zaverage[1].append(x[2]) #Yaverage[1].append(x[1]) #Xaverage[1].append(x[0]) ZLTemp= np.average(Zaverage[0]) #Finding the average distance from Head to Wheel and using that to filter ZRTemp= np.average(Zaverage[1]) #Done for Left and Right, so if robot is not entirely aligned it should work #Xval = XTemp= np.average(Xaverage[0]) #Yval = YTemp= np.average(Yaverage[0]) ResetCalcValues() for x in Trimmed: temp1 = np.greater_equal(x[3], Greycolor[0]) temp2 = np.less_equal(x[3], Greycolor[1]) temp3 = np.array_equal([True, True, True], temp1) and np.array_equal([True, True, True], temp2) if x[3][0] == x[3][1] and x[3][2] == x[3][1] and temp3: ##Is grey and isn't white variance = 0.05 if x[0] >=0: #Left or right if x[2]<=(ZLTemp-0.07)*(1+variance) and x[2] >=(ZRTemp-0.090)*(1-variance): #Is closer than the average red/blue part and not too close (+-5%) Xaverage[0].append(x[0]) Yaverage[0].append(x[1]) Zaverage[0].append(x[2]) CPoints.append(x) else: if x[2]<=(ZRTemp-0.07)*(1+variance) and x[2] >=(ZRTemp-0.090)*(1-variance): Xaverage[1].append(x[0]) Yaverage[1].append(x[1]) Zaverage[1].append(x[2]) CPoints.append(x) if len(CPoints) < 200: print O.color.BOLD + O.color.RED + "FATAL PROBLEM" + O.color.END #ADD EXCEPTION #sys.exit(2) stuff = String() stuff.data = "PointList Less than 200!" print stuff.data conn.publish(stuff) PointCloudGen(CPoints) LeftPoint = [-np.average(Xaverage[0])+.03, -np.average(Yaverage[0]) , np.average(Zaverage[0])] # Left(-) or right(+), up down, forward backwards RightPoint = [-np.average(Xaverage[1])+0.03, -np.average(Yaverage[1]) , np.average(Zaverage[1])] ResetCalcValues() return LeftPoint, RightPoint
def pr2_mover(object_list): labels = [] centroids = [] # to be list of tuples (x, y, z) for object in object_list: labels.append(object.label) # Get the PointCloud for a given object and obtain it's centroid points_arr = ros_to_pcl(object.cloud).to_array() centroid = np.mean(points_arr, axis=0)[:3] centroids.append(centroid) dropboxes = [] dropbox_list_param = rospy.get_param('/dropbox') #for dropbox_param in dropbox_list_param # dropboxes.append([dropbox_param['name'], dropbox_param['group'], dropbox_param['position']) object_list_param = rospy.get_param('/object_list') yaml_dicts = [] test_scene_num = Int32() test_scene_num.data = 3 for object_param in object_list_param: object_name = String() object_name.data = object_param['name'] object_group = object_param['group'] # TODO: Create 'place_pose' for the object # find the object in the labels list: i = labels.index(object_name.data) object_centroid = centroids[i] pick_pose = Pose() pick_pose.position.x = np.asscalar(object_centroid[0]) pick_pose.position.y = np.asscalar(object_centroid[1]) pick_pose.position.z = np.asscalar(object_centroid[2]) place_pose = Pose() for dropbox in dropbox_list_param: if(dropbox['group'] == object_group): place_pose.position.x = dropbox['position'][0] place_pose.position.y = dropbox['position'][1] place_pose.position.z = dropbox['position'][2] # TODO: Assign the arm to be used for pick_place arm_name = String() if object_group == 'green': arm_name.data = 'right' else: arm_name.data = 'left' # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dicts.append(make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)) # Wait for 'pick_place_routine' service to come up #rospy.wait_for_service('pick_place_routine') #try: # pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request # resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose) # print ("Response: ",resp.success) # except rospy.ServiceException, e: # print "Service call failed: %s"%e # TODO: Initialize variables # TODO: Get/Read parameters # TODO: Parse parameters into individual variables # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list # TODO: Output your request parameters into output yaml file send_to_yaml('output3.yml', yaml_dicts)
def pr2_mover(object_list): # TODO: Initialize variables output_dict_list = [] # Create a dictionary of detected objects for easy lookup of point cloud using label detected_obj_dict = {} for do in object_list: detected_obj_dict[do.label] = do.cloud # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') # TODO: Parse parameters into individual variables # Create a dictionary of dropbox for easy lookup of position using group as key dropbox_dict = {} for box in dropbox_param: dropbox_dict[box['group']] = box['position'] # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list for pl_obj in object_list_param: # Pick list object values pl_obj_name = pl_obj['name'] pl_obj_group = pl_obj['group'] # Skip if the object is not in the list of detected objects if pl_obj_name not in detected_obj_dict: continue # Set the ROS test scene and object name parameters test_scene_num = Int32() test_scene_num.data = 1 object_name = String() object_name.data = pl_obj_name # TODO: Get the PointCloud for a given object and obtain it's centroid cloud = detected_obj_dict[pl_obj_name] points_arr = ros_to_pcl(cloud).to_array() centroid = np.mean(points_arr, axis=0)[:3] # Create 'pick_pose' and set position by recasting centroid to python float pick_pose = Pose() pick_pose.position.x = np.asscalar(centroid[0]) pick_pose.position.y = np.asscalar(centroid[1]) pick_pose.position.z = np.asscalar(centroid[2]) # TODO: Create 'place_pose' for the object # Set it's position to the corresponding dropbox group position place_pose = Pose() place_pose.position.x = dropbox_dict[pl_obj_group][0] place_pose.position.y = dropbox_dict[pl_obj_group][1] place_pose.position.z = dropbox_dict[pl_obj_group][2] # TODO: Assign the arm to be used for pick_place arm_name = String() if pl_obj_group == 'green': arm_name.data = 'right' else: arm_name.data = 'left' # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) output_dict_list.append(yaml_dict) """ # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) print ("Response: ",resp.success) except rospy.ServiceException, e: print "Service call failed: %s"%e """ # TODO: Output your request parameters into output yaml file send_to_yaml('../config/output_' + str(test_scene_num.data) + '.yaml', output_dict_list)
# handle kill command # def signal_handler(sig, frame): # print('Stopping .....') # sys.exit(0) if __name__ == '__main__': rospy.init_node('publisher_node') rospy.loginfo("Publisher has been started") # Create a publisher pub = rospy.Publisher("/publisher_topic", String, queue_size=10) rate = rospy.Rate(2) print("Created /publisher_topic. Starting to publish messages...") # while the node is running (not shutdown) i = 1 try: while not rospy.is_shutdown(): msg = String() msg.data = "News radio info .. {}".format(i) pub.publish(msg) print("..published message .. {}".format(i)) i = i + 1 rate.sleep() # this will 'pulse' the loop at the rate except KeyboardInterrupt: print("Stopping ....") sys.exit(0) sys.exit(0)
def pub_msg(self, num): msg = String() msg.data = 'detect {} people'.format(num) self.publisher_.publish(msg) self.get_logger().info('Publishing: "%s"' % msg.data)
def final_result(self, hyp, uttid): """ Insert the final result. """ msg = String() msg.data = str(hyp.lower()) rospy.loginfo(msg.data) self.pub.publish(msg)
def pr2_mover(object_list): # TODO: Initialize variables object_list_param=[] labels=[] centroids=[] points_arr=[] dict_list=[] # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') # TODO: Parse parameters into individual variables # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list for object in object_list_param: #labels.append(object.label) labels.append(object['name']) i=0 for i in range(len(object_list)): if (object['name']==object_list[i].label): break test_scene_num=Int32() #test_scene_num.data=1 #test_scene_num.data=2 test_scene_num.data=3 object_name=String() object_name.data=object['name'] object_group=object['group'] # TODO: Get the PointCloud for a given object and obtain it's centroid points_arr=ros_to_pcl(object_list[i].cloud).to_array() centroids.append(np.mean(points_arr,axis=0)[:3]) pick_pose=Pose() pick_pose.position.x=np.asscalar(np.mean(points_arr,axis=0)[0]) pick_pose.position.y=np.asscalar(np.mean(points_arr,axis=0)[1]) pick_pose.position.z=np.asscalar(np.mean(points_arr,axis=0)[2]) # TODO: Create 'place_pose' for the object place_pose_param=rospy.get_param('/dropbox') place_pose=Pose() j=0 if (object_group=='green'): j=1 place_pose.position.x=place_pose_param[j]['position'][0] place_pose.position.y=place_pose_param[j]['position'][1] place_pose.position.z=place_pose_param[j]['position'][2] # TODO: Assign the arm to be used for pick_place arm_name=String() if (object_group=='green'): arm_name.data='right' else: arm_name.data='left' # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose) print ("Response: ",resp.success) except rospy.ServiceException, e: print "Service call failed: %s"%e
def receiveInputGlobal(self, empty): pose = [ self.robotPTAMPose.position.x, self.robotPTAMPose.position.y, tan( euler_from_quaternion([ self.robotPTAMPose.orientation.x, self.robotPTAMPose.orientation.y, self.robotPTAMPose.orientation.z, self.robotPTAMPose.orientation.w ])[2]), 0 ] # pose = [self.robotState.position.x, self.robotState.position.y, # tan(euler_from_quaternion([ self.robotState.orientation.x, # self.robotState.orientation.y, # self.robotState.orientation.z, # self.robotState.orientation.w])[2]),0] # distance = linalg.norm(array([self.robotPTAMPose.position.x, self.robotPTAMPose.position.y]) # - array([self.points[0], self.points[1]])) # distance2 = linalg.norm(array([self.goal[0], self.goal[1]]) # - array([self.points[0], self.points[1]])) # self.points[-1] = 1.2*self.tf*distance1/(distance1+distance2) goal = self.goal[:] my_points = self.points[:] print goal print my_points # if(self.map==1 or self.map==2 or self.map==3): # if pose[0]>=4: # points_done = 1 # goal[-1]=14.0 # my_points = [] # else: # if pose[1]>=4: # my_points[-1]=7.0 # goal[-1]=21.0 # ps = PoseStamped() # ps.header.stamp = rospy.Time.now() # ps.header.frame_id="world" # if my_points==[]: # ps.pose.position.x = goal[0] # ps.pose.position.y = goal[1] # q = quaternion_from_euler(0,0,arctan(goal[2])) # else: # ps.pose.position.x = my_points[0] # ps.pose.position.y = my_points[1] # q = quaternion_from_euler(0,0,arctan(my_points[2])) # ps.pose.orientation.w = q[3] # ps.pose.orientation.x = q[0] # ps.pose.orientation.y = q[1] # ps.pose.orientation.z = q[2] # self.waypoint_pub.publish(ps) # elif self.map==4: # if pose[0] < -1.5: # goal = goal[0:1] # my_points = my_points[0:1] # points_done = 0 # print 'if' # elif pose[0] >=-1.5 and pose[0] < 1.5: # goal = goal[-1:] # my_points = my_points[-1:] # points_done = 0 # print 'elif1' # elif pose[0] >= 1.5: # goal = goal[-1:] # goal[0][-1]=14.0 # my_points = my_points[-1:] # points_done = 1 # print 'elif2' # print goal # print my_points points = [pose] + my_points + goal print points, len(points) # if pose[1]<=4: # points_done = 3 # else: # points_done = 2 # else: # if pose[1]>=4: # points_done = 1 # else: # points_done = 0 status = String() inp = [ pose[0], pose[1], pose[2], goal[0], goal[1], goal[2], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] # print inp # print points[1:-1] status.data = "BUSY" self.status_pub.publish(status) to = self.to tf = goal[-1] # if points[1:-1]==[]: # coeffs = get_coeffs(inp,to,tf) # else: # coeffs = get_coeffs_points(inp,to,tf,self.points) coeffs = get_coeffs(inp, to, tf, my_points) self.status_pub.publish(status) t = to self.status_pub.publish(status) self.state = 2 r = rospy.Rate(self.delay) cmd = Twist() self.GlobalPath.points = [] while t < tf: point1, kt1 = getPos(coeffs, to, tf, t, pose[1]) point1.x, point1.y, point1.z = point1.z, point1.x, 0.0 self.GlobalPath.points.append(point1) t = t + 0.1 t = to self.global_path_pub.publish(self.GlobalPath) while t < tf and self.state == 2: self.global_path_pub.publish(self.GlobalPath) dx, dy, dk = getVel(coeffs, to, tf, t) point1, kt1 = getPos(coeffs, to, tf, t, pose[1]) point2, kt2 = getPos(coeffs, to, tf, min(t + 1, tf), pose[1]) message = Float32MultiArray() message.data = [ 0.0, 0.0, 0.0, point2.z - point1.z, point2.x - point1.x, tan(arctan(kt2) - arctan(kt1)), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] #print self.robotPTAMPose.position.x, self.robotPTAMPose.position.y self.status_pub.publish(status) yaw = euler_from_quaternion([ self.robotState.orientation.x, self.robotState.orientation.y, self.robotState.orientation.z, self.robotState.orientation.w ])[2] # yaw = euler_from_quaternion([ self.robotPTAMPose.orientation.x, # self.robotPTAMPose.orientation.y, # self.robotPTAMPose.orientation.z, # self.robotPTAMPose.orientation.w])[2] if fabs(yaw) < pi / 2.0: cmd.linear.x = sign(dx) * sqrt(dx**2 + dy**2) elif fabs(yaw) > pi / 2.0: cmd.linear.x = -sign(dx) * sqrt(dx**2 + dy**2) else: cmd.linear.x = sign(yaw) * sign(dy) * sqrt(dx**2 + dy**2) cmd.angular.z = self.VEL_SCALE * dk cmd.linear.x = self.VEL_SCALE * cmd.linear.x message.data[-1] = sign(cmd.linear.x) # T = 10*t # if(int(T)==T): self.inp_pub.publish(message) lookahead = [ point1.z, point1.x, kt1, point2.z, point2.x, kt2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] #traj = array(self.trajjer(lookahead+[0,1,1])) # traj = traj.T # traj[0], traj[1] = traj[1], traj[0] # traj = traj.T #self.lookaheadPath.points = traj.tolist() #self.lookaheadPath.pose = self.robotPTAMPose self.vel_pub.publish(cmd) r.sleep() t = t + 1.0 / self.delay self.status_pub.publish(status) self.vel_pub.publish(Twist()) self.vel_pub.publish(Twist()) self.vel_pub.publish(Twist()) status.data = "DONE1" rospy.Rate(2).sleep() self.status_pub.publish(status) self.state = 0
def pr2_mover(object_list): # TODO: Initialize variables # Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') # TODO: Parse parameters into individual variables # TODO: Rotate PR2 in place to capture side tables for the collision map centroids = [] dict_list = [] # Loop through the pick list for i, object in enumerate(object_list): # Get the PointCloud for a given object and obtain it's centroid points_arr = ros_to_pcl(object.cloud).to_array() centroids.append(np.mean(points_arr, axis=0)[:3]) # Create 'place_pose' for the object test_scene_num = Int32() test_scene_num.data = 3 object_name = String() object_name.data = str(object.label) place_pose = Pose() # Assign the arm to be used for pick_place which_arm = String() for object_param in object_list_param: if object_param['name'] == object.label: if object_param['group'] == 'red': which_arm.data = 'left' place_pose.position.x = dropbox_param[0]['position'][0] place_pose.position.y = dropbox_param[0]['position'][1] place_pose.position.z = dropbox_param[0]['position'][2] elif object_param['group'] == 'green': which_arm.data = 'right' place_pose.position.x = dropbox_param[1]['position'][0] place_pose.position.y = dropbox_param[1]['position'][1] place_pose.position.z = dropbox_param[1]['position'][2] # rospy.loginfo('place_pose {}'.format(place_pose)) pick_pose = Pose() pick_pose.position.x = np.asscalar(centroids[i][0]) pick_pose.position.y = np.asscalar(centroids[i][1]) pick_pose.position.z = np.asscalar(centroids[i][2]) # Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict = make_yaml_dict(test_scene_num, which_arm, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # Wait for 'pick_place_routine' service to come up # rospy.wait_for_service('pick_place_routine') # # try: # pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # # # TODO: Insert your message variables to be sent as a service request # resp = pick_place_routine(test_scene_num, object_name, which_arm, pick_pose, place_pose) # # print ("Response: ",resp.success) # # except rospy.ServiceException, e: # print "Service call failed: %s"%e # Output your request parameters into output yaml file # send_to_yaml('output_1.yaml', dict_list) # send_to_yaml('output_2.yaml', dict_list) send_to_yaml('output_3.yaml', dict_list)
def callback(data, pub): msg = String() msg.data = str(data.steering_target) + " " \ + str(data.throttle) + " " \ + str(data.brake) pub.publish(msg)
def Task1Processor(PointList): global firstTime global yC, yT, pC, pT #Angles of Left and Right Arm #Prints out Current Angles, or Success if m.fabs(pC - pT) > m.radians(5): print Cont.O.color.BOLD + Cont.O.color.UNDERLINE + Cont.O.color.DARKCYAN print "Pitch (Blue) Delta: ", m.fabs(pC - pT) stuff = String() stuff.data = "Pitch (Blue) Delta: ", m.fabs(pC - pT) conn.publish(stuff) print Cont.O.color.END else: print Cont.O.color.GREEN + Cont.O.color.BOLD + "PITCH GOOD!" + Cont.O.color.END stuff = String() stuff.data = "PITCH GOOD!" conn.publish(stuff) if m.fabs(yC - yT) > m.radians(5): print Cont.O.color.BOLD + Cont.O.color.UNDERLINE + Cont.O.color.DARKCYAN print "Yaw (Red) Delta: ", m.fabs(yC - yT) stuff = String() stuff.data = "Yaw (Red) Delta: ", m.fabs(yC - yT) conn.publish(stuff) print Cont.O.color.END else: print Cont.O.color.GREEN + Cont.O.color.BOLD + "YAW GOOD!" + Cont.O.color.END stuff = String() stuff.data = "YAW GOOD" conn.publish(stuff) #Finds Point, and Publishes LeftPoint, RightPoint = FindOutside(PointList) stuff = String() stuff.data = "LEFT POINT: " + str(LeftPoint) + " RIGHT POINT: " + str(RightPoint) conn.publish(stuff) if LeftPoint == 0: return False #Stops the method from continuing Cont.mC.addMarker(RightPoint, 1) Cont.mC.addMarker(LeftPoint, 0) #Moves arms from Task1.py Reset Values to Proper reset values if firstTime: print LeftPoint, RightPoint ResetArms() print "First Time only" stuff = String() stuff.data = "First Time Only" conn.publish(stuff) t.sleep(20) firstTime = False #Adjusts hands to clenched Cont.hhC.adjustRightHand([0.0, 1.0, 1.0, 1.0, 1.0]) Cont.hhC.adjustLeftHand([0.0, -1.0, -1.0, -1.0, -1.0]) #Moves arms in right directions if m.fabs(pC - pT) > m.radians(5): #BLUE print Cont.O.color.CYAN if pC > pT: RCC(RightPoint, 0) else: RC(RightPoint, 0) if m.fabs(yC - yT) > m.radians(5): #RED if yC < yT: LC(LeftPoint, 0) else: LCC(LeftPoint, 0) print Cont.O.color.END
step = 0.1 # mV interval = 5e-3 # 5 msec. fixtime = 1. # 1 sec. roop_vg1 = int((final_voltage - initial_voltage) / step) roop_vg2 = int((final_voltage - initial_voltage) / step) # Start Chopper # Set Param ctrl.output_loatt_current(config=True) #ctrl.set_1st_lo(config=True) ctrl.output_sis_voltage(config=True) # Start Log. msg = String() msg.data = str(time.time()) flag_name = 'hemt_sweep_trigger' pub = rospy.Publisher(flag_name, String, queue_size=1) time.sleep(1.5) pub.publish(msg) try: for vol1 in range(roop_vg1 + 1): for vol2 in range(roop_vg2 + 1): for _ in beam_list: ctrl.output_hemt_voltage(beam=_, vd=1.2, vg1=vol1 * step + initial_voltage, vg2=vol2 * step + initial_voltage) time.sleep(interval) time.sleep(fixtime)
import rosbag from std_msgs.msg import Int32, String bag = rosbag.Bag('test.bag', 'w') try: str = String() str.data = 'foo' i1 = Int32(data=42) i2 = Int32(data=43) bag.write('chatter', i1) bag.write('chatter', i1) bag.write('num', str) finally: bag.close()
def Task1ProcessorO(PointList, LC, LCC, RC, RCC, zAdjust, which): global firstTime global yC, yT, pC, pT #Angles of Left and Right Arm #Prints out Current Angles, or Success if m.fabs(pC - pT) > m.radians(5): print Cont.O.color.BOLD + Cont.O.color.UNDERLINE + Cont.O.color.DARKCYAN print "Pitch (Blue) Delta: ", m.fabs(pC - pT) stuff = String() stuff.data = "Pitch (Blue) Delta: ", m.fabs(pC - pT) conn.publish(stuff) print Cont.O.color.END else: print Cont.O.color.GREEN + Cont.O.color.BOLD + "PITCH GOOD!" + Cont.O.color.END stuff = String() stuff.data = "PITCH GOOD!" conn.publish(stuff) if m.fabs(yC - yT) > m.radians(5): print Cont.O.color.BOLD + Cont.O.color.UNDERLINE + Cont.O.color.DARKCYAN print "Yaw (Red) Delta: ", m.fabs(yC - yT) stuff = String() stuff.data = "Yaw (Red) Delta: ", m.fabs(yC - yT) conn.publish(stuff) print Cont.O.color.END else: print Cont.O.color.GREEN + Cont.O.color.BOLD + "YAW GOOD!" + Cont.O.color.END stuff = String() stuff.data = "YAW GOOD!" conn.publish(stuff) #Finds Point, and Publishes LeftPoint, RightPoint = FindOutside(PointList) Cont.mC.addMarker(RightPoint, 1) Cont.mC.addMarker(LeftPoint, 0) #Moves arms from Task1.py Reset Values to Proper reset values if firstTime: print LeftPoint, RightPoint ResetArms() print "First Time only" stuff = String() stuff.data = "First Time only" conn.publish(stuff) t.sleep(20) firstTime = False #Adjusts hands to clenched Cont.hhC.adjustRightHand([0.0, 1.0, 1.0, 1.0, 1.0]) Cont.hhC.adjustLeftHand([0.0, -1.0, -1.0, -1.0, -1.0]) #Moves arms in right directions print Cont.O.color.CYAN if m.fabs(pC - pT) > m.radians(5): #BLUE if pC > pT: RCC(RightPoint, 0) else: RC(RightPoint, 0) if m.fabs(yC - yT) > m.radians(5): #RED if yC < yT: LC(LeftPoint, 0) else: LCC(LeftPoint, 0) print Cont.O.color.END makeAdjustments(LeftPoint, RightPoint, LC, LCC, RC, RCC) if zAdjust != '0' : if which == '1' : LC(LeftPoint, zAdjust) if which == '2' : LCC(LeftPoint, zAdjust) if which == '3' : RC(RightPoint, zAdjust) if which == '4' : RCC(RightPoint, zAdjust)
sd = ShapeDetector() shape = sd.detect(c) #print "SHAPE: " +str(shape) # multiply the contour (x, y)-coordinates by the resize ratio, # then draw the contours and the name of the shape on the image c = c.astype("int") cv2.drawContours(frame, [c], -1, (0, 255, 0), 2) cv2.putText(frame, shape, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) # show the output image #cv2.imshow("gray",gray) #cv2.imshow("mavi",mavi) #cv2.imshow("thresh",thresh) #cv2.imshow("frame",frame) if shape == "square": msg = String() msg.data = "durak" pub.publish(msg) rate.sleep() if cv2.waitKey(1) & 0xFF == ord('q'): break counter = counter % 5 """subDurak = rospy.Subscriber("durak", String, callback) rospy.spin() def callback(msg): if msg=="durak": """
def publish_mode(): msg = String() for i in group['mode']: if sensors[i] == True: msg.data = i mode_publisher.publish(msg)
def speakPub(self, s): speakMsg = String() speakMsg.data = s self.espeakPub.publish(speakMsg)
def timer_callback(self): msg = String() msg.data = 'Hello World!' self.publisher.publish(msg) self.get_logger().info(msg.data)
def timer_callback(self): msg = String() msg.data = 'Hello World: %d' % self.i self.publisher_.publish(msg) self.get_logger().info('Publishing: "%s"' % msg.data) self.i += 1
def publish_charging_state(): msg = String() for i in group['charging_state']: if sensors[i] == True: msg.data = i charging_state_publisher.publish(msg)
def feedbackPub(self, s): fbMsg = String() fbMsg.data = s self.fbPub.publish(fbMsg)
def changeLFParams(self, params, reset_time): data = {"params": params, "time": reset_time} msg = String() msg.data = json.dumps(data) self.pub_LF_params.publish(msg)
def make_pan(self, name): pan_msg = String() pan_msg.data = "pan" self.pan_pub.publish(pan_msg)
def _get_string(self, px4_cmd): cmd_string = String() cmd_string.data = px4_cmd return cmd_string
def pr2_mover(object_list): # TODO: Initialize variables world_id = rospy.get_param("world_id") yaml_out = [] # TODO: Get/Read parameters object_list_param = rospy.get_param("/object_list") boxes_param = rospy.get_param("/dropbox") # TODO: Parse parameters into individual variables # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list for o in object_list_param: name = o["name"] group = o["group"] oid = None # TODO: Get the PointCloud for a given object and obtain it's centroid for i, obj in enumerate(object_list): if name != obj.label: continue else: oid = i # TODO: Create 'place_pose' for the object ros_world = Int32() selected_object = String() selected_object.data = name ros_world.data = world_id selected_robo_arm = String() # TODO: Assign the arm to be used for pick_place if group == 'green': selected_robo_arm.data = 'right' else: selected_robo_arm.data = 'left' pick_pose = Pose() object_cloud_pcl = ros_to_pcl(obj.cloud) pt_array = object_cloud_pcl.to_array() centroid = np.mean(pt_array, axis=0)[:3] centroid_float = [np.asscalar(x) for x in centroid] pick_pose.position.x = centroid_float[0] pick_pose.position.y = centroid_float[1] pick_pose.position.z = centroid_float[2] selected_box_pose = [0, 0, 0] for elem in boxes_param: if (elem['name'] == selected_robo_arm.data): selected_box_pose = elem['position'] break place_pose = Pose() place_pose.position.x = selected_box_pose[0] place_pose.position.y = selected_box_pose[0] place_pose.position.z = selected_box_pose[0] # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_out.append( make_yaml_dict(ros_world, selected_robo_arm, selected_object, pick_pose, place_pose)) del object_list[oid] #break # move on to next object in params list ''' # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) print ("Response: ",resp.success) except rospy.ServiceException, e: print "Service call failed: %s"%e ''' # TODO: Output your request parameters into output yaml file send_to_yaml('output_%i.yaml' % world_id, yaml_out)