ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

husky_robot topples on calling /base_laser -> /odom transform

asked 2020-11-03 16:47:11 -0600

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 (velocity_publisher) and two Subscribers, namely 'scan_subscriber' and 'tf_subscriber'. Both the Subscribers subscribe to the '/scan' topic. Their callback functions are 'laser_callback' 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()
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-11-04 23:20:10 -0600

miura gravatar image

I suggest that laser_callback and tf_callback be combined into one method. You may only have one of the methods working on the /scan topic.

I'm also wondering if the /scan topic's frame_id is base_laser? You can check it with rostopic echo /scan.

edit flag offensive delete link more

Comments

'/scan' topic's frame_id is 'base_laser'. I had initially tried to combine the two under the same callback functions. That didn't resolve the issue. And, that kind of makes sense as well.

Also, while debugging, I found that the bot is toppling because of the line listener.waitForTransform('/odom' , '/base_laser' , rospy.Time(), rospy.Duration(4.0))

If I just comment out that line and keep odom_point = listener.transformPoint('/odom' , laser_point), the bot works just fine. However, I get an exception that "odom" passed to lookupTransform argument target_frame does not exist.

How can odom_point = listener.transformPoint('/odom' , laser_point) interfere with the publishing of the topic '/cmd_vel' ?

skpro19 gravatar image skpro19  ( 2020-11-05 07:39:16 -0600 )edit

Try Changing rospy.Time() to rospy.Time().now() in listener.waitForTransform('/odom', '/base_laser', rospy.Time(), rospy.Duration(4.0)). ref: http://wiki.ros.org/tf/Tutorials/tf%2...

miura gravatar image miura  ( 2020-11-05 08:38:59 -0600 )edit

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: 2020-11-03 16:47:11 -0600

Seen: 178 times

Last updated: Nov 04 '20