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)
