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
runworks one time; it's the second call that encounters a differentbear(andhead). I would try to get rid of thisglobalsapproach to saving the state. Saving state as attributes of a class object is better. But I'm not sure how that will play with therosbycalls.