Python geometry_msgs.msg.Pose2D() Examples

def range_at(x, y, theta):
    pose = Pose2D()
    pose.x = x
    pose.y = y
    pose.theta = theta
    global r
    r = None
    move(pose)  # will throw rospy.service.ServiceException on attempting to hit a wall.
    attempts = 0
    while r is None:
        time.sleep(0.1)
        attempts += 1
        if attempts > 10:
            print >>sys.stderr, "lock up on range?"
            exit(0)
    return r 

def set_robot_pos(self, x, y, theta):
        """
        Move robot to position (x, y, theta) in simulation
        :param x x-position of the robot
        :param y y-position of the robot
        :param theta theta-position of the robot
        """
        pose = Pose2D()
        pose.x = x
        pose.y = y
        pose.theta = theta
        rospy.wait_for_service('%s/move_model' % self.NS)
        self.__move_robot_to('robot_1', pose)
        self.take_sim_step()
        self.__pub_initial_position(x, y, theta) 

def __spawn_random_static_objects(self):
        """
        Spawns a random number of static objects randomly on the path.
        """
        max_num_obstacles = int(len(self.__path.poses) / 150)
        self.__static_object_types["index"] = [0, 0, 0]
        models = []

        if max_num_obstacles == 0:
            num_static_obstacles = 0
        else:
            num_static_obstacles = random.randint(1, max_num_obstacles)
        for i in range(num_static_obstacles):
            model_type = random.randint(0, len(self.__static_object_types["name"])-1)
            model_name = self.__static_object_types["name"][model_type]
            [x, y] = self.__generate_rand_pos_on_path(self.__path.poses, 100, 1.0)
            theta = random.uniform(-math.pi, math.pi)
            model = Model()
            model.yaml_path = "%s/objects/%s" % (self.__flatland_path, model_name)
            model.name = "%s_%d"%(model_name.split('.')[0], self.__static_object_types["index"][model_type])
            model.ns = "stat_obj_%d" % i
            model.pose = Pose2D()
            model.pose.x = x
            model.pose.y = y
            model.pose.theta = theta
            models.append(model)
            self.__static_object_types["index"][model_type] +=1
            # self.spawn_object(model_name, i, x, y, theta)
        self.respawn_static_objects(models)
        return 

def reset_robot_random_pose(self):
        if self.starting_random_positions is None:
            self.starting_random_positions = []
            # top straight
            for x in range(1, 10):
               self.starting_random_positions.append((x, 9))
            # rhs straight
            for y in range(1, 9):  
                self.starting_random_positions.append((9, y))
            # lhs zip / zag, top to bottom
            for y in range(5, 9):
                self.starting_random_positions.append((1, y))
            for x in range(2, 5):
                self.starting_random_positions.append((x, 5))
            for y in range(1, 5):
                self.starting_random_positions.append((5, y))
            for x in range(6, 9):
                self.starting_random_positions.append((x, 1))
            # check no dups
            assert len(self.starting_random_positions) == len(set(self.starting_random_positions)),\
                ("%s" % self.starting_random_positions)

        # pick a random starting pose
        start_x, start_y = random.choice(self.starting_random_positions)
        new_pose = Pose2D()
        new_pose.x = start_x
        new_pose.y = start_y
        new_pose.theta = random.random() * 2 * math.pi
        self.move(new_pose) 

def reset_robot_on_straight_section(self):
        if self.straight_section_poses is None:
            self.straight_section_poses = [(3,9,0), (7,9,0), (9,7,4.71),
                                           (9,3,4.71), (7,1,3.14), (5,3,1.57),
                                           (3,5,3.14), (1,7,1.57)]
        start_x, start_y, start_theta = random.choice(self.straight_section_poses)
        new_pose = Pose2D()
        new_pose.x = start_x
        new_pose.y = start_y
        new_pose.theta = start_theta
        self.move(new_pose) 

def pose_to_rel_pose(new_center, absolute):
    """Transforms an absolute pose to one relative to a give new center.

    :param new_center: The new center the absolute pose should be relative to.
    :param absolute:  The pose to transform
    :return: The transformed pose now relative to the new center.
    """
    rel = Pose2D()
    rel.x = (absolute.x - new_center.x) * math.cos(new_center.theta) - (absolute.y - new_center.y) * math.sin(new_center.theta)
    rel.y = -(absolute.x - new_center.x) * math.sin(new_center.theta) + (absolute.y - new_center.y) * math.cos(new_center.theta)
    rel.theta = wrap(absolute.theta - new_center.theta)
    return rel 

def rel_polar(center, absolute):
    """Return polar coordinates of a point relative to a given center.

    :param center: The coordinate center.
    :param absolute: The point.
    :return: Polar coordinates of the provided absolute point relative to the given center.
    """
    rel = Pose2D()
    rel.x = (absolute.x - center.x) * math.cos(-center.theta) \
            - (absolute.y - center.y) * math.sin(-center.theta)
    rel.y = -(absolute.x - center.x) * math.sin(-center.theta) \
            + (absolute.y - center.y) * math.cos(-center.theta)
    dist = math.hypot(rel.x, rel.y)
    theta = wrap(np.arctan2(rel.y, rel.x))
    return dist, theta 

def pose_to_rel_pose(new_center, absolute):
    """Transforms an absolute pose to one relative to a give new center.

    :param new_center: The new center the absolute pose should be relative to.
    :param absolute:  The pose to transform
    :return: The transformed pose now relative to the new center.
    """
    rel = Pose2D()
    rel.x = (absolute.x - new_center.x) * math.cos(new_center.theta) - (absolute.y - new_center.y) * math.sin(new_center.theta)
    rel.y = -(absolute.x - new_center.x) * math.sin(new_center.theta) + (absolute.y - new_center.y) * math.cos(new_center.theta)
    rel.theta = wrap(absolute.theta - new_center.theta)
    return rel 

def rel_polar(center, absolute):
    """Return polar coordinates of a point relative to a given center.

    :param center: The coordinate center.
    :param absolute: The point.
    :return: Polar coordinates of the provided absolute point relative to the given center.
    """
    rel = Pose2D()
    rel.x = (absolute.x - center.x) * math.cos(-center.theta) \
            - (absolute.y - center.y) * math.sin(-center.theta)
    rel.y = -(absolute.x - center.x) * math.sin(-center.theta) \
            + (absolute.y - center.y) * math.cos(-center.theta)
    dist = math.hypot(rel.x, rel.y)
    theta = wrap(np.arctan2(rel.y, rel.x))
    return dist, theta 

def spawn_object(self, model_name, index, x, y, theta):
        """
        Spawns one static object.
        :param  model_name object type
        :param  x x-position of the object
        :param  y y-position of the object
        :param  theta orientation of the object
        """
        srv = SpawnModel()
        srv.yaml_path = "%s/objects/%s" % (self.__flatland_path, model_name)
        srv.name = "stat_obj_%d" % index
        srv.ns = "stat_obj_%d" % index
        temp = Pose2D()
        temp.x = x
        temp.y = y
        temp.theta = theta
        srv.pose = temp
        rospy.wait_for_service('%s/spawn_model' % self.NS)
        try:
            self.__spawn_model.call(srv.yaml_path, srv.name, srv.ns, srv.pose)
        except (TypeError):
            print('Spawn object: TypeError.')
            return
        except (rospy.ServiceException):
            print('Spawn object: rospy.ServiceException. Closing serivce')
            try:
                self.__spawn_model.close()
            except AttributeError:
                print('Spawn object close(): AttributeError.')
                return
            return
        except AttributeError:
            print('Spawn object: AttributeError.')
            return

        stat_obj = Model()
        stat_obj.yaml_path = srv.yaml_path
        stat_obj.name = srv.name
        stat_obj.ns = srv.ns
        stat_obj.pose = srv.pose
        self.__static_objects.append(stat_obj)
        return 

def __init__(self):
        NaoqiNode.__init__(self, 'nao_walker')

        self.connectNaoQi()

        # walking pattern params:
        self.stepFrequency = rospy.get_param('~step_frequency', 0.5)

        self.useStartWalkPose = rospy.get_param('~use_walk_pose', False)
        self.needsStartWalkPose = True

        # other params
        self.maxHeadSpeed = rospy.get_param('~max_head_speed', 0.2)
        # initial stiffness (defaults to 0 so it doesn't strain the robot when no teleoperation is running)
        # set to 1.0 if you want to control the robot immediately
        initStiffness = rospy.get_param('~init_stiffness', 0.0)

        self.useFootGaitConfig = rospy.get_param('~use_foot_gait_config', False)
        rospy.loginfo("useFootGaitConfig = %d" % self.useFootGaitConfig)
        if self.useFootGaitConfig:
            self.footGaitConfig = rospy.get_param('~foot_gait_config', self.motionProxy.getFootGaitConfig("Default"))
        else:
            self.footGaitConfig = self.motionProxy.getFootGaitConfig("Default")

        # TODO: parameterize
        if initStiffness > 0.0 and initStiffness <= 1.0:
            self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)

        try:
            enableFootContactProtection = rospy.get_param('~enable_foot_contact_protection')
            self.motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", enableFootContactProtection]])
            if enableFootContactProtection:
                rospy.loginfo("Enabled foot contact protection")
            else:
                rospy.loginfo("Disabled foot contact protection")
        except KeyError:
            # do not change foot contact protection
            pass

        # last: ROS subscriptions (after all vars are initialized)
        rospy.Subscriber("cmd_vel", Twist, self.handleCmdVel, queue_size=1)
        rospy.Subscriber("cmd_pose", Pose2D, self.handleTargetPose, queue_size=1)
        rospy.Subscriber("cmd_step", StepTarget, self.handleStep, queue_size=50)

        # Create ROS publisher for speech
        self.pub = rospy.Publisher("speech", String, latch = True)

        # ROS services (blocking functions)
        self.cmdPoseSrv = rospy.Service("cmd_pose_srv", CmdPoseService, self.handleTargetPoseService)
        self.cmdVelSrv = rospy.Service("cmd_vel_srv", CmdVelService, self.handleCmdVelService)
        self.stepToSrv = rospy.Service("cmd_step_srv", StepTargetService, self.handleStepSrv)
        self.stopWalkSrv = rospy.Service("stop_walk_srv", Empty, self.handleStopWalkSrv)
        self.needsStartWalkPoseSrv = rospy.Service("needs_start_walk_pose_srv", Empty, self.handleNeedsStartWalkPoseSrv)
        self.readFootGaitConfigSrv = rospy.Service("read_foot_gait_config_srv", Empty, self.handleReadFootGaitConfigSrv)
        self.setArmsEnabledSrv = rospy.Service("enable_arms_walking_srv", SetArmsEnabled, self.handleSetArmsEnabledSrv)

        self.say("Walker online")

        rospy.loginfo("nao_walker initialized")