Robotics StackExchange | Archived questions

Creating a node which contains both a subscriber and publisher with Python Class

I am creating a node to solve the following quiz: In this Quiz, you will create a code to make the robot avoid the wall that is in front of him. To help you achieve this, let's divide the project down into smaller units:

In this Quiz, you will create a code to make the robot avoid the wall that is in front of him. To help you achieve this, let's divide the project down into smaller units:

My solution which doesn't work is shown as below:

#! /usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist


class LaserCmdNode(object):

    def __init__(self, topic_name='/kobuki/laser/scan'):

        self._topic_name = topic_name

        self._laser_scan_sub = rospy.Subscriber(
                '/kobuki/laser/scan', LaserScan, self.topic_callback)
            self._laserscan_object = LaserScan()

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

def topic_callback(self, msg):

self._laserscan_object = msg

print self._laserscan_object.ranges[360]
# self.get_laserscan_to_cmd()

# return self._laserscan_object

def get_laserscan_to_cmd(self):
# return self._laserscan_object

if self._laserscan_object.ranges[360] > 1:
    self._twist_object.linear.x = 0.1
    self._twist_object.angular.z = 0
if self._laserscan_object.ranges[360] < 1:
    self._twist_object.linear.x = 0
    self._twist_object.angular.z = 0.5
if self._laserscan_object.ranges[719] < 1:
    self._twist_objectd.linear.x = 0
    self._twist_object.angular.z = -0.5
if self._laserscan_object.ranges[0] < 1:
        self._twist_object.linear.x = 0
            self._twist_object.angular.z = 0.5
        else:
            pass
    # pub.publish(self._twist_object)
        self._cmd_vel_pub.publish(self._twist_object)


if __name__ == "__main__":
    rospy.init_node('laserscan_cmd_node')
    LaserCmdNode_class_object = LaserCmdNode()
    rate = rospy.Rate(1)

ctrl_c = False

def shutdownhook():
    global ctrl_c
    print "shutdown time! Steven is a great guy and he never gives up. He is gonna earn his PhD soon."
rospy.on_shutdown(shutdownhook)

while not ctrl_c:
    LaserCmdNode_class_object.get_laserscan_to_cmd()
    rate.sleep

After I made the following change the code works as expected: I made the following two change in my code and it starts working. First, I added self.getlaserscantocmd() in my def topiccallback(self, msg): Secondly, I commented out this line : LaserCmdNodeclassobject.getlaserscantocmd() in my if name == "main": With your expertise, do you know why it works this time? Do I have to call my getlaserscantocmd() in my topic_callback function? Why can't I call it in my if name == "main": part of code?Thanks a lot?

The following is the working code after my changes:

! /usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class LaserCmdNode(object):

def __init__(self, topic_name='/kobuki/laser/scan'):

    self._topic_name = topic_name

        self._laser_scan_sub = rospy.Subscriber(
        '/kobuki/laser/scan', LaserScan, self.topic_callback)
    self._laserscan_object = LaserScan()

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

def topic_callback(self, msg):

self._laserscan_object = msg

print self._laserscan_object.ranges[360]
self.get_laserscan_to_cmd()

# return self._laserscan_object

def get_laserscan_to_cmd(self):
# return self._laserscan_object

if self._laserscan_object.ranges[360] > 1:
        self._twist_object.linear.x = 0.1
        self._twist_object.angular.z = 0
    if self._laserscan_object.ranges[360] < 1:
        self._twist_object.linear.x = 0
        self._twist_object.angular.z = 0.5
    if self._laserscan_object.ranges[719] < 1:
        self._twist_objectd.linear.x = 0
            self._twist_object.angular.z = -0.5
        if self._laserscan_object.ranges[0] < 1:
            self._twist_object.linear.x = 0
        self._twist_object.angular.z = 0.5
    else:
        pass
# pub.publish(self._twist_object)
    self._cmd_vel_pub.publish(self._twist_object)
if name == "main":
rospy.init_node('laserscan_cmd_node')
LaserCmdNode_class_object = LaserCmdNode()

    rate = rospy.Rate(1)

ctrl_c = False

def shutdownhook():
    global ctrl_c
    print "shutdown time! Steven is a great guy and he never gives up. He is gonna earn his PhD soon."
rospy.on_shutdown(shutdownhook)

while not ctrl_c:
    # LaserCmdNode_class_object.get_laserscan_to_cmd()
    rate.sleep

Asked by stevensu1838 on 2019-03-02 18:53:04 UTC

Comments

Answers

It hard to read your python code since the formatting is a mess. Please correct at least the indentation. Also I believe you meant:

rate.sleep()  # remember parentheses 

Asked by knxa on 2019-03-03 02:59:43 UTC

Comments