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