Ask Your Question
0

My node is stopping - error in threading.py

asked 2015-11-25 13:17:41 -0600

Oevli gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2015-11-26 01:44:49 -0600

Oevli gravatar image

updated 2015-11-27 01:40:44 -0600

I was able to fix the problem by adding rospy.sleep(1) in the end of the while not rosy.is_shutdown(): statement

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-11-25 13:17:41 -0600

Seen: 170 times

Last updated: Nov 27 '15