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 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
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 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 obj.object_list[num_object] = ObjectPose() 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 ) break else: 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) 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: # 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])) 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: return succeeded if status == GoalStatus.ABORTED: return aborted