Revision history [back]

So as I Understand you code you first aim to target pos and then drive to it.

distance=sqrt(pow(goal1.x-x,2) + pow(goal1.y-y, 2)) 
scale=1.0
scaleVel=1.0
if abs(angle_to_goal-theta) > 0.15:
  dir=(angle_to_goal-theta) /abs( angle_to_goal-theta) 
  angSpeed= min(0.5, abs( angle_to_goal-theta) / scale) 
  speed.linear.x = 0.0
  speed.angular.z = dir*angSpeed
else:
  speed.linear.x = min(1.5, distance * scaleVel) 
  speed.angular.z = 0.0

Vou can tune the scale values for daster drive and rotate.

So as I Understand you code you first aim to target pos and then drive to it.

distance=sqrt(pow(goal1.x-x,2) + pow(goal1.y-y, 2)) 
scale=1.0
scaleVel=1.0
scale=0.3
scaleVel=0.3
if abs(angle_to_goal-theta) > 0.15:
  dir=(angle_to_goal-theta) /abs( angle_to_goal-theta) 
  angSpeed= min(0.5, abs( angle_to_goal-theta) / scale) 
  speed.linear.x = 0.0
  speed.angular.z = dir*angSpeed
else:
  speed.linear.x = min(1.5, distance * scaleVel) 
  speed.angular.z = 0.0

Vou can tune the scale values for daster drive and rotate. rotate.