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()