def publish_data(self, event=None):
p_t=pose1()
p_t.position.x=self.pose.x
p_t.position.y=self.pose.y
self.pose_pub.publish(p_t)
#random.normal(0,sd_trans*sd_trans)
p_t_noisy=pose1()
p_t_noisy.position.x=p_t.position.x+random.normal(0,.5)
p_t_noisy.position.y=p_t.position.y+random.normal(0,.5)
self.pose_noisy_pub.publish(p_t_noisy)
def __init__(self):
#Creating our node,publisher and subscriber
rospy.init_node('turtlebot_chase', anonymous=True)
self.vel_pub = rospy.Publisher('/turtle2/cmd_vel',
Twist,
queue_size=10)
self.pose_subscriber = rospy.Subscriber('/rt_real_pose', pose1,
self.target_callback)
self.pose_subscriber = rospy.Subscriber('/turtle2/pose', pose2,
self.callback)
self.pose = pose2()
self.goal_pose = pose1()
self.rate = rospy.Rate(10)
def find_goal(self, candr, p0, v, target_v, tol):
goal_pose = pose1()
ind = 0
#finding the neareast point on the circle
p0 = np.array([
candr[0], candr[1]
]) + candr[2] * (p0 - np.array([candr[0], candr[1]])) / np.linalg.norm(
p0 - np.array([candr[0], candr[1]]))
d = sqrt(
pow(candr[0] - self.pose.x, 2) + pow(candr[1] - self.pose.y, 2))
t_lower = (d - candr[2]) / v
t_higher = (d + candr[2]) / v
t_mean = (-t_lower + t_higher) / 2
theta0 = atan2(-candr[1] + p0[1], -candr[0] + p0[0])
for t in np.linspace(t_lower - t_mean / 4, t_higher + t_mean / 4, 20):
intersections = self.get_circle_intersections(
candr[0], candr[1], candr[2], self.pose.x, self.pose.y, t * v)
print(t)
if len(intersections) != 0:
x_target = candr[2] * sin(
(target_v / candr[2]) * t + theta0) + p0[0]
y_target = -candr[2] * cos(
(target_v / candr[2]) * t + theta0) + p0[1]
first_intersection_distance = pow(
intersections[1] - y_target, 2) + pow(
intersections[0] - x_target, 2)
second_intersection_distance = pow(
intersections[3] - y_target, 2) + pow(
intersections[2] - x_target, 2)
if first_intersection_distance < tol:
goal_pose.position.x = intersections[0]
goal_pose.position.y = intersections[1]
ind = 1
elif second_intersection_distance < tol:
goal_pose.position.x = intersections[2]
goal_pose.position.y = intersections[3]
ind = 1
if ind == 1:
print('sucess')
break
if ind == 0:
goal_pose.position.x = p0[0]
goal_pose.position.y = p0[1]
return (goal_pose, ind)
def find_goal(self, candr, p0, v, target_v, tol):
goal_pose = pose1()
ind = 0
d = sqrt(
pow(candr[0] - self.pose.x, 2) + pow(candr[1] - self.pose.y, 2))
t_lower = (d - candr[2]) / v
t_higher = (d + candr[2]) / v
theta0 = atan2(-candr[1] + p0[1], -candr[0] + p0[0])
for t in np.linspace(t_lower, t_higher, 20):
intersections = self.get_circle_intersections(
candr[0], candr[1], candr[2], self.pose.x, self.pose.y, t * v)
if len(intersections) != 0:
x_target = candr[2] * sin(
(target_v / candr[2]) * t + theta0) + p0[0]
y_target = -candr[2] * cos(
(target_v / candr[2]) * t + theta0) + p0[1]
first_intersection_distance = pow(
intersections[1] - y_target, 2) + pow(
intersections[0] - x_target, 2)
second_intersection_distance = pow(
intersections[3] - y_target, 2) + pow(
intersections[2] - x_target, 2)
if first_intersection_distance < tol:
goal_pose.position.x = intersections[0]
goal_pose.position.y = intersections[1]
ind = 1
elif second_intersection_distance < tol:
goal_pose.position.x = intersections[2]
goal_pose.position.y = intersections[3]
ind = 1
if ind == 1:
print('sucess')
break
if ind == 0:
goal_pose.position.x = p0[0]
goal_pose.position.y = p0[1]
return (goal_pose, ind)
def chase_target(self):
print('hey')
distance_tolerance = .3
vel_msg = Twist()
goal_pose = pose1()
v = 0.3
target_v = 0.8
error = 0
error_i = 0
error_d = 0
errort_i = 0
errort_d = 0
errort = 0
error_pre = 0
errort_pre = 0
kp = 8.0
kd = 0.0
ki = 0.1
kp_t = 4.0
ki_t = 0.0
kd_t = 0.0
#print(self.goal_pose.position.x)
p1 = rospy.wait_for_message('/rt_real_pose', pose1)
p1 = p1.position
p2 = rospy.wait_for_message('/rt_real_pose', pose1)
p2 = p2.position
p3 = rospy.wait_for_message('/rt_real_pose', pose1)
p3 = p3.position
p = self.find_circle(p1.x, p1.y, p2.x, p2.y, p3.x, p3.y)
goal_pose, ind = self.find_goal(p, np.array([p3.x, p3.y]), v, target_v,
distance_tolerance)
ind_i = 0
while sqrt(
pow((goal_pose.position.x - self.pose.x), 2) +
pow((goal_pose.position.y - self.pose.y), 2)
) >= distance_tolerance and (sqrt(
pow((self.target_pose.x - self.pose.x), 2) +
pow((self.target_pose.y - self.pose.y), 2)) >=
distance_tolerance):
p_temp = self.t_pose
if p3.x != self.t_pose.position.x and ind == 0 and ind_i == 0:
p4 = p_temp.position
c_r = self.find_circle(p2.x, p2.y, p3.x, p3.y, p4.x, p4.y)
goal_pose, ind = self.find_goal(
c_r, np.array([p_temp.position.x, p_temp.position.y]), v,
target_v, distance_tolerance)
p3 = self.t_pose.position
ind_i = 1
#Error terms for speed
error = sqrt(
pow((goal_pose.position.x - self.pose.x), 2) +
pow((goal_pose.position.y - self.pose.y), 2))
error_d = error - error_d
error_i = error_i + error_pre
#Error terms for angular speed
errort = atan2(
goal_pose.position.y - self.pose.y,
goal_pose.position.x - self.pose.x) - self.pose.theta
errort = atan2(sin(errort), cos(errort))
errort_d = errort - errort_pre
errort_i = errort + errort_i
v_temp = v #kp * error +kd*error_d +ki*error_i
w_temp = kp_t * errort + kd_t * errort_d + ki_t * errort_i
if (v_temp - self.pose.linear_velocity) > MAX_ACCELERATION:
v_temp = self.pose.linear_velocity + MAX_ACCELERATION
vel_msg.linear.x = v_temp
vel_msg.linear.y = 0
vel_msg.linear.z = 0
#angular velocity in the z-axis:
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = w_temp
#Publishing our vel_msg
self.vel_pub.publish(vel_msg)
error_pre = error
errort_pre = errort
self.rate.sleep()
#Stopping our robot after the movement is over
v_f = 0
while (v_f - self.pose.linear_velocity) <= MAX_DECELERATION:
v_temp = self.pose.linear_velocity + MAX_DECELERATION
# self.decelerate(v_temp,w_temp)
#else:
vel_msg.linear.x = v_temp
vel_msg.angular.z = 0
self.vel_pub.publish(vel_msg)
#vel_msg.linear.x = 0
#vel_msg.angular.z =0
#self.vel_pub.publish(vel_msg)
rospy.spin()
def chase_target(self):
print('hey')
distance_tolerance = .5
vel_msg = Twist()
goal_pose = pose1()
v = 0.3
target_v = 0.8
error = 0
error_i = 0
error_d = 0
errort_i = 0
errort_d = 0
errort = 0
error_pre = 0
errort_pre = 0
kp = 8.0
kd = 0.0
ki = 0.1
kp_t = 4.0
ki_t = 0.0
kd_t = 0.0
#print(self.goal_pose.position.x)
p1 = rospy.wait_for_message('/rt_noisy_pose', pose1)
p2 = rospy.wait_for_message('/rt_noisy_pose', pose1)
p3 = rospy.wait_for_message('/rt_noisy_pose', pose1)
a1 = (p1.position.x**2 + p1.position.y**2)
p1_t = np.array([p1.position.x, p1.position.y, 1])
a2 = (p2.position.x**2 + p2.position.y**2)
p2_t = np.array([p2.position.x, p2.position.y, 1])
a3 = (p3.position.x**2 + p3.position.y**2)
p3_t = np.array([p3.position.x, p3.position.y, 1])
B = np.vstack([p1_t, p2_t, p3_t])
d = np.vstack([a1, a2, a3])
c_r = self.estimate_circle(B, d)
goal_pose, ind = self.find_goal(
c_r, np.array([p3.position.x, p3.position.y]), v, target_v,
distance_tolerance)
ind_i = 0
while (sqrt(
pow((self.target_pose.x - self.pose.x), 2) +
pow((self.target_pose.y - self.pose.y), 2)) >= .8):
p_temp = self.t_pose
if p3.position.x != self.t_pose.position.x and ind == 0 and ind_i == 0:
a_temp = (p_temp.position.x**2 + p_temp.position.y**2)
p_t = np.array([p_temp.position.x, p_temp.position.y, 1])
B = np.vstack([B, p_t])
d = np.vstack([d, a_temp])
c_r = self.estimate_circle(B, d)
goal_pose, ind = self.find_goal(
c_r, np.array([p_temp.position.x, p_temp.position.y]), v,
target_v, distance_tolerance)
p3 = self.t_pose
ind_i = 1
#Error terms for speed
error = sqrt(
pow((goal_pose.position.x - self.pose.x), 2) +
pow((goal_pose.position.y - self.pose.y), 2))
error_d = error - error_d
error_i = error_i + error_pre
#Error terms for angular speed
errort = atan2(
goal_pose.position.y - self.pose.y,
goal_pose.position.x - self.pose.x) - self.pose.theta
errort = atan2(sin(errort), cos(errort))
errort_d = errort - errort_pre
errort_i = errort + errort_i
v_temp = v #kp * error +kd*error_d +ki*error_i
w_temp = kp_t * errort + kd_t * errort_d + ki_t * errort_i
if (v_temp - self.pose.linear_velocity) > MAX_ACCELERATION:
v_temp = self.pose.linear_velocity + MAX_ACCELERATION
vel_msg.linear.x = v_temp
vel_msg.linear.y = 0
vel_msg.linear.z = 0
#angular velocity in the z-axis:
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = w_temp
#Publishing our vel_msg
self.vel_pub.publish(vel_msg)
error_pre = error
errort_pre = errort
self.rate.sleep()
#Stopping our robot after the movement is over
v_f = 0
while (v_f - self.pose.linear_velocity) <= MAX_DECELERATION:
v_temp = self.pose.linear_velocity + MAX_DECELERATION
# self.decelerate(v_temp,w_temp)
#else:
vel_msg.linear.x = v_temp
vel_msg.angular.z = 0
self.vel_pub.publish(vel_msg)
self.rate.sleep()
#vel_msg.linear.x = 0
#vel_msg.angular.z =0
#self.vel_pub.publish(vel_msg)
rospy.spin()
def chase_target(self):
print('hey')
distance_tolerance = .8
vel_msg = Twist()
goal_pose = pose1()
v = 0.3
error = 0
error_i = 0
error_d = 0
errort_i = 0
errort_d = 0
errort = 0
error_pre = 0
errort_pre = 0
kp = 8.0
kd = 0.0
ki = 0.1
kp_t = 4.0
ki_t = 0.0
kd_t = 0.0
#print(self.goal_pose.position.x)
p1 = rospy.wait_for_message('/rt_real_pose', pose1)
print(p1)
p2 = rospy.wait_for_message('/rt_real_pose', pose1)
print(p2)
p3 = rospy.wait_for_message('/rt_real_pose', pose1)
for t in np.linspace(20, 40, 20):
intersections = self.get_circle_intersections(
p[0], p[1], p[2], self.pose.x, self.pose.y, t * v)
if len(intersections) != 0:
break
print(intersections)
first_intersection_angle = atan2(intersections[1] - self.pose.y,
intersections[0] - self.pose.x)
second_intersection_angle = atan2(intersections[3] - self.pose.y,
intersections[2] - self.pose.x)
if abs(first_intersection_angle) < abs(second_intersection_angle):
goal_pose.position.x = intersections[0]
goal_pose.position.y = intersections[1]
else:
goal_pose.position.x = intersections[2]
goal_pose.position.y = intersections[3]
while sqrt(
pow((goal_pose.position.x - self.pose.x), 2) +
pow((goal_pose.position.y -
self.pose.y), 2)) >= distance_tolerance:
#Error terms for speed
error = sqrt(
pow((goal_pose.position.x - self.pose.x), 2) +
pow((goal_pose.position.y - self.pose.y), 2))
error_d = error - error_d
error_i = error_i + error_pre
#Error terms for angular speed
errort = atan2(
goal_pose.position.y - self.pose.y,
goal_pose.position.x - self.pose.x) - self.pose.theta
errort = atan2(sin(errort), cos(errort))
errort_d = errort - errort_pre
errort_i = errort + errort_i
print("hello", error)
v_temp = v #kp * error +kd*error_d +ki*error_i
w_temp = kp_t * errort + kd_t * errort_d + ki_t * errort_i
if (v_temp - self.pose.linear_velocity) > MAX_ACCELERATION:
v_temp = self.pose.linear_velocity + MAX_ACCELERATION
vel_msg.linear.x = v_temp
vel_msg.linear.y = 0
vel_msg.linear.z = 0
#angular velocity in the z-axis:
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = w_temp
#Publishing our vel_msg
self.vel_pub.publish(vel_msg)
error_pre = error
errort_pre = errort
self.rate.sleep()
#Stopping our robot after the movement is over
v_f = 0
while (v_f - self.pose.linear_velocity) <= MAX_DECELERATION:
v_temp = self.pose.linear_velocity + MAX_DECELERATION
# self.decelerate(v_temp,w_temp)
#else:
vel_msg.linear.x = v_temp
vel_msg.angular.z = 0
self.vel_pub.publish(vel_msg)
#vel_msg.linear.x = 0
#vel_msg.angular.z =0
#self.vel_pub.publish(vel_msg)
rospy.spin()
