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):
rospy.loginfo("Going into shower...")
my_string = String()
my_string.data = "La salle de bain est chaude. Vous pouvez aller vous doucher."
self.french_pub.publish(my_string)
rospy.sleep(600)
return 'succeeded'
def recognizer():
pub = rospy.Publisher('/recognizer/output',String, queue_size=10)
rospy.init_node('HRI_Recognizer')
msg = String()
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(("127.0.0.1", 0))
port = s.getsockname()[1]
s.listen(1)
p = multiprocessing.Process(target=callJulius, args=(port,))
p.start()
conn, addr = s.accept()
while not rospy.is_shutdown():
data = conn.recv(4096)
if data.find("sentence1") >= 0:
data=data[data.find("sentence1")+15:data.find(" </s>")]
if data == "GO A HEAD":
data = "GO AHEAD"
elif data == "GOLEFT":
data = "GO LEFT"
elif data == "TAKEOFF":
data = "TAKE OFF"
msg.data = data
pub.publish(msg)
s.close()
p.close()
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 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 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 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 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 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 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 test_unsubscribe_does_not_receive_further_msgs(self):
topic = "/test_unsubscribe_does_not_receive_further_msgs"
msg_type = "std_msgs/String"
client = "client_test_unsubscribe_does_not_receive_further_msgs"
msg = String()
msg.data = "dsajfadsufasdjf"
pub = rospy.Publisher(topic, String)
multi = MultiSubscriber(topic, msg_type)
received = {"count": 0}
def cb(msg):
received["count"] = received["count"] + 1
multi.subscribe(client, cb)
sleep(0.5)
pub.publish(msg)
sleep(0.5)
self.assertEqual(received["count"], 1)
multi.unsubscribe(client)
sleep(0.5)
pub.publish(msg)
sleep(0.5)
self.assertEqual(received["count"], 1)
def cbLabels(self, ros_data):
labels = ros_data.labels
if labels is None or self.filename is None:
return
messages = []
idx = 0
for kType in self.knowledgeTypes:
#affList = affList + self.affDictionary[label.rsplit(None,1)[-1]] + ','
message = []
for label in labels:
msg = String()
if label.data in self.dictionaries[idx]:
msg.data = self.dictionaries[idx][label.data]
message.append(msg)
else:
print "Object " + label.data + " not found in knowledge source"
idx += 1
arrMsg = ObjectKnowledgeArray()
arrMsg.data = message
messages.append(arrMsg)
kmsg = ObjectKnowledge()
kmsg.knowledge = messages
kmsg.labels = labels
self.knowPub.publish(kmsg)
def test_cmd_handler_right(self):
"""Checks that person rotates clockwise when given right msg."""
msg = String()
msg.data = "right"
self.person.cmd_handler(msg)
self.assertTrue(self.person.rotation_executing)
self.assertEqual(self.person.velocity.angular.z, -0.5)
def final_result(self, hyp, uttid):
""" Insert the final result. """
msg = String()
msg.data = str(hyp.lower())
#print '****************************faceCommander node: %s'%msg.data
rospy.loginfo("<Speech> faceCommander: %s", msg.data)
self.pub.publish(msg)
def test_immediate_publish(self):
""" This test makes sure the PublisherConsistencyListener is working"""
topic = "/test_immediate_publish"
msg_class = String
msg = String()
string = "why halo thar"
msg.data = string
received = {"msg": None}
def callback(msg):
print "Received a msg! ", msg
received["msg"] = msg
rospy.Subscriber(topic, msg_class, callback)
class temp_listener(rospy.SubscribeListener):
def peer_subscribe(self, topic_name, topic_publish, peer_publish):
print "peer subscribe in temp listener"
listener = PublisherConsistencyListener()
publisher = rospy.Publisher(topic, msg_class, temp_listener())
listener.attach(publisher)
publisher.publish(msg)
sleep(0.5)
self.assertEqual(received["msg"], 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 default(self, ci='unused'):
""" Publish the data of the semantic camera as a ROS String message that contains a lisp-list.
This function was designed for the use with CRAM and the Adapto group
"""
string = String()
string.data = "("
for obj in self.data['visible_objects']:
# if object has no description, set to '-'
if obj['description'] == '':
description = '-'
# send tf-frame for every object
self.sendTransform(self, obj['position'], obj['orientation'], \
rospy.Time.now(), str(obj['name']), "/map")
# Build string from name, description, location and orientation in the global world frame
string.data += "(" + str(obj['name']) + " " + description + " " + \
str(obj['position'].x) + " " + \
str(obj['position'].y) + " " + \
str(obj['position'].z) + " " + \
str(obj['orientation'].x) + " " + \
str(obj['orientation'].y) + " " + \
str(obj['orientation'].z) + " " + \
str(obj['orientation'].w) + ")"
string.data += ")"
self.publish(string)
def execute(self, ud):
tts_msg = String()
rospy.sleep(rospy.Duration(5))
self.face_cmd.publish(YELLOW_FACE)
tts_msg.data = "I can not guide you back safely. I will go back to the stating location on my own."
self.tts_pub.publish(tts_msg)
tf_stamped = self.tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0))
lst = []
for i in range(len(ud.sg_waypoints)):
distance = sqrt((tf_stamped.transform.translation.x - ud.sg_waypoints[i].pose.position.x)**2 +
(tf_stamped.transform.translation.y - ud.sg_waypoints[i].pose.position.y)**2)
lst.append([distance, i])
lst.sort()
if lst[0][1] == 0:
ud.sg_target_wp = 0
else:
ud.sg_target_wp = lst[0][1] - 1
return 'go_back'
def publish_haptic_control(self, ctrl):
# publish haptic feedback
haptic_msg = String()
haptic_msg.data = "{:d},{:d}".format(int(ctrl[0]), int(ctrl[1]))
print haptic_msg.data, "\r"
self.haptic_msg_pub.publish(haptic_msg)
def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = ud.move_waypoints[ud.move_target_wp]
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result()
status = self.move_base_client.get_state()
tts_msg = String()
if status == GoalStatus.SUCCEEDED:
tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "."
self.tts_pub.publish(tts_msg)
if ud.move_target_wp == 0:
return 'test_finished'
else:
ud.move_target_wp -= 1
return 'succeeded'
else:
return 'wp_not_reached'
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 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 run(self):
while not rospy.is_shutdown():
msg = String()
msg.data = 'yeah'
self.pub_test.publish(msg)
# update states
# option 1: direct
# option 2: hybrid
if self.enabled:
gain_vel = 0.1
gain_rot = 0.5
self.cmdtwist.vel *= gain_vel
self.cmdtwist.rot *= gain_rot
# self.cmdfrm.Integrate(self.cmdtwist, self.freq)
tmptwist = self.cmdfrm.M.Inverse(self.cmdtwist)
self.cmdfrm.Integrate(tmptwist, self.freq)
# hybrid
self.ctrl_hybrid()
cmdtrans = (self.cmdfrm.p.x(), self.cmdfrm.p.y(), self.cmdfrm.p.z())
cmdrot = self.cmdfrm.M.GetQuaternion()
self.ber.sendTransform(cmdtrans,
cmdrot,
rospy.Time.now(),
"wam/cmd",
"wam/base_link")
# print cmdTrans
self.rate.sleep()
def _on_check_sensor_pressed(self):
str = String()
str.data = "check_sensor"
self._publishersys.publish(str)
self._widget.victim_label.setText('Check SS')
self._widget.CO2.setText('0')
self._widget.current_z_angular_label.setText('0')
def _send_heat(self, heat):
h = Int16()
h.data = heat
self._publisherth.publish(h)
str = String()
str.data = "setheat"
self._publisherauto.publish(str)
def main(argv=sys.argv[1:]):
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'--reliable', dest='reliable', action='store_true',
help='set qos profile to reliable')
parser.set_defaults(reliable=False)
parser.add_argument(
'-n', '--number_of_cycles', type=int, default=20,
help='number of sending attempts')
args = parser.parse_args(argv)
rclpy.init()
if args.reliable:
custom_qos_profile = qos_profile_default
print('Reliable publisher')
else:
custom_qos_profile = qos_profile_sensor_data
print('Best effort publisher')
node = rclpy.create_node('talker_qos')
chatter_pub = node.create_publisher(String, 'chatter_qos', custom_qos_profile)
msg = String()
cycle_count = 0
while rclpy.ok() and cycle_count < args.number_of_cycles:
msg.data = 'Hello World: {0}'.format(cycle_count)
print('Publishing: "{0}"'.format(msg.data))
chatter_pub.publish(msg)
cycle_count += 1
sleep(1)
def auto_callback(self, data):
#self._widget.textEdit.append('autocall')
str = String()
if data.data == "detectC":
self._widget.victim_label.setText('Found Hole')
self._widget.current_z_angular_label.setEnabled(True)
self._widget.label.setEnabled(True)
# self._widget.groupBox.setEnabled(True)
self._widget.comfirm_button.setEnabled(True)
self._widget.approach_victim_button.setEnabled(True)
self._widget.ignor_button.setEnabled(True)
str.data = "stop"
self._publishersys.publish(str)
if data.data == "detectT":
self._widget.victim_label.setText('Found Temp')
self._widget.current_z_angular_label.setEnabled(True)
self._widget.label.setEnabled(True)
# self._widget.groupBox.setEnabled(True)
self._widget.comfirm_button.setEnabled(True)
self._widget.approach_victim_button.setEnabled(True)
self._widget.ignor_button.setEnabled(True)
if data.data == "detectQ":
self._widget.victim_label.setText('QR Found')
self._widget.current_z_angular_label.setEnabled(True)
self._widget.label.setEnabled(True)
def publish_users():
# build list of current users online
active_user, mod_time = get_active_user()
online_users = []
# if users == {} and active_user and time.time() - mod_time > INACTIVITY_LOGOUT:
# we should remove the active user
# remove_active_user()
# active_user = ""
for user in users.keys():
last_ping = users[user]
if user == active_user:
# the active user file may have been updated before presence was published
last_ping = max(last_ping, mod_time)
rospy.logdebug("active user %s being considered; now: %f, last_ping: %f, mod_time: %f" % (user, time.time(), last_ping, mod_time))
if time.time() - last_ping < INACTIVITY_LOGOUT:
online_users.append(user)
else:
if user == active_user:
# we should remove the active user
remove_active_user()
active_user = ""
online_users.sort()
out = String()
out.data = "%s:%s" % (','.join(online_users), active_user)
if active_user and online_users != []:
publisher.publish(out)
rospy.logdebug("publishing online users: %s" % out)
def createTopLevel(self, stimuliFile, numReps):
"""Creates the top level directory, randomizes input list"""
self.sessionID = str(rospy.Time.now())
self.topleveldir = os.environ["HOME"] + "/UltraspeechData/" + str(datetime.date.today()) + "_" + self.sessionID
os.makedirs(self.topleveldir)
monocamdir = self.topleveldir + "/monocam"
os.makedirs(monocamdir)
stereorightdir = self.topleveldir + "/stereoright"
os.makedirs(stereorightdir)
stereoleftdir = self.topleveldir + "/stereoleft"
os.makedirs(stereoleftdir)
ultrasounddir = self.topleveldir + "/ultrasound"
os.makedirs(ultrasounddir)
stimuli = open(stimuliFile, "r").readlines()
random.shuffle(stimuli)
self.stimuliList = []
for i in range(numReps):
for j in stimuli:
if j != "\n":
self.stimuliList.append(j[:-1])
self.numReps = numReps
self.numBatches = int(math.ceil(float(len(self.stimuliList)) / 10.0))
self.console.set_text("Press Start to begin")
for i in range(2):
# this tells the logger where to create the logfile
msg = String()
msg.data = str(self.topleveldir)
self.startupTopic.publish(msg)
time.sleep(1) # for some reason the first one is not published
def execute(self, ud):
initial_pose = PoseWithCovarianceStamped()
initial_pose.header.frame_id = 'map'
initial_pose.pose.pose.position.x = 0.0
initial_pose.pose.pose.position.y = 0.0
initial_pose.pose.pose.orientation.x = 0.0
initial_pose.pose.pose.orientation.y = 0.0
initial_pose.pose.pose.orientation.z = 0.0
initial_pose.pose.pose.orientation.w = 1.0
self.amcl_initial_pose_pub.publish(initial_pose)
neck_cmd = Float64()
neck_cmd.data = -2.0
self.neck_pub.publish(neck_cmd)
rospy.sleep(rospy.Duration(2))
neck_cmd.data = -0.7
self.neck_pub.publish(neck_cmd)
rospy.sleep(rospy.Duration(4))
tts_msg = String()
tts_msg.data = "I am ready to begin the inspection."
self.tts_pub.publish(tts_msg)
return 'init_done'
def detectMarker(self):
self.data = self.marker_req(String('marker'))
self.data = self.data.data
def callback(self, scan):
#result = Twist()
#result.linear.x = 1
#listener = tf.TransformListener()
data = String()
frontClear = False
rightClear = False
leftClear = False
frontCloseCount = 0
rightCloseCount = 0
leftCloseCount = 0
angleRange = 7
frontAngle = 0
rightAngle = 90
leftAngle = -90
for i, distance in enumerate(scan.ranges):
angle = AutoDrive.RAD2DEG(scan.angle_min +
scan.angle_increment * i)
#print("angle: {}".format(angle))
if frontAngle - angleRange <= angle <= frontAngle + angleRange:
if AutoDrive.isTooClose(distance):
frontCloseCount += 1
elif AutoDrive.isClear(distance):
frontClear = True
elif rightAngle - angleRange <= angle <= rightAngle + angleRange:
if AutoDrive.isTooClose(distance):
rightCloseCount += 1
elif AutoDrive.isClear(distance):
rightClear = True
elif leftAngle - angleRange <= angle <= leftAngle + angleRange:
if AutoDrive.isTooClose(distance):
leftCloseCount += 1
elif AutoDrive.isClear(distance):
leftClear = True
tooCloseThreshold = 10
frontTooClose = not frontClear and frontCloseCount >= tooCloseThreshold
rightTooClose = not rightClear and rightCloseCount >= tooCloseThreshold
leftTooClose = not leftClear and leftCloseCount >= tooCloseThreshold
print("===========================")
print("frontClear : ", frontClear)
print("rightClear : ", rightClear)
print("leftClear : ", leftClear)
print("frontTooClose : ", frontTooClose)
print("rightTooClose : ", rightTooClose)
print("leftTooClose : ", leftTooClose)
print("===========================")
# Front is Clear && Left is Wall
if frontClear and not leftClear:
if not leftTooClose:
data = "F"
else:
data = "FL"
# Front is Clear && Left is Clear
elif frontClear and leftClear:
data = "FR"
# Front is BLOCKED!!!
elif not frontClear:
data = "L"
else:
data = "stop"
# machine arrived
#x, y = self.getPosition()
#x_gap = self.init_x - x
#y_gap = self.init_y - y
#total_gap = x_gap + y_gap
#if total_gap <= 0.5:
# data = stop
rospy.loginfo(data)
self.pub.publish("stop")
self.pub.publish(data)
time.sleep(0.05)
def respond(self, response):
self.runner.topics['events'].publish(Event('chat_end', 0))
self.talking = True
self.runner.topics['tts']['default'].publish(String(response))
def pr2_mover(object_list):
# TODO: Initialize variables
test_scene_num = Int32()
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
# TODO: Get/Read parameters
objects = rospy.get_param('/object_list')
dropboxs = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
test_scene_num.data = 1
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
yaml_dists = []
for detected_object in object_list:
object_name.data = str(detected_object.label)
# TODO: Get the PointCloud for a given object and obtain it's centroid
points = ros_to_pcl(detected_object.cloud).to_array()
x, y, z = np.mean(points, axis=0)[:3]
pick_pose.position.x = np.asscalar(x)
pick_pose.position.y = np.asscalar(y)
pick_pose.position.z = np.asscalar(z)
# TODO: Create 'place_pose' for the object
for item in objects:
if item['name'] == object_name.data:
for box in dropboxs:
if box['group'] == item['group']:
x, y, z = box['position']
place_pose.position.x = np.float(x)
place_pose.position.y = np.float(y)
place_pose.position.z = np.float(z)
arm_name.data = box['name']
# TODO: Assign the arm to be used for pick_place
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dists.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: Output your request parameters into output yaml file
send_to_yaml('./yaml/output_1.yaml', yaml_dists)
def pr2_mover(object_list):
# TODO: Initialize variables
labels = []
centroids = []
test_scene_num = Int32()
test_scene_num.data = 2
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
place_pose_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
pick_list = []
for i in range(0, len(object_list_param)):
object_name = String()
object_name.data = object_list_param[i]['name']
object_group = object_list_param[i]['group']
pick_list.append((object_name, object_group))
for object in object_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# TODO: Rotate PR2 in place to capture side tables for the collision map
yaml_dict_list = []
# TODO: Loop through the pick list
for object_name, object_group in pick_list:
object_index = 0
if object_name.data in labels:
object_index = labels.index(object_name.data)
# TODO: Get the PointCloud for a given object and obtain it's centroid
pick_pose.position.x = np.asscalar(centroids[object_index][0])
pick_pose.position.y = np.asscalar(centroids[object_index][1])
pick_pose.position.z = np.asscalar(centroids[object_index][2])
# TODO: Create 'place_pose' for the object
dropbox = 0 if object_group == 'red' else 1
place_pose.position.x = place_pose_param[dropbox]['position'][0]
place_pose.position.y = place_pose_param[dropbox]['position'][1]
place_pose.position.z = place_pose_param[dropbox]['position'][2]
# TODO: Assign the arm to be used for pick_place
arm_name.data = 'left' if object_group == 'red' else 'right'
# 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)
yaml_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
# TODO: Output your request parameters into output yaml file
send_to_yaml('output_2.yaml', yaml_dict_list)
import rosbag
from std_msgs.msg import Int32, String
bag = rosbag.Bag('Test.bag', 'w')
try:
s = String()
s.data = 'foo'
i = Int32()
i.data = 42
bag.write('chatter', s)
bag.write('numbers', i)
finally:
bag.close()
def log(self,msg):
rospy.loginfo(msg)
t = String(msg)
self.pub_log.publish(t)
P10 = []
P10.append(np.copy(L))
P11 = []
P11.append(np.copy(MR1))
P11.append(np.copy(MR2))
P11.append(np.copy(MR3))
P12 = []
P12.append(np.copy(ML1))
P12.append(np.copy(ML2))
P12.append(np.copy(ML3))
P13 = []
P13.append(np.copy(M1))
from std_msgs.msg import String
str_msg = String()
pub = rospy.Publisher('pattern_topic', String, queue_size=10)
def patter_publisher(pattern_type):
str_msg.data = pattern_type
pub.publish(str_msg)
def tactile_patterns(drone1, drone2, drone3, human, l, move_right, move_left):
global prev_pattern_time
global pattern_duration
global area_pattern
global left_right_pattern
global prev_pattern
def pr2_mover(objects_list):
# TODO: Initialize variables
test_scene_num = Int32()
object_name = String()
arm_name = String()
object_group = String()
which_arm = String()
pick_pose = Pose()
place_pose = Pose()
labels = []
centroids = []
dict_list = []
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
drop_list_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
for i,objects in enumerate(object_list_param):
object_name.data = object_list_param[i]['name']
object_group = object_list_param[i]['group']
if object_group == 'green':
which_arm = 'right'
else:
which_arm = 'left'
for j,drop_obs in enumerate(drop_list_param):
drop_pos = drop_list_param[j]['position']
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
# TODO: Get the PointCloud for a given object and obtain it's centroid
#for object in objects:
for obj in objects_list:
labels.append(obj.label)
points_arr = ros_to_pcl(obj.cloud).to_array()
#centroids.append(np.asscalar(np.mean(points_arr, axis=0)[:3]))
centroids.append(np.mean(points_arr, axis=0)[:3])
#centroids = [type(np.asscalar(i)) for i in centroids]
#centroids = type(np.asscalar(centroids))
X_p = centroids[0]
#print(X_p)
c_x=np.asscalar(np.array(X_p[0]))
c_y=np.asscalar(np.array(X_p[1]))
c_z=np.asscalar(np.array(X_p[2]))
#center=[c_x, c_y, c_z]
X_p[0]=c_x
X_p[1]=c_y
X_p[2]=c_z
#print(centroids)
# TODO: Create 'place_pose' for the object
pick_pose.position.x = X_p[0]
pick_pose.position.y = X_p[1]
pick_pose.position.z = X_p[2]
#pick_pose = X_p
#place_pose = drop_pos
place_pose.position.x = drop_pos[0]
place_pose.position.y = drop_pos[1]
place_pose.position.z = drop_pos[2]
# TODO: Assign the arm to be used for pick_place
#if object_group == "green"
# which_arm = 'right'
#else
# which_arm = 'left'
arm_name.data = which_arm
test_scene_num.data = 1
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
# def make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose):
# yaml_dict = {}
# yaml_dict["test_scene_num"] = test_scene_num.data
# yaml_dict["arm_name"] = which_arm.data
# yaml_dict["object_name"] = object_name.data
# yaml_dict["pick_pose"] = message_converter.convert_ros_message_to_dictionary(pick_pose)
# yaml_dict["place_pose"] = message_converter.convert_ros_message_to_dictionary(place_pose)
# return yaml_dict
#dict_list = []
for i in range(0, len(object_list_param)):
# Populate various ROS messages
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
test_scene_num.data = 1
arm_name.data = which_arm
#for i,objects in enumerate(object_list_param):
# object_name.data = object_list_param[i]['name']
# object_group = object_list_param[i]['group']
# if object_group == 'green':
# which_arm = 'right'
# else:
# which_arm = 'left'
object_name.data[0] = object_list_param[0]['name']
pick_pose.position.x = X_p[0]
pick_pose.position.y = X_p[1]
pick_pose.position.z = X_p[2]
place_pose.position.x = drop_pos[0]
place_pose.position.y = drop_pos[1]
place_pose.position.z = drop_pos[2]
#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
def sendMove(self, pub, string):
st = String()
st.data = string
pub.publish(st)
print("Published", string)
def _callback_navigate_outside_lab211(req):
# define a client to send goal requests to the move_base server through a SimpleActionClient
ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# wait for the action server to come up
while (not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.logwarn("Waiting for the move_base action server to come up")
'''while(not ac_gaz.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base_simple action server to come up")'''
goal = MoveBaseGoal()
#set up the frame parameters
goal.target_pose.header.frame_id = "/map"
goal.target_pose.header.stamp = rospy.Time.now()
# moving towards the goal*/
goal.target_pose.pose.position = Point(-5.904, 4.274,
0) # (-4.284, 2.952, 0)
orientation = tf.transformations.quaternion_from_euler(
0, 0, 3.093) # (0, 0, -3.0)
goal.target_pose.pose.orientation.x = orientation[0]
goal.target_pose.pose.orientation.y = orientation[1]
goal.target_pose.pose.orientation.z = orientation[2]
goal.target_pose.pose.orientation.w = orientation[3]
rospy.loginfo("Sending goal location ...")
ac.send_goal(goal)
ac.wait_for_result(rospy.Duration(60))
time.sleep(2)
#repeat just for solid execution
# define a client to send goal requests to the move_base server through a SimpleActionClient
ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# wait for the action server to come up
while (not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.logwarn("Waiting for the move_base action server to come up")
'''while(not ac_gaz.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base_simple action server to come up")'''
goal = MoveBaseGoal()
#set up the frame parameters
goal.target_pose.header.frame_id = "/map"
goal.target_pose.header.stamp = rospy.Time.now()
# moving towards the goal*/
goal.target_pose.pose.position = Point(-6.609, 4.020,
0) # (-4.84, 2.952, 0)
orientation = tf.transformations.quaternion_from_euler(
0, 0, 3.093) # (0, 0, -3.0)
goal.target_pose.pose.orientation.x = orientation[0]
goal.target_pose.pose.orientation.y = orientation[1]
goal.target_pose.pose.orientation.z = orientation[2]
goal.target_pose.pose.orientation.w = orientation[3]
rospy.loginfo("Sending goal location ...")
ac.send_goal(goal)
ac.wait_for_result(rospy.Duration(60))
feedback_pub = rospy.Publisher("feedback_for_nav", String, queue_size=1)
if (ac.get_state() == GoalStatus.SUCCEEDED):
rospy.logerr("You have reached the open area")
ser_messageResponse(True)
msg = String()
msg.data = "success"
for ii in range(7):
feedback_pub.publish(msg)
time.sleep(1)
else:
rospy.logerr("The robot failed to reach the open area")
ser_messageResponse(False)
msg = String()
msg.data = "failure"
for ii in range(7):
feedback_pub.publish(msg)
time.sleep(1)
def pr2_mover(object_list):
# TODO: Initialize variables
centroids = {}
dict_list = []
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Get the PointCloud for a given object and obtain it's centroid
for object in object_list:
points_arr = ros_to_pcl(object.cloud).to_array()
centroids[object.label] = np.mean(points_arr, axis=0)[:3]
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for object_param in object_list_param:
# Prepare world num
test_scene_num = Int32()
test_scene_num.data = 1
# TODO: Parse parameters into individual variables
object_name = object_param['name']
object_group = object_param['group']
object_name_ros = String()
object_name_ros.data = object_name
# TODO: Create 'pick_pose' for the object
pick_pose = Pose()
pick_pose.position.x = centroids[object_name][0]
pick_pose.position.y = centroids[object_name][1]
pick_pose.position.z = centroids[object_name][2]
# TODO: Create 'place_pose' for the object
# TODO: Assign the arm to be used for pick_place
place_pose = Pose()
arm_name = String()
place_index = 0
print(object_group)
if object_group == 'red':
arm_name.data = 'left'
place_index = 0
else:
arm_name.data = 'right'
place_index = 1
place_pose.position.x = dropbox_param[place_index]['position'][0]
place_pose.position.y = dropbox_param[place_index]['position'][1]
place_pose.position.z = dropbox_param[place_index]['position'][2]
# 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_ros,
pick_pose, place_pose)
print(yaml_dict)
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_ros,
arm_name, pick_pose, place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def pr2_mover(cloud_objects, cloud_table, cluster_indices, object_list):
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
test_scene_num = Int32()
test_scene_num.data = 1
dropbox_left = dropbox_param[0]['position']
dropbox_right = dropbox_param[1]['position']
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
grasp_list = rospy.get_param('/grasp_list')[0]
for i in range(len(object_list)):
object = object_list[i]
collision_indices = []
for index in range(cloud_objects.size):
if index not in cluster_indices[i]:
collision_indices.append(index)
cloud_collision = cloud_objects.extract(cluster_indices[i])
print "cloud_objects size: %s" % cloud_objects.size
print "cloud_collision size: %s" % cloud_collision.size
collison_pub.publish(pcl_to_ros(cloud_collision))
time.sleep(3)
object_name = String()
object_name.data = object.label
# TODO: Create 'place_pose' for the object
pick_pose = Pose()
pick_pose.position.x = grasp_list[object.label]['position']['x']
pick_pose.position.y = grasp_list[object.label]['position']['y']
pick_pose.position.z = grasp_list[object.label]['position']['z']
place_pose = Pose()
# TODO: Assign the arm to be used for pick_place
# specify the place_pose here, vary the x value of position with [-0.2,0.2] so that objects will not stack in the dropbox
arm_name = String()
if True:
arm_name.data = 'left'
place_pose.position.x = dropbox_left[0] - 0.2
place_pose.position.y = dropbox_left[1]
place_pose.position.z = 0.9
else:
arm_name.data = 'right'
place_pose.position.x = dropbox_right[0] - 0.2 + i * 0.05
place_pose.position.y = dropbox_right[1]
place_pose.position.z = 0.9
# collision
# 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 pr2_mover(detected_objects_list):
# # TODO: Initialize variables
# # TODO: Get/Read parameters
# get parameters (-object list, test scene num, and so on) from ros parameter server which are loaded from project launch file
object_list_param = rospy.get_param('/object_list')
#Dropbox later?
# This variable is not a simple integer but a ROS message of type std_msgs/Int32.
#1: Test scene number
test_scene_num = Int32()
#CHANGE THIS 1,2,3
test_scene_value = 3
test_scene_num.data = int(test_scene_value)
# # TODO: Parse parameters into individual variables
# # TODO: Rotate PR2 in place to capture side tables for the collision map: LATER
# # TODO: Loop through the pick list
# # TODO: Get the PointCloud for a given object and obtain it's centroid
accuracy = get_accuracy(object_list_param, detected_objects_list)
rospy.loginfo('Accuracy {} in total {} objects.'.format(
accuracy, len(object_list_param)))
ordered_list_final = ordered_list(object_list_param, detected_objects_list)
#rospy.loginfo('ordered list is {}'.format(ordered_list_final))
drop_groups_final = ordered_drop_groups(ordered_list_final,
object_list_param)
centroids = centroid_coordinates(ordered_list_final)
dict_list = []
for i in range(len(ordered_list_final)):
#Let's rock n roll
#2: Object name
object_name = String()
object_name.data = str(ordered_list_final[i].label)
# 3: Arm Name
object_group = drop_groups_final[i]
arm_name = String()
if object_group == 'green':
arm_name.data = 'right'
else:
arm_name.data = 'left'
#4: Pick Pose (Centroid)
pick_pose = Pose()
current_centroid = centroids[i]
#WARNING: ROS messages expect native Python data types but having computed centroids as above your list centroids will be of type numpy.float64. To recast to native Python float type you can use np.asscalar(),
float_centroid = [
np.asscalar(centroid_t) for centroid_t in current_centroid
]
pick_pose.position.x = float_centroid[0]
pick_pose.position.y = float_centroid[1]
pick_pose.position.z = float_centroid[2]
#5: LATER: Let's make PR2 happy later..
place_pose = Pose()
#place_pose.position.x = 0
#place_pose.position.y = 0
#place_pose.position.z = 0
# Populate various ROS messages
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
##Based on the group associated with each object (that you extracted from the pick list .yaml file), decide which arm of the robot should be used.
# # TODO: Create 'place_pose' for the object, Assign the arm to be used for pick_place, Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
# # 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: {}".format(resp.success))
except rospy.ServiceException, e:
print("Service call failed: {}".format(e))
def callback(yaw_msg):
deg = round(math.degrees(yaw_msg.data), 2)
publisher.publish(String(output % (yaw_msg.data, deg)))
def __init__(self):
self.sub = rospy.Subscriber('chatter', String, self.callback,queue_size=10000)
self.pub = rospy.Publisher('chatter_repeated', String, queue_size=10)
self.msg = String()
self.str = ''
def pr2_mover(object_list):
dict_list = []
# Get/Read the detected object parameters
object_list_param = rospy.get_param('/object_list')
# Loop through the pick list
labels = []
centroids = [] # to be list of tuples (x, y, z)
for i in range(len(object_list_param)):
# Put the object parameters into individual variables
object_name = object_list_param[i]['name']
object_group = object_list_param[i]['group']
# TODO: Rotate PR2 in place to capture side tables for the collision map
#topic_name = "/pr2/world_joint_controller/command"
#pub_PR2_rotate = rospy.Publisher(topic_name, std_msgs.msg.Float64, queue_size=3)
#pub_PR2_rotate.publish(np.pi/2)
for j, object in enumerate(object_list):
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# check if the object is the next object on the list to relocate
if (object.label == object_name):
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
# Get the PointCloud for a given object and obtain it's centroid
centroids.append(np.mean(points_arr, axis=0)[:3])
center_x = np.asscalar(centroids[j][0])
center_y = np.asscalar(centroids[j][1])
center_z = np.asscalar(centroids[j][2])
# store which environment is used
test_scene_num = Int32()
test_scene_num.data = scene_num
# Initialize a variable
object_name = String()
# Populate the data field
object_name.data = object_list_param[i]['name']
# Create the objects position to be picked up from
# initialize an empty pose message
pick_pose = Pose()
pick_pose.position.x = center_x
pick_pose.position.y = center_y
pick_pose.position.z = center_z
cent = (center_x, center_y, center_z)
# get parameters
dropbox_param = rospy.get_param('/dropbox')
place_pose = Pose()
# check which box the object is to be dropped into and assign that boxes arm
arm_name = String()
if (object_group == dropbox_param[0]['group']):
# Assign the left arm to be used for pick_place
arm_name.data = dropbox_param[0]['name']
# Create the objects final resting position to be placed
# offset each item so they do not stack on top of each other
# TODO: better object drop location tracking to prevent objects from dropping outside the boxes with large picking list.
place_pose.position.x = dropbox_param[0]['position'][
0] - 0.03 - 0.05 * i
place_pose.position.y = dropbox_param[0]['position'][
1] + 0.03 * i
place_pose.position.z = dropbox_param[0]['position'][2]
elif (object_group == dropbox_param[1]['group']):
# Assign the right arm to be used for pick_place
arm_name.data = dropbox_param[1]['name']
# Create the objects final resting position to be placed
# TODO: As described in the above TODO. better drop location tracking
place_pose.position.x = dropbox_param[1]['position'][
0] - 0.03 - 0.05 * i
place_pose.position.y = dropbox_param[1]['position'][
1] - 0.06 * i
place_pose.position.z = dropbox_param[1]['position'][2]
# make a dictionary of the robot and objects movement data
yaml_dict = {}
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)
# send a service request for the objects pick and place movement
# pick_pose - the position of the object location in which it will be picked up
# place_pose - the location of the final place it is to be dropped at
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 set_status(self, status):
self.STATUS = String(status.name)
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
import rospy
from superros.comm import RosNode
from std_msgs.msg import String
def topic_collback(msg):
print msg.data
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("tf_export_test")
node.setupParameter("hz", 30)
node.setHz(node.getParameter("hz"))
topic_pub = node.createPublisher("/topic_1", String)
node.createSubscriber("/topic_2", String, topic_collback)
while node.isActive():
msg = String()
msg.data = "hello world"
topic_pub.publish(msg)
node.tick()
def emit(self, record):
message = self.format(record)
self._pub.publish(self._topic, String(message))
import rospy
from std_msgs.msg import String
rospy.init_node('no2')
mat_string = String()
def subCallBack(msg):
global mat_string
mat_string = msg
def timerCallBack(event):
soma = 0
for i in range(len(mat_string.data)):
soma = soma + int(mat_string.data[i])
#print('somando matricula . . . ('+mat_string.data + ')')
msg_soma = String()
msg_soma.data = str(soma)
pub.publish(msg_soma)
sub = rospy.Subscriber('/no1/mat', String, subCallBack)
pub = rospy.Publisher('no2/sum', String, queue_size=1)
timer = rospy.Timer(rospy.Duration(1), timerCallBack)
rospy.spin()
def index():
msg = String()
msg.data = 1
return 'Social Robot Dialogflow HYU Homecare'
def execute_move_action(self, req):
action_str = req.actions
print action_str
self.action_publisher.publish(String(data=action_str))
return self.success
def cam_pose_callback(data):
global theta
q = Quaternion(data.pose.orientation.w, data.pose.orientation.x,
data.pose.orientation.y, data.pose.orientation.z)
theta = q.yaw_pitch_roll[1]
if __name__ == "__main__":
height = 0
target_height = 0
target_set = False
find = False
not_find_time = 0
get_time = False
twist = Twist()
cmd = String()
theta = 0
u_center = 640 / 2
v_center = 360 / 2
fx = 205.46963709898583
fy = 205.46963709898583
Kp_xy = 0.5
Kp_z = 1
vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
rospy.init_node('yolo_human_tracking')
rospy.Subscriber("/uav_0/darknet_ros/bounding_boxes", BoundingBoxes,
darknet_callback)
rospy.Subscriber(
vehicle_type + '_' + vehicle_id + "/mavros/local_position/pose",
PoseStamped, local_pose_callback)
def __init__(self):
rospy.init_node("avoid_path")
self.goalListX = rospy.get_param('~goalListX', '0.0, 0.0')
self.goalListY = rospy.get_param('~goalListY', '0.0, 0.0')
self.goalListZ = rospy.get_param('~goalListZ', '0.0, 0.0')
self.goalListYaw = rospy.get_param('~goalListYaw', '0.0, 0.0')
self.goals = [[
float(x), float(y), float(z),
float(yaw)
] for (
x, y, z,
yaw) in zip(self.goalListX.split(","), self.goalListY.split(
","), self.goalListZ.split(","), self.goalListYaw.split(","))]
self.imu = None
self.current_heading = None
self.received_imu = False
self.local_pose = None
self.local_pose_body = PoseStamped()
self.target_pose = PoseStamped()
self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
self.local_pose_callback)
self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu,
self.imu_callback)
self.custom_activity_pub = rospy.Publisher('/set_activity/type',
String,
queue_size=10)
self.position_target_pub = rospy.Publisher('/set_pose/position',
PoseStamped,
queue_size=10)
self.target_point = 0
time.sleep(1)
self.custom_activity_pub.publish(String("HOVER"))
time.sleep(2)
check_str = raw_input("Press 1 to continue: ")
self.init_pose_x = self.local_pose.pose.position.x
self.init_pose_y = self.local_pose.pose.position.y
print "input str is: ", check_str
if check_str != str(1):
return
print("init_pose_x: " + str(self.init_pose_x) + " init_pose_y: " +
str(self.init_pose_y))
self.move(self.goals[0][0] - self.init_pose_x,
self.goals[0][1] - self.init_pose_y, self.goals[0][2])
self.update_target_pose(self.target_point)
rospy.loginfo("moving to target_point: 0")
time.sleep(5)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if (self.target_point > 3):
self.custom_activity_pub.publish(String("HOVER"))
else:
self.target_point += 1
self.move(self.goals[self.target_point][0] - self.init_pose_x,
self.goals[self.target_point][1] - self.init_pose_y,
self.goals[self.target_point][2])
self.update_target_pose(self.target_point)
rospy.loginfo("moving to target_point: " +
str(self.target_point))
time.sleep(5)
r.sleep()
def cruise_control(self, car1, car2):
test = String()
ack_msg = AckermannDriveStamped()
rate = rospy.Rate(PUB_RATE)
raw_distance = self.get_distance(car1.pose.position.x,
car1.pose.position.y,
car2.pose.position.x,
car2.pose.position.y)
#Adding gaussian noise to signal
distance = np.random.normal(raw_distance, self.sigma, 1)
self.SNRBuffer.append((distance / (distance - raw_distance)))
#================================= PID CONRTOLLER (PD)
# offSet = distance - 3
# self.PIDBuf.append(offSet)
# if len(self.PIDBuf) >= PIDDistBufSize:
# self.dedt = (self.PIDBuf[PIDDistBufSize-1] - self.PIDBuf[0])/(PIDdt)
# self.integral = 0.01*self.PIDBuf[0] + ((self.PIDBuf[4]-self.PIDBuf[0])*0.01/2)
# self.PIDBuf = []
# #proportional gain = how strongly the car wants to to desired offset distance (accel)
# #derivative gain = how strongly the car resists too fast changes
# # for bufsize >=5 dt = 0.01 use kp = 0.1 kd = 0.01
# self.speed += 0.05*offSet + 0.0*self.integral + 0.0035*self.dedt
# # self.plotDist.append(self.speed)
# # if len(self.plotDist) == 50:
# # self.cpyPlot = self.plotDist
# # self.plot(self.cpyPlot)
# ack_msg.header.stamp = rospy.Time.now()
# ack_msg.header.frame_id = '/map'
# ack_msg.drive.steering_angle = 0
# ack_msg.drive.speed = self.speed
# self.pub.publish(ack_msg)
# test = 'car2 speed: ' + str(self.speed) + ' distance : ' + str(distance) + ' offset' + str(offSet)
# self.pub2.publish(test)
#=========================================================
#####################################################################################################
self.distBuf.append(distance)
bufLength = len(self.distBuf)
##############=====================
#detect change to sync with change of speed
if bufLength >= 2:
diffLog = self.distBuf[self.countdT] - self.distBuf[self.countdT -
1]
if abs(diffLog) > 0.0075:
self.countdT = 0
deltaD = self.distBuf[len(self.distBuf) - 1] - self.distBuf[0]
deltaT = float(bufLength) / PUB_RATE
AVGSpeed = (deltaD / deltaT) / 5 + self.prevSpd
if AVGSpeed > 0.6:
if AVGSpeed >= 2.15:
AVGSpeed = 2.3
else:
AVGSpeed = 2
self.distBuf = []
self.speed = AVGSpeed
self.prevSpd = AVGSpeed
self.countdT += 1
test = 'Car2 Speed: ' + str(
self.speed) + ' distance : ' + str(distance)
self.pub2.publish(test)
# self.plotDist.append(self.speed)
# if len(self.plotDist) == 100:
# self.cpyPlot = self.plotDist
# self.plot(self.cpyPlot)
ack_msg.header.stamp = rospy.Time.now()
ack_msg.header.frame_id = '/map'
ack_msg.drive.steering_angle = 0
ack_msg.drive.speed = self.speed
self.pub.publish(ack_msg)
# -----------------evaluation----------------
self.evaluateArray.append(self.speed)
if len(self.evaluateArray) >= 200:
self.cpyPlot2 = self.evaluateArray
self.evaluate(self.cpyPlot2)
from tensorflow.keras.applications.resnet50 import ResNet50, preprocess_input, decode_predictions
# import model and implement fix found here.
# https://github.com/fchollet/keras/issues/2397
model = ResNet50(weights='imagenet')
model._make_predict_function()
#graph = tf.compat.v1.get_default_graph()
target_size = (224, 224)
rospy.init_node('classify', anonymous=True)
#These should be combined into a single message
pub = rospy.Publisher('/object_detected', String, queue_size = 1)
pub1 = rospy.Publisher('/object_detected_probability', Float32, queue_size = 1)
bridge = CvBridge()
msg_string = String()
msg_float = Float32()
def callback(image_msg):
#First convert the image to OpenCV image
cv_image = bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
cv_image = cv2.resize(cv_image, target_size) # resize image
np_image = np.asarray(cv_image) # read as np array
np_image = np.expand_dims(np_image, axis=0) # Add another dimension for tensorflow
np_image = np_image.astype(float) # preprocess needs float64 and img is uint8
np_image = preprocess_input(np_image) # Regularize the data
# global graph # This is a workaround for asynchronous execution
# with graph.as_default():
#! /usr/bin/env python
import rospy
from std_msgs.msg import String
rospy.init_node('node2')
pub = rospy.Publisher('myTopic', String, queue_size=10)
rate = rospy.Rate(1)
my_msg = String()
my_msg.data = "Ahmad"
while not rospy.is_shutdown():
pub.publish(my_msg)
rate.sleep()
def pr2_mover(object_list, pcl_table):
# TODO: Initialize variables
test_scene_num = Int32()
test_scene_num.data = 2 # is there a way to determine this automatically?
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
yaml_list = []
# build a dictionary from object labels to centroids
centroids = {}
for object in object_list:
label = object.label
points_arr = ros_to_pcl(object.cloud).to_array()
centroids[label] = np.mean(points_arr, axis=0)[:3]
centroids[label] = [np.asscalar(c) for c in centroids[label]]
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
left_box_pos = None
right_box_pos = None
for box in dropbox_param:
if box['name'] == 'left':
left_box_pos = box['position']
elif box['name'] == 'right':
right_box_pos = box['position']
# 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:
object_name_param = object['name']
object_group_param = object['group']
object_name.data = object_name_param
arm_name.data = 'left' if object_group_param == 'red' else 'right'
collision_cloud = pcl.PointCloud_PointXYZRGB()
collision_point_list = [p for p in pcl_table]
# TODO: fix up collision so that previous objects don't stick around
# current_obj_idx = None
# for idx, obj in enumerate(object_list):
# if obj.label != object_name_param:
# pcl_obj = ros_to_pcl(obj.cloud)
# for pt in pcl_obj:
# collision_point_list.append(pt)
# else:
# current_obj_idx = idx
# del object_list[current_obj_idx]
collision_cloud.from_list(collision_point_list)
ros_collision_cloud = pcl_to_ros(collision_cloud)
collision_map_pub.publish(ros_collision_cloud)
# TODO: Get the PointCloud for a given object and obtain it's centroid
if object_name_param in centroids:
centroid = centroids[object_name_param]
else:
print("Failed to identify {} in detected objects list".format(
object_name_param))
continue
# pick_pose.position = Point()
pick_pose.position.x = centroid[0]
pick_pose.position.y = centroid[1]
pick_pose.position.z = centroid[2]
# TODO: Create 'place_pose' for the object
target_box = left_box_pos if arm_name.data == 'left' else right_box_pos
# place_pose.position = Point()
place_pose.position.x = target_box[0]
place_pose.position.y = target_box[1]
place_pose.position.z = target_box[2]
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_list.append(
make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose,
place_pose))
# NB: Skipping the actual pick and place routing due to limitations in the
# configuration of the simulator
# 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: Output your request parameters into output yaml file
send_to_yaml('output_{}.yaml'.format(test_scene_num.data), yaml_list)
def pr2_mover(object_list):
# TODO: Initialize variables
labels = []
centroids = []
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
drop_items = rospy.get_param('/dropbox')
# create a dictionary with color green as the key
drop_position = dict()
for drop_item in drop_items:
drop_position[drop_item['group']] = (drop_item['name'],
drop_item['position'])
test_scene_num = Int32()
test_scene_num.data = 3
dict_list = []
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for pick_object in object_list_param:
for object in object_list:
if object.label == pick_object['name']:
# if their is a match create msg
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
# name
object_name.data = pick_object['name']
# pickpose
# Get the PointCloud for a given object and obtain it's centroid
points_arr = ros_to_pcl(object.cloud).to_array()
tmp = np.mean(points_arr, axis=0)[:3]
pick_pose.position.x = np.asscalar(tmp[0])
pick_pose.position.y = np.asscalar(tmp[1])
pick_pose.position.z = np.asscalar(tmp[2])
labels.append(object.label)
centroids.append(tmp)
# based on group (green or red) place object in left or right bins
object_group = pick_object['group']
arm_name.data = drop_position[object_group][0]
position = drop_position[object_group][1]
eps1 = 0.01
new_pos = [p + eps1 * np.random.rand() for p in position]
place_pose.position.x = new_pos[0]
place_pose.position.y = new_pos[1]
place_pose.position.z = new_pos[2]
yaml_dict = make_yaml_dict(test_scene_num, arm_name,
object_name, pick_pose, place_pose)
dict_list.append(yaml_dict)
yaml_filename = '/home/robond/catkin_ws/output_{}.yaml'.format(
test_scene_num.data)
send_to_yaml(yaml_filename, dict_list)
