I can't figure out why I'm getting this error. Any help would be appreciated.
This code is for basic autonomous navigation of a small all terrain vehicle. For some reason it's getting caught in the heading and bearing calculation functions. I've tried putting either one first in the run function, and it does the same thing.
I'm fairly inexperienced with python, so it's likely something simple that I am overlooking.
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3Stamped
from geometry_msgs.msg import Vector3
from math import radians
from sensor_msgs.msg import NavSatFix
import time
import numpy
lat = 0.0
lon = 0.0
x = 0.0
y = 0.0
z = 0.0
head = 0.0
bear = 0.0
###########################################################
destLat = 30.210406
# Destination
destLon = -92.022914
############################################################
class sub():
def __init__(self):
rospy.init_node('Test1', anonymous=False)
rospy.Subscriber("/imu_um6/mag", Vector3Stamped, self.call_1)
rospy.Subscriber("/gps/fix", NavSatFix, self.call_2)
def call_1(self, mag):
global x
global y
global z
x = mag.vector.x
y= mag.vector.y
z= mag.vector.z
def call_2(self, gps):
global lat
global lon
lat = gps.latitude
lon = gps.longitude
def head(lat, lon):
global head
# Define heading here
head = numpy.rad2deg(numpy.arctan2(z, y)) + 90
print(head)
def bear(y,z):
global bear
# Define bearing Here
dLon = destLon - lon
vert = numpy.sin(dLon) * numpy.cos(destLat)
horiz = numpy.cos(lat)*numpy.sin(destLat) - numpy.sin(lat)*numpy.cos(destLat)*numpy.cos(dLon)
bear = (numpy.rad2deg(numpy.arctan2(vert, horiz)) + 360) % 360
print(bear)
def nav(head, bear, destLat, destLon):
# Do Navigation Here
pub = rospy.Publisher('/husky_velocity_controller/cmd_vel', Twist, queue_size=10)
move_cmd = Twist()
turn_cmd = Twist()
move_cmd.linear.x = 2
turn_cmd.angular.z = radians(45)
turn_angle = head - bear
# Prettify the angle
if (turn_angle > 180):
turn_angle -= 360
elif (turn_angle < -180):
turn_angle += 360
else:
turn_angle = turn_angle
if (abs(lat-destLat)<.0005 and abs(lon-destLon)<.0005):
pub.publish(Twist())
r.sleep()
else:
if (abs(turn_angle) < 8):
pub.publish(move_cmd)
rospy.spin()
else:
pub.publish(turn_cmd)
r = rospy.Rate(5);
r.sleep()
def shutdown(self):
rospy.loginfo("Stop Husky")
cmd_vel.publish(Twist())
rospy.sleep(1)
def run():
sub()
bear(y,z)
head(lat,lon)
nav(head, bear, destLat, destLon)
print('here')
if __name__ == '__main__':
try:
while not rospy.is_shutdown():
run()
except rospy.ROSInterruptException:
rospy.loginfo("stopped")
pass
Here's the whole output:
163.11651134
90.0
here
Traceback (most recent call last):
File "classTest.py", line 117, in <module>
run()
File "classTest.py", line 107, in run
bear(y,z)
TypeError: 'numpy.float64' object is not callable
You can't use the same variable name for a function and a float (in the same namespace). And you both defined a bear
function and a bear
variable pointing to a float. You need to change one of the two names.
Wrong, so wrong!!! :)
def bear(y,z):
global bear
....
If you love us? You can donate to us via Paypal or buy me a coffee so we can maintain and grow! Thank you!
Donate Us With