def _step_angle()

in src/pyrobot/locobot/base_controllers.py [0:0]

• 46 lines of code
• 20 McCabe index (conditional complexity)

    def _step_angle(self, action=0.0):
        # target is absolute orientation wrt to current orientation
        # target is in radian
        target = action
        init_pose = self.bot_base.state.theta
        init_err = self._norm_pose(target)
        target_world = self._norm_pose(target + init_pose)

        cmd_vel = 0
        min_vel = 0
        got_min_vel = False
        prev_time = time.time()
        self.bot_base.should_stop = False

        ret_val = True

        while True:

            if self._as.is_preempt_requested():
                rospy.loginfo("Preempted the Proportional execution")

                return False

            if self.bot_base.should_stop:
                if not self.ignore_collisions:
                    rospy.loginfo("curr error = {} meters".format(cur_error))

                    self.stop()
                    return False

            if time.time() - prev_time > (1.0 / self.hz):
                cur_error = self._norm_pose(target_world - self.bot_base.state.theta)

                # stop if error goes beyond some value
                if abs(cur_error) < self.rot_error_thr:
                    rospy.loginfo("Reached goal")
                    rospy.loginfo("curr_error = {} degrees".format(degrees(cur_error)))
                    self._cmd_vel(rot_vel=0.0)
                    break

                # for getting the min velocity at wchich bot starts to move
                if not (got_min_vel) and abs(cur_error - init_err) > self.rot_move_thr:
                    got_min_vel = True
                    min_vel = abs(cmd_vel)

                # doing the linear increse part
                if init_err != 0 and cur_error / init_err > 0.5:
                    if abs(cmd_vel) < self.rot_max_vel:
                        cmd_vel += sign(cur_error) * self.vel_delta

                # elif abs(cur_error) < self.drop_ang:
                else:
                    if abs(cmd_vel) > 0.75 * min_vel:
                        # 0.75 as I found min velocity is always above the
                        # actual min required velocity
                        cmd_vel -= sign(cur_error) * self.vel_delta

                # change the sign of init error if it misses
                if abs(cur_error) > abs(init_err):
                    init_err = cur_error

                # chnage the init error if you overshooot
                if cur_error * init_err < 0:
                    cmd_vel = cmd_vel / 10
                    init_err = cur_error

                self._cmd_vel(rot_vel=cmd_vel)
                prev_time = time.time()

            rospy.sleep(SLEEP_TIME)
        self.bot_base.should_stop = False
        return ret_val