Hello
I have written an navigation algorithm, which takes input from an Imu and Hokuyo Urg laser range scanner and uses those to calculate a velocity and a angle for direction.
However my node stop after just about a second when running it.
I have hokuyo_node and razor_imu_9dof running in different terminalwindows.
I get the following error message:
Exception in thread /scan (most likely raised during interpreter shutdown): Traceback (most recent call last): File "/usr/lib/python2.7/threading.py", line 810, in __bootstrap_inner File "/usr/lib/python2.7/threading.py", line 763, in run File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_pubsub.py", line 183, in robust_connect_subscriber File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 792, in receive_loop <type 'exceptions.TypeError'>: 'NoneType' object is not callable
I code is quite long with lots of different callback - but here goes:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import numpy as np
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion
######## defining global variables ############
ranges = []
angle_min = []
angle_increment = []
wheelVel1 = []
wheelVel2 = []
wheelVel3 = []
wheelVel4 = []
target = []
turn = []
orientation = []
blocked = 0
######## Callback functions ####################
def scan_callback(scan):
global ranges, angle_min, angle_increment
rospy.loginfo(scan.ranges)
rospy.loginfo(scan.angle_increment)
rospy.loginfo(scan.angle_min)
ranges = scan.ranges
angle_min = scan.angle_min
angle_increment = scan.angle_increment
def imu_turn_callback():
global target
#use Imu data to for setting target
def imu_row_callback(Imu):
global orientation
rospy.loginfo(Imu.orientation)
q0 = Imu.orientation.x
q1 = Imu.orientation.y
q2 = Imu.orientation.z
q3 = Imu.orientation.w
#convertering from quaternion to euler degrees_ http://sunday-lab.blogspot.dk/2008/04/get-pitch-yaw-roll-from-quaternion.html
#Note not the most trust worty source - but formula is vertified
#yaw:
yaw = np.arctan2(2*(q0*q1 + q3*q2), q3*q3 + q0*q0 - q1*q1 - q2*q2)
orientation = yaw
def imu_init_callback(Imu):
global target
rospy.loginfo(Imu.orientation)
q0 = Imu.orientation.x
q1 = Imu.orientation.y
q2 = Imu.orientation.z
q3 = Imu.orientation.w
yaw = np.arctan2(2*(q0*q1 + q3*q2), q3*q3 + q0*q0 - q1*q1 - q2*q2)
target = yaw
def encoder_callback():
global wheelVel1, wheelVel2, wheelVel3, wheelVel4
#update global wheelVel
###### Subscriber functions ########
def init_target():
rospy.Subscriber("imu", Imu, imu_init_callback)
def laser_listener():
rospy.Subscriber("scan", LaserScan, scan_callback)
def imu_listener_turn():
rospy.Subscriber("imu", Imu, imu_turn_callback)
#def encoder_info():
# rospy.Subscriber()##Tilpas encoder msg!!!!, encoder_callback)
def imu_row_listener():
rospy.Subscriber("imu", Imu, imu_row_callback)
######## Navigation algoritms #################
def turning(): #turning global algoritm -- use IMU for updating new target for row navigation
global target
#do turn
#use IMU for updating global target
def VPH():
global ranges, angle_min, angle_increment, wheelVel1, wheelVel2, wheelVel3, wheelVel4, target, orientation, blocked
#Number of scans in plane (#obstacles)
noPoints = len(ranges)
#encoder_info() #updates the global wheelVel variables
imu_row_listener() #get current robot orientation in order to compute current target Dtarget
Dtarget = np.array(target) - np.array(orientation) #computes a transformed target from the global set target and the current orientation
#Compute V_t from wheelVel(1-4)
r = 0.12 #radius of wheels
V_t = np.average(wheelVel1 + wheelVel2 + wheelVel3 + wheelVel4)*r #MIDLERTID V_t REPRÆSENTATION - find på bedre funktion
### Robot and navigation parameters ###
D_max = 4 #Max range of laserscanner
R = 0.3 #Radius of robot
D = 0.05 #"safe" distance between robot and obstacle
V_max = 4.0 #robot topspeed
W_max = 10 #minimal window for opening
k1 = 4 #coefficient making the path more target orientated
k2 = 2 #coefficient making the path more smooth
angI = np.zeros(noPoints)
H = np.zeros(noPoints)
for i in range(0,noPoints):
angI[i] = angle_min + i * angle_increment
D[i] = ((V_t^2*np.cos(angI[i])*np.cos(angI[i])) / (2*a)) + R + D_safe
if ranges[i] >= D[i]:
H[i] = 1
count = 0
W = np.zeros(noPoints)
for i in range(0,noPoints):
if H[i] == 1:
count += 1
else:
if count >= W_max:
for n in range(i - count, i-1):
W[n] = count
count = 0
else:
count = 0
U = []
for i in range(0, noPoints):
if H[i] == 1 and W[i] >= W_max:
U = i
if not(U):
blocked = 1
else:
for i in U:
C[i] = k1 *(AngI[i] - Dtarget) + k2*AngI[i]
steering_angle_entri = np.argmin(C)
steering_angle = angI[steering_angle_entri]
V = (1 - (D_max - ranges[angI[steering_angle_entri]])/(D_max-(R+D)))*V_max
#pub = rospy.Publisher("", , queue_size=5)
if __name__ == '__main__':
rospy.init_node('mark_pi_navigation', anonymous=True)
init_target()
while not rospy.is_shutdown():
laser_listener()
if turn==1:
turning()
else:
VPH()
if blocked == 1:
print("blocked")#start "I´m stucked navigation
Originally posted by Oevli on ROS Answers with karma: 30 on 2015-11-25
Post score: 0