def send_velocity():
global sonarF_val
global sonarFL_val
global sonarFR_val
global sonarL_val
global sonarR_val
global pd0, pd, state, substate, state2start
global sonarF, sonarL, sonarFL
global imuYaw
global state
global count_state_0
velocity_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
velocity = Twist()
"""
PUT YOUR MAIN CODE HERE
"""
velocity.linear.x = 0.0
velocity.angular.z = 0
if point_dist(X, end_point) < 0.13:
state = 3
# line pose torwards target point
if state == 0:
velocity.angular.z = -min(max(pd_turn.pd_out(imuYaw), -2.8), 2.8)
if abs(imuYaw - theta) < 0.07:
count_state_0 += 1
if count_state_0 >= 5:
velocity.angular.z = 0
print "f**k"
state = 2
# follow the straight line connection x, y to target point
elif state == 1:
velocity.linear.x = 0.2
theta1 = math.atan2(end_point[1]-X[1], end_point[0]-X[0])
pd_line = pdcon.pd_controller(setpoint = theta1, kp = 12, kd = 1.5)
velocity.angular.z = -min(max(pd_line.pd_out(X[2]), -0.5), 0.5)
elif state == 2:
velocity.linear.x = 0.2
minp = float("inf")
mini = 0
for i in range(0, 8):
x_cell, y_cell = find_cell(X[0], X[1])
print "{} {}".format(x_cell,y_cell)
x_cell += moves[i][0]
y_cell += moves[i][1]
print "{} {} {} {}".format(x_cell, y_cell, len(pmap), len(pmap[0]))
if x_cell >= len(pmap) or y_cell >= len(pmap[0]):
p = float("inf")
else:
p = pmap[x_cell][y_cell]
if minp > p:
minp = p
mini = i
x_choice = x_cell * grid_resolution - 0.75
y_choice = y_cell * grid_resolution - 0.75
print "{} {}".format(x_choice, y_choice)
theta_choice = math.atan2(y_choice-X[1], x_choice-X[0])
pd_turn1 = pdcon.pd_controller(setpoint = theta_choice, kp = 9, kd = 1.9)
velocity.angular.z = -min(max(pd_turn1.pd_out(X[2]), -1.5), 1.5)
# target reached
elif state == 3:
velocity.linear.x = 0.0
velocity.angular.z = 0.0
#rospy.loginfo("Velocity x, z: %s, %s", velocity.linear.x, velocity.angular.z)
### rospy.loginfo("Right Scan %s", sonarR_val)
# rospy.loginfo("Front Right Scan %s", sonarFR_val)
# rospy.loginfo("Left Scan %s", sonarL_val)
#### rospy.loginfo("Front Left Scan %s", sonarFL_val)
# rospy.loginfo("Front Scan %s", sonarF_val)
rospy.loginfo("Imu Yaw %s", imuYaw)
rospy.loginfo("Velocity %s", velocity.angular.z)
rospy.loginfo("Target %s", theta)
rospy.loginfo("X[2] %s", X[2] * 180 / math.pi)
#fhandle.write("Right Scan {}\n".format( sonarR_val))
#fhandle.write("Front Right Scan {}\n".format( sonarFR_val))
#fhandle.write("Left Scan {}\n".format( sonarL_val))
#fhandle.write("Front Left Scan {}\n".format( sonarFL_val))
#fhandle.write("Front Scan {}\n".format( sonarF_val))
#fhandle.write("Angular Velocity {}\nLinear Velocity {}\n".format(velocity.angular.z, velocity.linear.x))
velocity_pub.publish(velocity)
imuAngVelZ = 0.0
imuLinAccX = 0.0 # linear acceleration
imuLinAccY = 0.0
imuLinAccZ = 0.0
start_point = [0.45, 0.45, -math.pi / 2]
end_point = [0.0, -0.45]
target_ang = 0.0
state = 2
count_state_0 = 0
theta = math.atan2(end_point[1]- start_point[1], end_point[0]- start_point[0]) - start_point[2]
pd_turn = pdcon.pd_controller(setpoint = theta, kp = 9, kd = 1.9)
attractive_gain = 80.0
repulsive_gain = 150.0
grid_area_width = 1.5
grid_resolution = 0.075
robot_radious = 0.17
cells_count = int(grid_area_width / grid_resolution)
obstacle = [(0.15 + 0.075, -0.15 - 0.075)]
obstacle1 = (0.1,-0.1)
moves = [[1,0],
[0,1],
[-1,0],
[0,-1],
[-1,-1],
cur, obs, obs_s = curvemath.find_curve(
start=start,
end=end,
resolution=resolution,
safety_net=safety_net,
obstacles=obstacles,
smoothing_range=0.8,
plot_ret=True) # curvemath returns the curve
startang_curve = np.arctan2(cur[1][1] - cur[0][1],
cur[1][0] - cur[0][0]) # find the starting angle
startangle = linemath.norm(startang_curve - startang_robot)
fhandle = file("/home/ubuntu/catkin_ws/src/alpha_star/scripts/info",
'w') # for debugging and plotting
pd_turn = pdcon.pd_controller(setpoint=startangle,
kp=5,
kd=0.4,
f=linemath.norm)
esc_pd = False
def send_velocity():
global pd0, pd, state, state2start, startangle, ready_to_write
global imuYaw
global state, esc_pd
global count_state_0
global X
x, y, theta = X
velocity_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
velocity = Twist()
if time.time() - globalstarttime > 40 and ready_to_write:
state = 2
state = 0
ready_to_write = True
angular_velocity_limit = 2
#planning
start = (0.45, 0.45)
end = (0.30, -0.55)
startang_robot = -np.pi / 2
obstacles = [((0.30, 0.15), (-0.30, -0.15))]
cur, obs, obs_s = curvemath.find_curve(start=start,
end=end,
obstacles=obstacles,
smoothing_range=0.8,
plot_ret=True)
pd = pdcon.pd_controller(setpoint=0, kp=13, kd=5)
startang_curve = np.arctan2(cur[1][1] - cur[0][1], cur[1][0] - cur[0][0])
startangle = linemath.norm(startang_curve - startang_robot)
fhandle = file("/home/ubuntu/catkin_ws/src/alpha_star/scripts/info", 'w')
pd_turn = pdcon.pd_controller(setpoint=startangle, kp=5, kd=0.4)
def send_velocity():
global pd0, pd, state, substate, state2start, startangle, ready_to_write
global imuYaw
global state
global count_state_0
global X
x, y, theta = X
velocity_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
velocity = Twist()
#using 3 as inf
sonarFL_val = 3.0
sonarFR_val = 3.0
sonarL_val = 3.0
sonarR_val = 3.0
sonarL = [] #lists for smoothing inputs
sonarFL = []
sonarF = []
bound = 0.4
see_bound = 0.3
pd_bound = 0.3
state = 0
substate = 0
state2start = 0
pd0 = pdcon.pd_controller(setpoint=0.3, kp=25, kd=2.5)
pd = pdcon.pd_controller(setpoint=0.2, kp=25, kd=2.5)
def send_velocity():
global sonarF_val
global sonarFL_val
global sonarFR_val
global sonarL_val
global sonarR_val
global pd0, pd, state, substate, state2start
global sonarF, sonarL, sonarFL
velocity_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
velocity = Twist()
ctime = time.time()
tentative_gScore = gScore.get(current, inf) + h(current, neighbor)
if neighbor not in openSet:
openSet.add(neighbor)
if tentative_gScore < gScore.get(neighbor, inf):
cameFrom[neighbor] = current
gScore[neighbor] = tentative_gScore
fScore[neighbor] = gScore.get(neighbor, inf) + h(
neighbor, goal)
return None
curve = A_Star(which_cell(start), which_cell(end), my_dist)
#1, 30
#0.5, 40
#pda = pdcon.pd_controller(setpoint = 0, kp = 0.05, kd = 1)
pda = pdcon.pd_controller(setpoint=0, kp=4 / np.pi, kd=1 / (2 * np.pi))
pd = pdcon.pd_controller(setpoint=0, kp=10, kd=12)
obs = [coords(i) for i in plotobstacles]
cur = [coords(i) for i in curve]
startang_curve = np.arctan2(cur[1][1] - cur[0][1], cur[1][0] - cur[0][0])
startangle = linemath.norm(startang_curve - startang_robot)
fhandle = file("/home/ubuntu/catkin_ws/src/alpha_star/scripts/info", 'w')
pd_turn = pdcon.pd_controller(setpoint=startangle, kp=6, kd=4)
def send_velocity():
global sonarF_val
global sonarFL_val
global sonarFR_val
global sonarL_val