in src/pyrobot/locobot/base_controllers.py [0:0]
def _step_x(self, action=0.0):
# target is the distance in x direction that robot has to move
# target is in meter(only works for positive )
target = action
init_x = self.bot_base.state.x
init_y = self.bot_base.state.y
init_err = abs(target)
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 = abs(
abs(target)
- sqrt(
(self.bot_base.state.x - init_x) ** 2
+ (self.bot_base.state.y - init_y) ** 2
)
)
# stop if error goes beyond some value
if abs(cur_error) < self.dist_error_thr:
rospy.loginfo("Reached goal")
rospy.loginfo("curr error = {} meters".format(cur_error))
self._cmd_vel(lin_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.lin_move_thr:
got_min_vel = True
min_vel = abs(cmd_vel)
# rospy.loginfo("min vel = ", min_vel)
# doing the linear increse part
if cur_error / init_err > 0.6:
if abs(cmd_vel) < self.lin_max_vel:
cmd_vel += sign(target) * self.vel_delta
# elif abs(cur_error) < self.drop_ang:
else:
if abs(cmd_vel) > 0.75 * min_vel:
cmd_vel -= sign(target) * self.vel_delta
# change the sign of init error if it misses
if abs(cur_error) > abs(init_err):
init_err = cur_error
target = sign(target) * cur_error
# chnage the init error if you overshooot
if cur_error * init_err < 0:
cmd_vel = cmd_vel / 10
init_err = cur_error
target = -sign(target) * cur_error
self._cmd_vel(lin_vel=cmd_vel)
prev_time = time.time()
rospy.sleep(SLEEP_TIME)
self.bot_base.should_stop = False
return ret_val