How to use values from one callback function to another

asked 2021-05-20 10:40:20 -0500

sersurguchev gravatar image

updated 2021-05-31 13:56:11 -0500

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

Comments

Did you create your object like my_obj = Test() and call spinning functions in your main? If so, are you sure that those topics are being published? Where/when are you calling that my_obj.SampleTree() function?

Orhan gravatar image Orhan  ( 2021-05-21 17:29:37 -0500 )edit

Okay now i see the full code, you are creating your object and then exiting. Try adding rospy.spin() to the end of your main function.

Orhan gravatar image Orhan  ( 2021-05-23 17:46:23 -0500 )edit

I've changed my code in this question a little again to remove a few errors, but SampleTree function still doesn't take published data for getFrontConeObstacles function. (output: map is empty, return) I'm running two publishers. One of them sends custom array containing float64 x and float64 y as Map. Also I'm sending Odometry message as second publisher and then I'm running the subscriber with code provided above. What am I doing wrong?

sersurguchev gravatar image sersurguchev  ( 2021-05-31 14:14:05 -0500 )edit

You are calling your function just once, probably before any callback. What you need is probably a timer. This demo_callbackhere in this answer, is called once in every second. (rospy.Duration(1))

Orhan gravatar image Orhan  ( 2021-06-04 20:24:53 -0500 )edit