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")
Make sure the values are actually updating in self.map Check by printing new map information in your callback function.
Also add :
rospy.spin() will execute the code in an infinite loop unless interrupted.
Suggestion:
You can name the function
instead of
As you can access self.map in the class easily.