def grasp_target_action_result_cb(userdata, status, result):
# here parse the result and compose a ObjectPoseList of what we found and return it in the output key object_found
obj = ObjectPoseList()
obj.header = result.detectionResult.table.pose.header # just copying some header
num_object = 0
i = 0
try:
for i in range(0, 10): # there wont be more objects, come on!
if result.detectionResult.models[i].model_list[0].model_id == userdata.object_id_from_name or userdata.object_id_from_name <= 0: # I think we only get one returned anyways
op = ObjectPose()
obj.object_list.append(op)
if userdata.object_id_from_name > 0:
obj.object_list[num_object].name = userdata.object_to_search_for
obj.object_list[num_object].pose = result.detectionResult.models[i].model_list[0].pose.pose
num_object += 1
break
else:
rospy.loginfo("Going to translate ID: " + str(result.detectionResult.models[i].model_list[0].model_id))
rospy.wait_for_service('object_translator_dataBase')
obj_trans_db = rospy.ServiceProxy('object_translator_dataBase', ObjectTranslatorDataBase)
obj_info = obj_trans_db(result.detectionResult.models[i].model_list[0].model_id)
#rospy.loginfo("Got info from db: " + str(obj_info))
if obj_info.objname == '':
obj.object_list[num_object].name = 'unknown'
else:
obj.object_list[num_object].name = obj_info.objname
obj.object_list[num_object].pose = result.detectionResult.models[i].model_list[0].pose.pose
num_object += 1
except Exception as e:
rospy.loginfo("Exception when looping over result.detectionResult.models[" + str(i) + "]")
rospy.loginfo("result.detectionResult.models[i] looks like:\n" + str(result.detectionResult.models[i]))
if str(e) != "list index out of range":
rospy.loginfo("Got exception: " + str(e))
pass # this exception should be that we are looping too far...
userdata.object_found = obj
rospy.loginfo("userdata.object_found generated as: " + str(obj))
if status == GoalStatus.SUCCEEDED and num_object > 0:
return succeeded
else:
global OUTPUT
OUTPUT = GRASP_TARGET_ACTION_FAILED
return aborted
def ObjectRecognitionPublisher():
print "object_recognition_mock running..."
pub = rospy.Publisher('/iri_moped_handler/outputOPL', ObjectPoseList)
rate = rospy.Rate(10) # Hz
# change to my msg
msg = ObjectPoseList()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "/head_2_link" # "/head_mount_xtion_optical_frame"
msg.object_list = [None] * 7
msg.object_list[0] = ObjectPose()
msg.object_list[0].name = "coke-close"
msg.object_list[0].pose = Pose()
# 1) Old secure pose in reference to /base_link
# msg.object_list[0].pose.position.x = 0.30
# msg.object_list[0].pose.position.y = -0.30
# msg.object_list[0].pose.position.z = 1.13
# msg.object_list[0].pose.orientation.x = 0.5
# msg.object_list[0].pose.orientation.y = -0.5
# msg.object_list[0].pose.orientation.z = 0.5
# msg.object_list[0].pose.orientation.w = -0.5
# Position of hand in front in reference of /kinect_link
# msg.object_list[0].pose.position.x = 0.222509
# msg.object_list[0].pose.position.y = -0.551083 # was positive before
# msg.object_list[0].pose.position.z = -0.764526
# msg.object_list[0].pose.orientation.x = 0.081764
# msg.object_list[0].pose.orientation.y = -0.38946
# msg.object_list[0].pose.orientation.z = 0.714737
# msg.object_list[0].pose.orientation.w = 0.575141
# Real detection pose in reference to /kinect_link
# msg.object_list[0].pose.position.x = 0.0231333337724
# msg.object_list[0].pose.position.y = 0.0350304767489
# msg.object_list[0].pose.position.z = 0.694000005722
# msg.object_list[0].pose.orientation.x = 0.946857988834
# msg.object_list[0].pose.orientation.y = -0.0297971088439
# msg.object_list[0].pose.orientation.z = -0.295009255409
# msg.object_list[0].pose.orientation.w = 0.12466647476
# Manually generated pose in reference of /openni_depth_frame from
# Asking tf to transform the 1) coords from base_link to openni_depth_frame
msg.object_list[0].pose.position.x = 0.611990241005
msg.object_list[0].pose.position.y = -0.317951137319
msg.object_list[0].pose.position.z = -0.38978881074
msg.object_list[0].pose.orientation.x = 0.946857988834
msg.object_list[0].pose.orientation.y = -0.0297971088439
msg.object_list[0].pose.orientation.z = -0.295009255409
msg.object_list[0].pose.orientation.w = 0.12466647476
# name: coke-close
#pose:
# position:
# x: 0.0231333337724
# y: 0.0350304767489
# z: 0.694000005722
#orientation:
# x: 0.946857988834
# y: -0.0297971088439
# z: -0.295009255409
# w: 0.12466647476
#pose2D:
# x: 337.68838501
# y: 266.798492432
# z: 0.0
msg.object_list[1] = ObjectPose()
msg.object_list[1].name = "milk"
msg.object_list[1].pose = Pose()
msg.object_list[1].pose.position.x = 0.30
msg.object_list[1].pose.position.y = -0.30
msg.object_list[1].pose.position.z = 1.13
msg.object_list[1].pose.orientation.x = 0.5
msg.object_list[1].pose.orientation.y = -0.5
msg.object_list[1].pose.orientation.z = 0.5
msg.object_list[1].pose.orientation.w = -0.5
msg.originalTimeStamp = rospy.Time.now()
msg.object_list[2] = ObjectPose()
msg.object_list[2].name = "redbull"
msg.object_list[2].pose = Pose()
msg.object_list[2].pose.position.x = 0.30
msg.object_list[2].pose.position.y = -0.30
msg.object_list[2].pose.position.z = 1.13
msg.object_list[2].pose.orientation.x = 0.5
msg.object_list[2].pose.orientation.y = -0.5
msg.object_list[2].pose.orientation.z = 0.5
msg.object_list[2].pose.orientation.w = -0.5
msg.originalTimeStamp = rospy.Time.now()
msg.object_list[3] = ObjectPose()
msg.object_list[3].name = "juice"
msg.object_list[3].pose = Pose()
msg.object_list[3].pose.position.x = 0.30
msg.object_list[3].pose.position.y = -0.30
msg.object_list[3].pose.position.z = 1.13
msg.object_list[3].pose.orientation.x = 0.5
msg.object_list[3].pose.orientation.y = -0.5
msg.object_list[3].pose.orientation.z = 0.5
msg.object_list[3].pose.orientation.w = -0.5
msg.originalTimeStamp = rospy.Time.now()
msg.object_list[4] = ObjectPose()
msg.object_list[4].name = "beer"
msg.object_list[4].pose = Pose()
msg.object_list[4].pose.position.x = 0.30
msg.object_list[4].pose.position.y = -0.30
msg.object_list[4].pose.position.z = 1.13
msg.object_list[4].pose.orientation.x = 0.5
msg.object_list[4].pose.orientation.y = -0.5
msg.object_list[4].pose.orientation.z = 0.5
msg.object_list[4].pose.orientation.w = -0.5
msg.originalTimeStamp = rospy.Time.now()
msg.object_list[5] = ObjectPose()
msg.object_list[5].name = "water"
msg.object_list[5].pose = Pose()
msg.object_list[5].pose.position.x = 0.30
msg.object_list[5].pose.position.y = -0.30
msg.object_list[5].pose.position.z = 1.13
msg.object_list[5].pose.orientation.x = 0.5
msg.object_list[5].pose.orientation.y = -0.5
msg.object_list[5].pose.orientation.z = 0.5
msg.object_list[5].pose.orientation.w = -0.5
msg.originalTimeStamp = rospy.Time.now()
msg.object_list[6] = ObjectPose()
msg.object_list[6].name = "wine"
msg.object_list[6].pose = Pose()
msg.object_list[6].pose.position.x = 0.30
msg.object_list[6].pose.position.y = -0.30
msg.object_list[6].pose.position.z = 1.13
msg.object_list[6].pose.orientation.x = 0.5
msg.object_list[6].pose.orientation.y = -0.5
msg.object_list[6].pose.orientation.z = 0.5
msg.object_list[6].pose.orientation.w = -0.5
msg.originalTimeStamp = rospy.Time.now()
while not rospy.is_shutdown():
pub.publish(msg)
print "Publishing close_object msg with ObjectName: " + msg.object_list[0].name
rate.sleep()
def grasp_target_result_cb(userdata, status, result):
#rospy.loginfo("Result of the cb: result: " + str(result) )
# result is:
# int32 status
# copy of the tabletop detection result
# tabletop_object_detector/TabletopDetectionResult detectionResult
# int32 NO_CLOUD_RECEIVED=1
# int32 NO_TABLE=2
# int32 OTHER_ERROR=3
# int32 SUCCESS=4
# tabletop_object_detector/Table table
# geometry_msgs/PoseStamped pose
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# geometry_msgs/Pose pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
# float32 x_min
# float32 x_max
# float32 y_min
# float32 y_max
# arm_navigation_msgs/Shape convex_hull
# byte SPHERE=0
# byte BOX=1
# byte CYLINDER=2
# byte MESH=3
# byte type
# float64[] dimensions
# int32[] triangles
# geometry_msgs/Point[] vertices
# float64 x
# float64 y
# float64 z
# sensor_msgs/PointCloud[] clusters
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# geometry_msgs/Point32[] points
# float32 x
# float32 y
# float32 z
# sensor_msgs/ChannelFloat32[] channels
# string name
# float32[] values
# household_objects_database_msgs/DatabaseModelPoseList[] models
# household_objects_database_msgs/DatabaseModelPose[] model_list
# int32 model_id
# geometry_msgs/PoseStamped pose
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# geometry_msgs/Pose pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
# float32 confidence
# string detector_name
# int32[] cluster_model_indices
# int32 result
# here parse the result and compose a ObjectPoseList of what we found and return it in the output key object_found
obj = ObjectPoseList()
# Header header
# ObjectPose[] object_list
# time originalTimeStamp
# time requestTimeStamp
obj.header = result.detectionResult.table.pose.header # just copying some header
####obj.object_list = [None] * 10 # ugly way to allocate space in python
# obj.object_list[0] = ObjectPose() # only putting the "interesting result" for now
# string name
# geometry_msgs/Pose pose
# geometry_msgs/Point32 pose2D
# int16[] convex_hull_x
# int16[] convex_hull_y
# float32 mean_quality
# int16 used_points
# NameTypeValue[] properties
# geometry_msgs/Pose[] pose_uncertainty_list
#obj.object_list[0].name = userdata.object_to_search_for
#rospy.loginfo("result.detectionResult.models:\n" + str(result.detectionResult.models))
num_object = 0
i=0
try:
for i in range(0, 10): # there wont be more objects, come on!
if result.detectionResult.models[i].model_list[0].model_id == userdata.object_id_from_name or userdata.object_id_from_name <= 0: # I think we only get one returned anyways
op = ObjectPose()
obj.object_list.append(op)
if userdata.object_id_from_name > 0:
obj.object_list[num_object].name = userdata.object_to_search_for
obj.object_list[num_object].pose = result.detectionResult.models[i].model_list[0].pose.pose
num_object += 1
break
else:
rospy.loginfo("Going to translate ID: " + str(result.detectionResult.models[i].model_list[0].model_id))
rospy.wait_for_service('object_translator_dataBase')
obj_trans_db = rospy.ServiceProxy('object_translator_dataBase', ObjectTranslatorDataBase)
obj_info = obj_trans_db(result.detectionResult.models[i].model_list[0].model_id)
#rospy.loginfo("Got info from db: " + str(obj_info))
if obj_info.objname == '':
obj.object_list[num_object].name = 'unknown'
else:
obj.object_list[num_object].name = obj_info.objname
obj.object_list[num_object].pose = result.detectionResult.models[i].model_list[0].pose.pose
num_object += 1
except Exception as e:
# rospy.loginfo("Exception when looping over result.detectionResult.models[" + str(i) + "]")
# rospy.loginfo("result.detectionResult.models[i] looks like:\n" + str(result.detectionResult.models[i]))
if str(e) != "list index out of range":
rospy.loginfo("Got exception: " + str(e))
pass # this exception should be that we are looping too far...
# for item, value in result.detectionResult.models.items(): # AttributeError: 'list' object has no attribute 'items' ITS A F*****G LIST??? WTF
# rospy.loginfo("item: " + item + "\n value: " + value)
# for i in range(len(result.detectionResult.models)): # TypeError: object of type 'DatabaseModelPoseList' has no len()
# for j in range(len(result.detectionResult.models[i])):
# if result.detectionResult.models[i].model_list[j].model_id == userdata.object_id_from_name:
# obj.object_list[0].pose = result.detectionResult.models[i].model_list[j].pose.pose
# CANT DO THIS BECAUSE PYTHON KEEPS SAYING THIS IS NOT ITERABLE /CRY
# for model_list in result.detectionResult.models: # TypeError: 'DatabaseModelPoseList' object is not iterable # maybe this is a UserDict?
# for model in model_list:
# if model.model_id == userdata.object_id_from_name:
# obj.object_list[0].pose = model.pose.pose
userdata.object_found = obj
rospy.loginfo("userdata.object_found generated as: " + str(obj) )
if status == GoalStatus.SUCCEEDED and num_object > 0:
return succeeded
else:
err = "Grasp target action failed. "
err += ("Status: %s" % str(status)) if status != GoalStatus.SUCCEEDED else ("num_object: %s" % str(num_object) )
set_grasp_error(err)
return aborted
def arm_goal_cb(userdata, old_goal):
print "Preparing MoveArmGoal() on SM_give"
arm_goal = MoveArmGoal()
objposlist = ObjectPoseList()
objposlist.object_list = [None]
# Pose with arm in front of the robot
objposlist.object_list[0] = ObjectPose()
objposlist.object_list[0].pose.position.x = 0.30
objposlist.object_list[0].pose.position.y = -0.30
objposlist.object_list[0].pose.position.z = 1.13
objposlist.object_list[0].pose.orientation.x = 0.5
objposlist.object_list[0].pose.orientation.y = -0.5
objposlist.object_list[0].pose.orientation.z = 0.5
objposlist.object_list[0].pose.orientation.w = -0.5
# Reading from the object_location
#objposlist.object_list[0] = ObjectPose()
#objposlist.object_list[0].pose.position = userdata.object_location_3d.position
#rospy.loginfo("I'm attempting to move the arm to position:" + str(userdata.object_location_3d.position.x) + " " + str(userdata.object_location_3d.position.y) + " " + str(userdata.object_location_3d.position.z) )
#objposlist.object_list[0].pose.orientation = userdata.pose_object.object_list[0].pose.orientation
#rospy.loginfo("I'm attempting to move the arm to orientation: " + str(userdata.pose_object.object_list[0].pose.orientation.x) + " " + str(userdata.pose_object.object_list[0].pose.orientation.y) + " " + str(userdata.pose_object.object_list[0].pose.orientation.z) + " " + str(userdata.pose_object.object_list[0].pose.orientation.w))
######## Message formed from the example message: http://pastebin.com/zcrWcaih ###########
arm_goal.planner_service_name = "ompl_planning/plan_kinematic_path"
#Planning Scene diff: empty!
#motion_plan_request
arm_goal.motion_plan_request = MotionPlanRequest()
# workspace_parameters, empty until we get to goal_constaints
arm_goal.motion_plan_request.workspace_parameters = WorkspaceParameters()
#arm_goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position = objposlist.object_list[0].pose.position
#arm_goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation = objposlist.object_list[0].pose.orientation #userdata.object_data.object_list[0].pose.position
arm_goal.motion_plan_request.goal_constraints.position_constraints = [None] * 1
arm_goal.motion_plan_request.goal_constraints.position_constraints[0] = PositionConstraint()
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].header = Header()
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = rospy.Time().now()
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "/base_link" #FIXME: This is ALWAYS GONNA BE BASE_LINK??? #userdata.object_location.header.frame_id
#userdata.object_data.header.frame_id#"torso_lift_link"
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].link_name = "arm_right_7_link"
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position.x = objposlist.object_list[0].pose.position.x
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position.y = objposlist.object_list[0].pose.position.y
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position.z = objposlist.object_list[0].pose.position.z
# Not used O.o
# arm_goal.motion_plan_request.goal_constraints.absolute_position_tolerance.x = 0.1;
# arm_goal.motion_plan_request.goal_constraints.absolute_position_tolerance.y = 0.1;
# arm_goal.motion_plan_request.goal_constraints.absolute_position_tolerance.z = 0.1;
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = 1 # 1 should be box
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions = [None] * 3
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions[0] = 0.040000000000000001
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions[1] = 0.040000000000000001
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions[2] = 0.040000000000000001
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0
arm_goal.motion_plan_request.goal_constraints.orientation_constraints = [None] * 1
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0] = OrientationConstraint()
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header = Header()
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = rospy.Time().now()
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "/base_link"
#userdata.object_data.header.frame_id
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "arm_right_7_link"
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = objposlist.object_list[0].pose.orientation.x
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = objposlist.object_list[0].pose.orientation.y
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = objposlist.object_list[0].pose.orientation.z
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = objposlist.object_list[0].pose.orientation.w
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.02
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.02
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.02
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0
arm_goal.motion_plan_request.planner_id = ""
arm_goal.motion_plan_request.group_name = "right_arm_torso"
arm_goal.motion_plan_request.num_planning_attempts = 1
arm_goal.motion_plan_request.allowed_planning_time = rospy.Duration(10)
arm_goal.motion_plan_request.expected_path_duration = rospy.Duration(0)
arm_goal.motion_plan_request.expected_path_dt = rospy.Duration(0)
print "After this print the message is sent!"
return arm_goal
def arm_goal_cb(userdata, old_goal):
print "Preparing MoveArmGoal() to move the arm to a safe arm position"
arm_goal = MoveArmGoal()
objposlist = ObjectPoseList()
objposlist.object_list = [None]
# Pose with arm down
objposlist.object_list[0] = ObjectPose()
objposlist.object_list[0].pose.position.x = -0.14
objposlist.object_list[0].pose.position.y = -0.27
objposlist.object_list[0].pose.position.z = 0.8
objposlist.object_list[0].pose.orientation.x = 0.78
objposlist.object_list[0].pose.orientation.y = -0.615
objposlist.object_list[0].pose.orientation.z = 0.081
objposlist.object_list[0].pose.orientation.w = -0.075
# Reading from the object_pose
#objposlist.object_list[0] = ObjectPose()
#objposlist.object_list[0].pose.position = userdata.object_data.object_list[0].pose.position
#objposlist.object_list[0].pose.orientation = userdata.object_data.object_list[0].pose.orientation
# Pose with arm down (taken from the robot) Error! reference from /map not /base_link
#objposlist.object_list[0] = ObjectPose()
#objposlist.object_list[0].pose.position.x = 0.130007
#objposlist.object_list[0].pose.position.y = -0.198804
#objposlist.object_list[0].pose.position.z = 0.900355
#objposlist.object_list[0].pose.orientation.x = 0.87292
#objposlist.object_list[0].pose.orientation.y = -0.244933
#objposlist.object_list[0].pose.orientation.z = 0.127606
#objposlist.object_list[0].pose.orientation.w = -0.402163
######## Message formed from the example message: http://pastebin.com/zcrWcaih ###########
arm_goal.planner_service_name = "ompl_planning/plan_kinematic_path"
#Planning Scene diff: empty!
#motion_plan_request
arm_goal.motion_plan_request = MotionPlanRequest()
# workspace_parameters, empty until we get to goal_constaints
arm_goal.motion_plan_request.workspace_parameters = WorkspaceParameters()
#arm_goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position = objposlist.object_list[0].pose.position
#arm_goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation = objposlist.object_list[0].pose.orientation #userdata.object_data.object_list[0].pose.position
arm_goal.motion_plan_request.goal_constraints.position_constraints = [None] * 1
arm_goal.motion_plan_request.goal_constraints.position_constraints[0] = PositionConstraint()
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].header = Header()
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = rospy.Time().now()
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "/base_link" #FIXME: This is ALWAYS GONNA BE BASE_LINK??? #userdata.object_location.header.frame_id
#userdata.object_data.header.frame_id#"torso_lift_link"
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].link_name = "arm_right_7_link"
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position.x = objposlist.object_list[0].pose.position.x
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position.y = objposlist.object_list[0].pose.position.y
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position.z = objposlist.object_list[0].pose.position.z
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = 1 # 1 should be box
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions = [None] * 3
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions[0] = 0.040000000000000001
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions[1] = 0.040000000000000001
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions[2] = 0.040000000000000001
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0
arm_goal.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0
arm_goal.motion_plan_request.goal_constraints.orientation_constraints = [None] * 1
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0] = OrientationConstraint()
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header = Header()
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = rospy.Time().now()
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "/base_link"
#userdata.object_data.header.frame_id
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "arm_right_7_link"
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = objposlist.object_list[0].pose.orientation.x
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = objposlist.object_list[0].pose.orientation.y
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = objposlist.object_list[0].pose.orientation.z
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = objposlist.object_list[0].pose.orientation.w
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.02
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.02
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.02
arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0
arm_goal.motion_plan_request.planner_id = ""
arm_goal.motion_plan_request.group_name = "right_arm_torso"
arm_goal.motion_plan_request.num_planning_attempts = 1
arm_goal.motion_plan_request.allowed_planning_time = rospy.Duration(10)
arm_goal.motion_plan_request.expected_path_duration = rospy.Duration(0)
arm_goal.motion_plan_request.expected_path_dt = rospy.Duration(0)
print "After this print the message is sent!"
return arm_goal
