Exemplo n.º 1
0
            def obj_response_cb(userdata, response):
                if response.exists:
                    pose = Pose()
                    #Wont there be problems mixing float32 with 64? TODO
                    pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0)
                    #TODO, this may give problems
                    pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z))
                    if MANTAIN_DISTANCE_FROM_OBJECT:
                        pose = pose_at_distance(pose, DISTANCE_FROM_OBJECT)
                    userdata.object_location = pose

                    release_pose = ObjectPose()
                    release_pose.pose = response.arm_coordinates
                    userdata.object_release_location = release_pose

                    print "\n\n printing pose for move to object"
                    print pose
                    print "\n\n printing pose for releasing the object"
                    print release_pose
                    print "\n that was the pose of the object\n\n"
                    print userdata.object_name
                    print "is the object name"
                    return succeeded
                else:
                    userdata.object_location = None
                    return aborted
Exemplo n.º 2
0
            def obj_response_cb(userdata, response):
                if response.exists:
                    pose = Pose()
                    pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0)
                    pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z))
                    userdata.object_location = pose

                    release_pose = ObjectPose()
                    release_pose.pose = response.arm_coordinates
                    userdata.object_release_location = release_pose

                    print "\n\n printing pose for move to object"
                    print pose
                    print "\n\n printing pose for releasing the object"
                    print release_pose
                    print "\n that was the pose of the object\n\n"
                    print userdata.object_name
                    print "is the object name"
                    return succeeded
                else:
                    userdata.object_location = None
                    return aborted