ROS how to use values from one callback function to another

2.2k views Asked by At

Sorry, English isn't my mother tongue. I hope you'll understand my question correctly.

I’m using Ubuntu 18.04 with ROS Melodic. I’m trying to take data from mapCallback and odometryCallback functions and use these values in SampleTree function. When I run the code a have below, I want to return frontCones array as a result, but my output is empty array. How can I make the SampleTree function take values, published in mapCallback and odometryCallback functions and use this data in a loop to return frontCones array?

Here is my code:

# !/usr/bin/python

import rospy

from nav_msgs.msg import Odometry
from egn_messages.msg import Map
from tf.transformations import euler_from_quaternion

odometry = Odometry()
map = Map()


class Test:
    def __init__(self):
        rospy.init_node('test_node')

        rospy.Subscriber('/map1', Map, self.mapCallback)
        rospy.Subscriber('/odometry', Odometry, self.odometryCallback)
        self.map = []

        self.carPosX = 0.0
        self.carPosY = 0.0
        self.carPosYaw = 0.0

    def odometryCallback(self, odometry):
        orientation_q = odometry.pose.pose.orientation
        orientation_list = [orientation_q.x, orientation_q.y,
                            orientation_q.z, orientation_q.w]
        (roll, pitch, yaw) = euler_from_quaternion(orientation_list)
        self.carPosX = odometry.pose.pose.position.x
        self.carPosY = odometry.pose.pose.position.y
        self.carPosYaw = yaw

    def mapCallback(self, map):
        self.map = map

    def SampleTree(self):
        if not self.map:
            print
            'map is empty, return'
            return

        frontConesDist = 12
        frontCones = self.getFrontConeObstacles(frontConesDist)
        print(frontCones)

    def getFrontConeObstacles(self, frontDist):
        if not map:
            return []

        headingVector = [1.0, 0]

        behindDist = 0.5
        carPosBehindPoint = [self.carPosX - behindDist * headingVector[0], self.carPosY - behindDist * headingVector[1]]

        frontDistSq = frontDist ** 2

        frontConeList = []
        for cone in map.cones:
            if (headingVectorOrt[0] * (cone.y - carPosBehindPoint[1]) - headingVectorOrt[1] * (
                    cone.x - carPosBehindPoint[0])) < 0:
                if ((cone.x - self.carPosX) ** 2 + (cone.y - self.carPosY) ** 2) < frontDistSq:
                    frontConeList.append(cone)
        return frontConeList


if __name__ == "__main__":
    inst = Test()
    inst.SampleTree()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

1

There are 1 answers

3
HARSH MITTAL On

Make sure the values are actually updating in self.map Check by printing new map information in your callback function.

Also add :

if __name__ == "__main__"():
    inst = Test()
    inst.SampleTree()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

rospy.spin() will execute the code in an infinite loop unless interrupted.

Suggestion:

You can name the function

def getFrontConeObstacles(self, frontDist):

instead of

def getFrontConeObstacles(self, map, frontDist):

As you can access self.map in the class easily.