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 pub_msg(self, num):
msg = String()
msg.data = 'detect {} people'.format(num)
self.publisher_.publish(msg)
self.get_logger().info('Publishing: "%s"' % msg.data)
import rospy
from std_msgs.msg import String
NAME_TOPIC = '/msgs_talk'
NAME_NODE = 'pub_node'
if __name__ == '__main__':
pub = rospy.Publisher(NAME_TOPIC, String, queue_size=10)
rospy.init_node(NAME_NODE, anonymous=True)
rate = rospy.Rate(10) # 10hz
msgs_pub = String()
while not rospy.is_shutdown():
msgs_pub.data = "hello ROS world"
pub.publish(msgs_pub)
rate.sleep()
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 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 commandPub(self, s):
cmdMsg = String()
cmdMsg.data = s
self.cmdPub.publish(cmdMsg)
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()
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 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)
import rospy
import mavros_msgs
from mavros_msgs import srv
from geometry_msgs.msg import PoseStamped, TwistStamped
from std_msgs.msg import String
from mavros_msgs.msg import State
goal_pose = PoseStamped()
current_state = State()
velocity_set = TwistStamped()
current_pose = PoseStamped()
controle = String()
def set_position(x, y, z):
global goal_pose
goal_pose.pose.position.x = x
goal_pose.pose.position.y = y
goal_pose.pose.position.z = z
local_position_pub.publish(goal_pose)
def state_callback(state_data):
global current_state
current_state = state_data
def pose_callback(pose_data):
global current_pose
current_pose = pose_data
import sys
from simple_ui_form import Ui_Form
#port imports
import serial.tools.list_ports as port_list
#Flags Publishers
species_flag_pub = rospy.Publisher('species_flag', String, queue_size=10)
line_follower_flag_pub = rospy.Publisher('line_follower_flag', String, queue_size=10)
cannon_flag_pub = rospy.Publisher('cannon_flag', String, queue_size=10)
judge_canon_pub = rospy.Publisher('cannon_numbers', cannonNumbers, queue_size=10)
#global variables
cannon_numbers = cannonNumbers()
species_count = Num()
joy_values = String()
rov_msg = rov_msgs()
uvc_img = Image()
crack_length = ''
#Text Variables
font = cv2.FONT_HERSHEY_SIMPLEX
bottomLeftCornerOfText = (10,500)
fontScale = 1
fontColor = (0,0,255)
lineType = 2
# Instantiate CvBridge, object
bridge = CvBridge()
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)
def joystick_send(self, string_data):
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.sendto(string_data, (UDP_IP, UDP_Port))
self.lastMessageTime = time.clock()
joyPub.publish(String(string_data))
def publish_mode():
msg = String()
for i in group['mode']:
if sensors[i] == True:
msg.data = i
mode_publisher.publish(msg)
def test_dictionary_with_string(self):
from std_msgs.msg import String
expected_message = String(data = 'Hello')
dictionary = { 'data': expected_message.data }
message = message_converter.convert_dictionary_to_ros_message('std_msgs/String', dictionary)
self.assertEqual(message, expected_message)
import PAL_clients as pal
import geo
# Global variables
stop_flag = False
auto_download = False
loaded_mission = False
flight_status = UInt8()
current_pos = PointStamped()
current_gps_pos = NavSatFix()
battery_state = BatteryState()
current_vel = Vector3Stamped()
ual_state = UInt8()
last_ual_state = UInt8()
adl_state = String()
## Json files with mission information
mission_status_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_status.json'
mission_waypoints_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_wps.json'
mission_data_file = os.path.expanduser("~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_data.json'
print mission_waypoints_file
## PAL srv Clients ##
# # Define PAL Clients object
# pal = PALClients()
# uav_id = rospy.get_param('~uav_id', 'uav_1')
# acept_radio = rospy.get_param('~acept_radio', 1.2)
def feedbackPub(self, s):
fbMsg = String()
fbMsg.data = s
self.fbPub.publish(fbMsg)
def speech_cb(self, msg):
""" ROS Callbacks """
speech = msg.utterance
on = (self.state == 'interacting') or (self.state == 'performing' and self.config['chat_during_performance'])
# Special states keywords
if self.state == 'opencog':
if self.check_keywords(self.OPENCOG_EXIT, speech):
self.to_interacting()
self.speech_pub.publish(msg)
if self.check_keywords(self.OPENCOG_ENTER, speech):
try:
self.start_opencog()
except Exception:
pass
self.speech_pub.publish(msg)
if 'go to sleep' in speech:
try:
self.btree_pub.publish(String("btree_off"))
# use to_performng() instead of perform() so it can be called from other than interaction states
self.to_performing()
self.after_performance = self.to_sleeping
self.performance_runner('shared/sleep')
return False
except:
pass
if 'wake' in speech or 'makeup' in speech:
try:
self.do_wake_up()
return False
except:
pass
if 'shutdown' in speech:
try:
self.shut()
return False
except:
pass
if 'be quiet' in speech:
try:
self.be_quiet()
return False
except:
pass
if 'hi sophia' in speech or \
'hey sophia' in speech or \
'hello sofia' in speech or \
'hello sophia' in speech or \
'hi sofia' in speech or \
'hey sofia' in speech:
try:
self.start_talking()
self.speech_pub.publish(msg)
except:
pass
# Try wake up
try:
self.do_wake_up()
return False
except:
pass
if ('go to analysis mode' in speech) or ('analysis mode' in speech) or (speech == 'analysis'):
try:
self.to_analysis()
return True
except:
pass
if ('exit' in speech) or ('return' in speech) or ('resume' in speech) or ('normal mode' in speech):
try:
rospy.logerr("Exiting interaction mode")
self.to_interacting()
return True
except:
pass
performances = self.find_performance_by_speech(speech)
# Split between performances for general modes and analysis
analysis_performances = [p for p in performances if ('shared/analysis' in p or 'robot/analysis' in p)]
for a in analysis_performances:
performances.remove(a)
if len(performances) > 0:
try:
self.perform()
on = False
self.performance_runner(random.choice(performances))
except:
pass
if self.state == 'analysis' and len(analysis_performances):
# Run performances explicitly in the analysis state (Only testing performances)
on = False
self.performance_runner(random.choice(analysis_performances))
if on and self.filter_stt(msg):
self.speech_pub.publish(msg)
def speakPub(self, s):
speakMsg = String()
speakMsg.data = s
self.espeakPub.publish(speakMsg)
def do_wake_up(self):
assert (self.state == 'sleeping')
self.btree_pub.publish(String("btree_on"))
self.after_performance = self.to_interacting
# Start performance before triggerring state change so soma state will be sinced with performance
self.performance_runner('shared/wakeup')
def _get_string(self, px4_cmd):
cmd_string = String()
cmd_string.data = px4_cmd
return cmd_string
def on_enter_opencog(self):
self.btree_pub.publish(String("opencog_on"))
self.set_chatbot_enabled(False)
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 on_exit_opencog(self):
self.btree_pub.publish(String("opencog_off"))
self.set_chatbot_enabled(True)
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 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 resetAlarm(self):
msg_str = String()
self.alarm_rest_pub.publish(msg_str)
def test_ros_message_with_string(self):
from std_msgs.msg import String
expected_dictionary = { 'data': 'Hello' }
message = String(data=expected_dictionary['data'])
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
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 changeLFParams(self, params, reset_time):
data = {"params": params, "time": reset_time}
msg = String()
msg.data = json.dumps(data)
self.pub_LF_params.publish(msg)
