class control_velocity(object):
def __init__(self):
self.linear_pid = PID(95, 85, 1, 62.915)
self.angular_pid = PID(6, 4, 0.1, 3.125)
self.linear_pid.setpoint = 0
self.angular_pid.setpoint = 0
self.left_pub = rospy.Publisher('/wheel_left/duty', Int16, queue_size=1)
self.right_pub = rospy.Publisher('/wheel_right/duty', Int16, queue_size=1)
rospy.Subscriber("/cmd_vel", Twist, self.update_setpoints)
rospy.Subscriber("/wheel_odom", Odometry, self.update_duty)
def update_setpoints(self, cmd_vel):
self.linear_pid.setpoint = cmd_vel.linear.x
self.angular_pid.setpoint = cmd_vel.angular.z
def update_duty(self, odom):
twist = odom.twist.twist
if self.linear_pid.setpoint == 0 and self.angular_pid.setpoint == 0:
self.linear_pid.clear_integrator()
self.angular_pid.clear_integrator()
linear = self.linear_pid.calc(twist.linear.x)
angular = self.angular_pid.calc(twist.angular.z)
self.left_pub.publish(Int16(-linear + angular))
self.right_pub.publish(Int16(-linear - angular))
class MotorController:
def __init__(self, mcp_channel, pins, pid_constants, forward):
self.current_sensor = CurrentSensor(mcp_channel)
self._motor = Motor(pins['pwm'], pins['dir'], forward)
self._encoder = RotaryEncoder(pins['a'], pins['b'])
self._pid = PID(pid_constants)
self._vel_filter = IrregularDoubleExponentialFilter(
*motor_vel_smoothing_factors)
self._vel = 0
def adjust_motor_speed(self, target_vel, time_elapsed):
# use a pi controller to control the speed and limit the output to
# [-1, 1] (-1 and 1 correspond to 100% pwm output)
error = (target_vel - self._vel) / max_wheel_vel
speed = min(max(self._pid.calc(error, time_elapsed), -1), 1)
self._motor.set_speed(speed)
def read_encoder(self, time_elapsed):
# gets the amount the wheel has turned in the last interval and
# calculates the wheel's speed with a filter added in
dist_traveled = self._encoder.get_pos_dif() * distance_ratio
self._vel = self._vel_filter.filter(dist_traveled / time_elapsed,
time_elapsed)
return dist_traveled
def reset(self):
self._motor.set_speed(0)
self._pid.reset()
self._encoder.reset()
def stop(self):
self._motor.stop()
self._encoder.stop()
class ServoController:
"""Handles the high-level tasks of controlling a servo. It keeps track of
the servo's angle and allows for relative movements."""
def __init__(self,
pin,
pid_constants,
left_limit=-pi * 2 / 3,
right_limit=pi / 2):
self._servo = Servo(pin)
self._pid = PID(pid_constants)
if left_limit >= right_limit:
raise Exception('left_limit >= right_limit')
self._left_limit = left_limit
self._right_limit = right_limit
self.angle = 0.0
# servo cannot be set faster than 50Hz, so 20ms lower_limit
self._timer = Timer(0.02)
def move_by(self, offset):
time_elapsed = self._timer.elapsed()
if time_elapsed is None:
return
offset = self._pid.calc(offset, time_elapsed)
# ensure that servo moves at most by max_move
offset = symmetric_limit(offset, max_servo_move)
self._move_to(self.angle + offset)
def _move_to(self, angle):
# ensure servo doesn't move beyond limits
angle = limit(angle, self._left_limit, self._right_limit)
self.angle = angle
self._servo.move_to(self._angle_ratio(angle))
def _angle_ratio(self, angle):
return (angle - self._left_limit) / (self._right_limit -
self._left_limit)
def start(self):
self._timer.start()
def stop(self):
self._move_to(0)
self._pid.reset()
self._timer.reset()
class Controller(object):
def __init__(self):
self.left_wall_pid = PID(14, 2, 0.5)
self.right_wall_pid = PID(14, 2, 0.5)
self.left_wall_pid.setpoint = 0.055
self.right_wall_pid.setpoint = 0.055
self.goal_angle = None
# distance to goal from start position
self.goal_distance = None
# distance to goal from current position
self.dist_to_goal = 0
self.start_position = None
self.position = None
self.start_angle = None
self.angle = None
self.left_dist = 0
self.right_dist = 0
self.front_dist = 0
self.commands = []
self.busy = False
self.callback = None
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rospy.Subscriber("/wheel_odom", Odometry, self.update)
rospy.Subscriber("/sensors/ir_left", Float32, self.ir_left)
rospy.Subscriber("/sensors/ir_right", Float32, self.ir_right)
rospy.Subscriber("/sensors/ir_front", Float32, self.ir_front)
def ir_left(self, data):
self.left_dist = data.data
def ir_right(self, data):
self.right_dist = data.data
def ir_front(self, data):
self.front_dist = data.data
def update(self, odom):
# populate current state variables
pose = odom.pose.pose
twist = odom.twist.twist
q = pose.orientation
self.angle = transformations.euler_from_quaternion(
[q.x, q.y, q.z, q.w])[2]
self.position = pose.position
if not self.busy and self.commands:
data = self.commands.pop(0)
data[0]()
self.callback = data[1]
print("Starting action: " + data[2])
self.busy = True
# traverse if needed
cmd_vel = Twist()
linear_vel = 0
angular_vel = 0
if self.goal_distance != None:
# heading correction
if self.left_dist > 0.03 and self.left_dist < 0.08:
angular_vel -= self.left_wall_pid.calc(self.left_dist)
if self.right_dist > 0.03 and self.right_dist < 0.08:
angular_vel += self.right_wall_pid.calc(self.right_dist)
# vroom
offset = abs(self.goal_distance) - dist(
self.position.x, self.position.y, self.start_position.x,
self.start_position.y)
self.dist_to_goal = offset
if abs(offset) < 0.01 and abs(twist.linear.x) < 0.01:
self.goal_distance = None
elif abs(offset) < 0.05:
linear_vel = signum(offset) * max(0,
0.15 - abs(angular_vel) / 10)
else:
linear_vel = signum(offset) * max(0,
0.2 - abs(angular_vel) / 10)
# absolute position correction via front IR
if offset > 0.08 and offset < 0.18:
error = self.front_dist - offset - 0.015
if abs(error) < 0.06:
self.goal_distance += error
if self.goal_angle != None:
offset = wrap_angle(self.goal_angle - self.angle)
# print(offset)
if abs(offset) < 0.06 and abs(twist.angular.z) < 0.2 and signum(
offset) == -signum(twist.angular.z):
self.goal_angle = None
elif abs(offset) < 0.05:
angular_vel = signum(offset) * 1.5
elif abs(offset) < 0.2:
angular_vel = signum(offset) * 2.5
else:
angular_vel = signum(offset) * 5
if self.busy and self.goal_angle == None and self.goal_distance == None:
self.busy = False
print('DONE')
linear_vel = 0
angular_vel = 0
self.callback and self.callback()
cmd_vel.linear.x = linear_vel
cmd_vel.angular.z = angular_vel
self.cmd_vel_pub.publish(cmd_vel)
def turn(self, angle, cb=None):
def f():
self.goal_angle = wrap_angle(self.angle + angle)
self.commands.append((f, cb, 'turn by ' + str(angle)))
def turnTo(self, angle, cb=None):
def f():
self.goal_angle = wrap_angle(angle)
self.commands.append((f, cb, 'turn to ' + str(angle)))
def straight(self, dist, cb=None):
def f():
self.start_position = self.position
self.start_angle = self.angle
self.goal_distance = dist
def c():
theta = atan2(self.position.y - self.start_position.y,
self.position.x - self.start_position.x)
self.goal_angle = theta
self.busy = True
self.callback = cb
self.commands.append((f, c, 'move ' + str(dist * 100) + 'cm'))
class Run():
RADIUS = 40 # MIRSのタイヤ間の距離[cm]
USS_RADIUS = 19
USS_DIST = 30 # MIRSの中心から超音波センサまでの距離[cm]
USS_DIFF = 6
Kp = 50
Ki = 1
Kd = 1
TARGET_DIST = 0.4 # 目標となる壁との距離[m]
INTERVAL = 0.01 # 制御周期[s]
USS_DICT_LIST = ["f", "sf", "s", "sb", "b"]
def __init__(self):
self.isLeft = True
self.vel_left_sum = 0
self.vel_right_sum = 0
self.uss = {}
self.speed = 15
self.cmd_prev = []
self.pid = PID((Run.Kp, Run.Ki, Run.Kd), 0.1, Run.TARGET_DIST)
def set_val(self, is_left, uss):
self.isLeft = is_left
self.uss["f"] = uss[0]
self.uss["b"] = uss[4]
if self.isLeft:
self.uss["sf"] = uss[7]
self.uss["s"] = uss[6]
self.uss["sb"] = uss[5]
else:
self.uss["sf"] = uss[1]
self.uss["s"] = uss[2]
self.uss["sb"] = uss[3]
def straight(self):
speed_mod = self.pid.calc(self.calc_dist()) # 近いと正、遠いと負
speed_l, speed_r = self.speed + speed_mod, self.speed - speed_mod
return self.is_duplicate_cmd([["velocity", speed_l, speed_r]])
def is_duplicate_cmd(self, cmd):
if self.cmd_prev == cmd:
return cmd
else:
return []
def calc_ratio(self):
front = self.uss["sf"]
back = self.uss["sb"]
tmp = ((back - front) / Run.USS_DIST)**2.0
return (tmp + 1)**-0.5
def calc_angle(self):
front = self.uss["sf"]
back = self.uss["sb"]
tmp = math.acos(self.calc_ratio())
if (self.isLeft and front > back) or (not self.isLeft
and back > front):
tmp *= -1
return math.degrees(tmp)
def calc_dist(self):
tmp = self.calc_ratio()
return tmp * (self.uss["s"] + Run.USS_RADIUS)
def control_process(self, *args):
'''This function will called from joystick_async as subprocess'''
control_optflow_pipe_read, control_cv_pipe_read, control_tof_pipe_read, control_imu_pipe_read, ext_control_pipe_write, ext_control_pipe_read, nice_level = args
os.nice(nice_level)
# Init the ToF kalman filter
tof_filter = self.tof_filter_init()
KFXY, KFXY_z, KFXY_u = self.xy_of_filter_init()
''' PID Init '''
throttle_pd = PID(self.PZ_GAIN, self.IZ_GAIN,
self.DZ_GAIN) #throttle PID
roll_pd = PID(self.PY_GAIN, self.IY_GAIN, self.DY_GAIN) #roll PID
pitch_pd = PID(self.PX_GAIN, self.IX_GAIN, self.DX_GAIN) #pitch PID
prev_velocity_pitch = 0
prev_velocity_roll = 0
prev_error_roll = 0
prev_error_pitch = 0
'''Z-axis init'''
prev_altitude_sensor = None
altitude_sensor = None
altitude = None
altitude_corrected = None
value_available = False
postition_hold = False
init_altitude = 0
'''CMDS init'''
CMDS = {'throttle': 0, 'roll': 0, 'pitch': 0}
prev_time = time.time()
OF_DIS = of_dis = 0
# For init
error_altitude = error_roll = error_pitch = velocity_pitch = velocity_roll = 0
'''Angular Speed'''
pre_roll = 0
pre_pitch = 0
while True:
CMDS['throttle'] = 0
CMDS['roll'] = 0
# The betaflight config trim the pitch -10 for unbalance of the drone
CMDS['pitch'] = 0
# Let the OF Pipe run
control_tof_pipe_read.send('a')
'''Read the joystick_node trigger the auto mode or not'''
if ext_control_pipe_write.poll(
): # joystick loop tells when to save the current values
postition_hold = ext_control_pipe_write.recv()
if not postition_hold:
self.LANDING = True
self.DATA = True
init_altitude = None
'''Update the IMU value'''
if control_imu_pipe_read.poll():
self.imu, battery_voltage, self.IMU_TIME = control_imu_pipe_read.recv(
) # [[accX,accY,accZ], [gyroX,gyroY,gyroZ], [roll,pitch,yaw]]
#DEBUG USE
imut = time.time()
angu_time = prev_time - self.IMU_TIME
if angu_time > 0.001:
angu_roll = (self.imu[2][0] - pre_roll) / angu_time
angu_pitch = (self.imu[2][1] - pre_pitch) / angu_time
pre_roll = self.imu[2][0]
pre_pitch = self.imu[2][1]
else:
angu_roll = angu_pitch = 0
if self.TAKEOFF:
# Tested voltage throttle relationship
TAKEOFF_THRUST = int(1015 - 60 * (battery_voltage))
'''Update the ToF Kalman Filter with the ground value'''
if postition_hold and altitude_sensor:
# Remember to reset integrator here too!
prev_altitude_sensor = init_altitude = altitude_sensor
postition_hold = False
# initial value from the sensor
# the cognifly have the initial heigth of 0.11m
tof_filter.x = np.array([[init_altitude], [0]])
continue
'''Vertical Movement Control'''
if init_altitude:
# Update the ToF Filter
dt = prev_time - self.TOF_Time
# For init reading will very large, but normal case would not larger than 1s
tof_filter.F[0, 1] = dt if abs(dt < 3) else 0
tof_filter.B[0] = 0.5 * (dt**2) if abs(dt < 3) else 0
tof_filter.B[1] = dt
tof_filter.predict(
u=0) #Just test for non -9.81*(0.99-imu[0][2])
# Capture the Predicted value
altitude = tof_filter.x[0, 0]
velocity = tof_filter.x[1, 0]
# '''Takeoff Setting'''
if self.TAKEOFF:
if len(self.TAKEOFF_LIST):
CMDS[
'throttle'] = self.TAKEOFF_LIST[0] * TAKEOFF_THRUST
value_available = True
self.TAKEOFF_LIST.pop(0)
cancel_gravity_value = CMDS['throttle']
else:
# control_optflow_pipe_read.send('a')
init_altitude = self.TAKEOFF_ALTITUDE
velocity = 0
self.TAKEOFF = False
# '''PID at Throttle'''
if (not self.TAKEOFF):
error_altitude = init_altitude - altitude # altitude
next_throttle = throttle_pd.calc(error_altitude,
time=dt,
velocity=-velocity)
# Set throttle by PID control
CMDS['throttle'] = self.value_limit(
next_throttle, self.ABS_MAX_VALUE_THROTTLE)
# Add the cancel gravity set point with the angle compensate
CMDS['throttle'] += cancel_gravity_value / (
(np.cos(self.imu[2][0] * np.pi * 1 / 180)) *
(np.cos(self.imu[2][1] * np.pi * 1 / 180)))
value_available = True
prev_altitude_sensor = altitude_corrected
# LANDING
if not self.TAKEOFF and self.LANDING:
CMDS['throttle'] = cancel_gravity_value + (15 /
(altitude + 0.5))
'''Update the ToF value'''
if control_tof_pipe_read.poll():
if not init_altitude:
altitude_sensor, self.TOF_Time = control_tof_pipe_read.recv(
) # Flushing the old value
# altitude_sensor = control_tof_pipe_read.recv() # Flushing the old value
else:
# turning the altitdue back to ground, reference as global coordinate
altitude_sensor, self.TOF_Time = control_tof_pipe_read.recv(
)
#DEBUG USE
toft = time.time()
# The sensor is not at the center axis of rotation
# The following parts are going to turn the offset bact the origin
# ROLL: (Measure * cos(roll_angle)) - (sensor_offset * sin(sensor_angle - roll_angle)
# PITCH: (Measure - offset) * cos(pitch_angle)
# combine: (Measure * cos(roll) * cos(pitch)) - (offset * sin(sensor-roll) * cos(pitch))
# CogniFly offset -> z: -40mm, y: +38mm
altitude_corrected = altitude_sensor * (np.cos(
self.imu[2][0] * np.pi * 1 / 180)) * (np.cos(
self.imu[2][1] * np.pi * 1 / 180))
offset = self.TOFOFFSET_R * np.sin(self.TOFOFFSET_ANGLE - (
self.imu[2][0] * np.pi * 1 / 180)) * (np.cos(
self.imu[2][1] * np.pi * 1 / 180))
tof_filter.update([
self.truncate(altitude_corrected - offset),
self.truncate(((altitude_corrected - offset) -
prev_altitude_sensor) / dt)
])
'''Update the XY Filter'''
# if ((not TAKEOFF) and (abs(error_altitude) < 0.2)):
if (not self.TAKEOFF):
# For init reading will very large, but normal case would not larger than 1s
dt_OF = prev_time - self.OF_TIME
dt_IMU = prev_time - self.IMU_TIME
# KFXY.R[0,0] = (0.005+(velocity/90)) # Increase the noise for the filter when up and down
# KFXY.R[1,1] = (0.005+(velocity/90))
# # For init reading will very large, but normal case would not larger than 1s
# KFXY.F[0,2] = dt_OF if abs(dt_OF<3) else 0
# KFXY.F[1,3] = dt_OF if abs(dt_OF<3) else 0
# KFXY.B[2,2] = dt_IMU if abs(dt_IMU<3) else 0
# KFXY.B[3,3] = dt_IMU if abs(dt_IMU<3) else 0
# # Another angular speed can be optained by (atitude/dt)
# # linear speed can be optained by angluar_speed*height
# KFXY_u[2,0] = self.truncate(9.81*(self.imu[0][0])*np.cos(self.imu[2][1])) #imu[0][0]->ax Pitch acc #imu[2][1]->Pitch angle
# KFXY_u[3,0] = self.truncate(9.81*(self.imu[0][1])*np.cos(self.imu[2][0])) #imu[0][1]->ay Roll acc #imu[2][0]->Roll angle
if control_optflow_pipe_read.poll():
KFXY_z[0, 0], KFXY_z[
1,
0], of_dis, self.OF_TIME = control_optflow_pipe_read.recv(
) # it will block until a brand new value comes.
#DEBUG USE
oft = time.time()
# KFXY.update(KFXY_z*(-altitude))# To real scale # X-Y reversed
# KFXY.predict(u=0)#KFXY_u) # [dx, dy, vx, vy]
OF_DIS += of_dis * altitude
'''X-Y control'''
factor = 1.6 #Seem the data is scaled up 1.6 times
error_roll = self.truncate((OF_DIS[1]) / factor)
error_pitch = self.truncate((OF_DIS[0]) / factor)
velocity_roll_tmp = self.truncate(KFXY_z[1, 0] * (-altitude))
velocity_pitch_tmp = self.truncate(KFXY_z[0, 0] * (-altitude))
self.a = np.exp(-abs(velocity_pitch_tmp - velocity_pitch))
velocity_pitch += self.a * (velocity_pitch_tmp -
velocity_pitch)
self.a = np.exp(-abs(velocity_roll_tmp - velocity_roll))
velocity_roll += self.a * (velocity_roll_tmp - velocity_roll)
# prev_error_roll = error_roll
# prev_error_pitch = error_pitch
# velocity_roll_tmp = -self.truncate((error_roll-prev_error_roll)/dt_OF)
# velocity_pitch_tmp = -self.truncate((error_pitch-prev_error_pitch)/dt_OF)
# velocity_roll += self.a*(velocity_roll_tmp - prev_velocity_roll)
# velocity_pitch += self.a*(velocity_pitch_tmp - prev_velocity_pitch)
# prev_velocity_roll = velocity_roll
# prev_velocity_pitch = velocity_pitch
# velocity_roll = self.truncate(KFXY.x[3,0])
# velocity_pitch = self.truncate(KFXY.x[2,0])
# The new cognifly is reversed the pi orientation
next_roll = roll_pd.calc(-error_roll,
velocity=velocity_roll) # Y
next_pitch = pitch_pd.calc(-error_pitch,
velocity=velocity_pitch) # X
CMDS['roll'] = next_roll if abs(
next_roll) <= self.ABS_MAX_VALUE_ROLL else (
-1 if next_roll < 0 else 1) * self.ABS_MAX_VALUE_ROLL
CMDS['pitch'] = next_pitch if abs(
next_pitch) <= self.ABS_MAX_VALUE_PITCH else (
-1 if next_pitch < 0 else 1) * self.ABS_MAX_VALUE_PITCH
value_available = True
print(
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
)
print("OF Distance: ", OF_DIS)
print(
"THROTTLE :{2:.2f} | ALT:{1:.2f} | ERR:{0:.2f} | Vec:{3:.2f}"
.format(error_altitude, altitude, next_throttle, velocity))
print("dt_OF:{:.2f} | dt_IMU:{:.2f}".format(dt_OF, dt_IMU))
print("Angular Roll: {:.2f} | Pitch: {:.2f}".format(
angu_roll, angu_pitch))
print("ERROR ROLL : %2.2f error| %2.2f roll| %2.2f of" %
(error_roll, next_roll, 0))
print("ERROR PITCH: %2.2f error| %2.2f pitch| %2.2f of" %
(error_pitch, next_pitch, 0))
print(
"ROLL velocity: ", -KFXY.x[3, 0],
KFXY_z[1, 0] * (-altitude),
self.truncate(
(self.imu[2][0] * np.pi * 1 / 180 * altitude / dt)))
print(
"PITCH velocity", -KFXY.x[2, 0],
KFXY_z[0, 0] * (-altitude),
self.truncate(
(self.imu[2][1] * np.pi * 1 / 180 * altitude / dt)))
print(
"TIME:{0:1.2f} | OF:{1:.2f} | IMU:{2:.2f} | TOF:{3:.2f}"
.format(time.time(), (self.OF_TIME - oft),
(self.IMU_TIME - imut), (self.TOF_Time - toft)))
self.data.append(
(CMDS['throttle'], CMDS['roll'], CMDS['pitch'], altitude,
error_altitude, velocity, error_roll, velocity_roll,
angu_roll, error_pitch, velocity_pitch, angu_pitch))
if self.DATA:
np.save("control_t_auto", self.data)
print(">>>>>>>>>>>>>>>>>>SAVED!")
self.DATA = False
'''Send out the CMDS values back to the joystick loop'''
if value_available and (not ext_control_pipe_read.poll()):
ext_control_pipe_write.send(CMDS)
value_available = False
time.sleep(self.PERIOD)
prev_time = time.time()
class Controller(object):
def __init__(self):
self.left_wall_pid = PID(14, 2, 0.5)
self.right_wall_pid = PID(14, 2, 0.5)
self.left_wall_pid.setpoint = 0.055
self.right_wall_pid.setpoint = 0.055
self.goal_angle = None
self.goal_distance = None
self.start_position = None
self.position = None
self.start_angle = None
self.angle = None
self.left_dist = 0
self.right_dist = 0
self.front_dist = 0
self.commands = []
self.busy = False
self.callback = None
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rospy.Subscriber("/wheel_odom", Odometry, self.update)
rospy.Subscriber("/sensors/ir_left", Float32, self.ir_left)
rospy.Subscriber("/sensors/ir_right", Float32, self.ir_right)
rospy.Subscriber("/sensors/ir_front", Float32, self.ir_front)
def ir_left(self, data):
print("ASJKFLAJKFSLJAKSHELLOOOOOOOOOOOOOOOOOO")
self.left_dist = data.data
def ir_right(self, data):
self.right_dist = data.data
def ir_front(self, data):
self.front_dist = data.data
def update(self, odom):
# populate current state variables
pose = odom.pose.pose
twist = odom.twist.twist
q = pose.orientation
self.angle = transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
self.position = pose.position
if not self.busy and self.commands:
data = self.commands.pop(0)
data[0]()
self.callback = data[1]
print("Starting action: " + data[2])
self.busy = True
# traverse if needed
cmd_vel = Twist()
linear_vel = 0
angular_vel = 0
if self.goal_distance != None:
# heading correction
if self.left_dist > 0.03 and self.left_dist < 0.08:
angular_vel -= self.left_wall_pid.calc(self.left_dist)
if self.right_dist > 0.03 and self.right_dist < 0.08:
angular_vel += self.right_wall_pid.calc(self.right_dist)
# vroom
offset = abs(self.goal_distance) - dist(self.position.x, self.position.y, self.start_position.x, self.start_position.y)
if abs(offset) < 0.01 and abs(twist.linear.x) < 0.01:
self.goal_distance = None
elif abs(offset) < 0.05:
linear_vel = signum(offset) * 0.12
else:
linear_vel = signum(offset) * 0.18
# absolute position correction via front IR
if offset > 0.08 and offset < 0.18:
error = self.front_dist - offset - 0.015
if abs(error) < 0.06:
self.goal_distance += error
if self.goal_angle != None:
offset = wrap_angle(self.goal_angle - self.angle)
# print(offset)
if abs(offset) < 0.06 and abs(twist.angular.z) < 0.2 and signum(offset) == -signum(twist.angular.z):
self.goal_angle = None
elif abs(offset) < 0.05:
angular_vel = signum(offset) * 1.5
elif abs(offset) < 0.2:
angular_vel = signum(offset) * 2.5
else:
angular_vel = signum(offset) * 5
if self.busy and self.goal_angle == None and self.goal_distance == None:
self.busy = False
print('DONE')
linear_vel = 0
angular_vel = 0
self.callback and self.callback()
cmd_vel.linear.x = linear_vel
cmd_vel.angular.z = angular_vel
self.cmd_vel_pub.publish(cmd_vel)
def turn(self, angle, cb=None):
def f():
self.goal_angle = wrap_angle(self.angle + angle)
self.commands.append((f, cb, 'turn by ' + str(angle)))
def turnTo(self, angle, cb=None):
def f():
self.goal_angle = wrap_angle(angle)
self.commands.append((f, cb, 'turn to ' + str(angle)))
def straight(self, dist, cb=None):
def f():
self.start_position = self.position
self.start_angle = self.angle
self.goal_distance = dist
def c():
theta = atan2(self.position.y - self.start_position.y, self.position.x - self.start_position.x)
self.goal_angle = theta
self.busy = True
self.callback = cb
self.commands.append((f, c, 'move ' + str(dist * 100) + 'cm'))
