I have a Gazebo world with a husky robot and an obstacle in front of it. I am writing a node to guide the robot towards the obstacle.

As we can see in the code below, inside the HuskyBot class Constructor, I have one Publisher (velocitypublisher) and two Subscribers, namely '**scansubscriber' and 'tfsubscriber'. Both the Subscribers subscribe to the '/scan' topic. Their callback functions are 'lasercallback' and 'tf_callback**' respectively.

The 'laser_callback' function determines the closest point w.r.t. to the laser mounted on the robot. It updates the value of self.laser_point and steers the robot towards the obstacle using a simple P controller. This is wokring exactly as intended.

However, when I enable the 'tf_callback' function and then run the node, the robot starts behaving randomly. After a second or two, it topples in the Gazebo world.

Why is this happening?

#!/usr/bin/python

import rospy
from sensor_msgs.msg import LaserScan
import math
from geometry_msgs.msg import PointStamped,  Point, Twist
import tf
from visualization_msgs.msg import Marker 

class HuskyBot: 

    def __init__(self): 

    rospy.init_node('husky_node')

    self.laser_point = Point(0,0,0) 

    self.scan_subscriber = rospy.Subscriber('/scan', LaserScan, self.laser_callback)

    self.tf_subscriber = rospy.Subscriber('/scan' , LaserScan, self.tf_callback)

    self.velocity_publisher = rospy.Publisher('/cmd_vel' , Twist, queue_size = 1) 

    self.marker_publisher = rospy.Publisher('/rviz_visusalizer' ,  Marker, queue_size = 1)

    self.rate = rospy.Rate(1)

    rospy.spin()


def tf_callback(self, data): 


    print "Inside the tf_callback  function" 


    laser_point  = PointStamped() 

    laser_point.header.frame_id = "base_laser" 
    laser_point.header.stamp = rospy.Time(0) 

    laser_point.point = self.laser_point 

    listener = tf.TransformListener()

    try: 

        listener.waitForTransform('/odom' , '/base_laser' , rospy.Time(), rospy.Duration(4.0))

        odom_point = listener.transformPoint('/odom' , laser_point)


        print "laser_point.x: ", self.laser_point.x , " laser_point.y: ", self.laser_point.y , " laser_point.z: " , self.laser_point.z 

        print " " 

        print "odom_point.x " , odom_point.point.x, " odom_point.y: ", odom_point.point.y ," odom_point.z: ", odom_point.point.z 


    except Exception as e: 

        print "Following exception was enconuntered while performing the transformation -> " + str(e)



def laser_callback(self, data): 


    pillar_dis = 100000  

    pos = 0 


    for idx, val in enumerate(data.ranges) : 

        if not math.isinf(val): 

            if val  < pillar_dis : 

                pillar_dis = val 

                pos = idx 

    pillar_ang  = data.angle_min + pos * data.angle_increment 


    vel_msg = Twist() 

    tolerance = 0.1 

    if pillar_dis > tolerance : 

        v_x = 1.5 * pillar_dis 

        w_z = 4 * (0 - pillar_ang) 

        vel_msg.linear.x = v_x 
        vel_msg.angular.z  = w_z 

        self.velocity_publisher.publish(vel_msg)



    p_x = pillar_dis * math.cos(pillar_ang) 
    p_y = pillar_dis * math.sin(pillar_ang) 
    p_z = 0 

    self.laser_point = Point(p_x, p_y, p_z) 

    print "laser_point.x: " , self.laser_point.x , " laser_point.y: ", self.laser_point.y, " laser_point.z: " , self.laser_point.z




if __name__ == '__main__' : 

    x = HuskyBot()