def talker(): pub = rospy.Publisher('/some/topic', target, queue_size=1000) rospy.init_node('target_dude', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): target_vels = target() target_vels.linvel = 0.2 target_vels.angvel = 0.0 pub.publish(target_vels) rate.sleep()
def calc_angle(cx, cy, ctheta, gx, gy, gtheta): pub = rospy.Publisher("/some/topic", target, queue_size=1000) delta_x = gx - cx delta_y = gy - cy kp = 0.8 msg = target() print("my goal is : ", gx, gy) if not delta_x == 0: # print("Detla X WAS NOT 0") if 0 > delta_x and 0 < delta_y: vtheta = pi + atan(delta_y / delta_x) elif 0 < delta_x and 0 > delta_y: vtheta = atan(delta_y / delta_x) elif 0 > delta_x and 0 > delta_y: vtheta = atan(delta_y / delta_x) - pi else: vtheta = atan(delta_y / delta_x) if 0 < ctheta and ctheta > vtheta + pi: angle = vtheta - ctheta + 2 * pi else: angle = vtheta - ctheta msg.linvel = 0.2 msg.angvel = angle * kp vr = (2 * msg.linvel + msg.angvel * 0.125) / (2 * 0.02) vl = (2 * msg.linvel - msg.angvel * 0.125) / (2 * 0.02) #print("rVel = %f" % vr) #print("lVel = %f" % vl) pub.publish(msg)
def calc_angle(cx, cy, ctheta, gx, gy, mode): global goal_reached pub = rospy.Publisher("/some/topic", target, queue_size=1000) #pub_log = rospy.Publisher("/wheel/vels", Pose2D, queue_size=1000) anglim = 7.0 linlim = 0.5 direct = True kp = 3 kp_lin = 0.7 delta_x = gx - cx delta_y = gy - cy msg = target() #-----------------------------Calc Angle Error-------------------------------- if not delta_x == 0: if 0 > delta_x and 0 < delta_y: vtheta = pi + atan(delta_y / delta_x) elif 0 < delta_x and 0 > delta_y: vtheta = atan(delta_y / delta_x) elif 0 > delta_x and 0 > delta_y: vtheta = atan(delta_y / delta_x) - pi else: vtheta = atan(delta_y / delta_x) if ctheta < vtheta - pi: angle = vtheta - ctheta - 2 * pi elif ctheta > vtheta + pi: angle = vtheta - ctheta + 2 * pi else: angle = vtheta - ctheta print("BEFORE :", degrees(angle)) if angle > pi / 2: angle = (angle - pi) direct = False if angle < -pi / 2: angle = (angle + pi) direct = False print("AFTER :", degrees(angle)) #print(vtheta, ctheta, degrees(angle)) #---------------------------Velocity assigment--------------------------------- angvel = angle * kp if angvel > anglim: angvel = anglim elif angvel < -anglim: angvel = -anglim linvel = linlim - (linlim / anglim) * abs(angvel) if direct == False: linvel = -linvel #--------------------------------Breaking-------------------------------------- dist = sqrt(delta_x**2 + delta_y**2) #print("I was called", gx, gy, mode) #print("DISTANCE TO Goal :", dist) if dist < break_length and linvel > linlim * kp_lin and mode == 0: if direct == True: linvel = linlim * kp_lin else: linvel = -linlim * kp_lin #--------------------------------Stopping-------------------------------------- if dist < 0.050: if mode != 0: linvel = 0 angvel = 0 msg.linvel = linvel msg.angvel = angvel pub.publish(msg) rospy.sleep(2) goal_reached = True msg.linvel = linvel msg.angvel = angvel pub.publish(msg)
#!/usr/bin/env python import rospy from AGV5.msg import target pub = rospy.Publisher('target/velocities', target, queue_size=1000) rospy.init_node('publish_mymessage') r = rospy.Rate(2) while not rospy.is_shutdown(): t = target() t.linvel = 0.8 t.angvel = 0.4 pub.publish(t) r.sleep()