Publishing a value calculated in a function in ROS
I am working on ROS for an autonomous driving system. I have defined and calculated a value 'priority' in a function as follows:
class Weight:
def __init__(self, cam_loc, prio):
self.cam_loc = cam_loc
self.prio = prio
def odometry_weight_model(velocity, orientation, steering_angle):
rospy.loginfo(velocity)
rospy.loginfo(orientation)
rospy.loginfo(steering_angle)`
const = 0.2
orientation_list = [orientation.x, orientation.y, orientation.z, orientation.w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
steer_ang = math.degrees(steering_angle)
steer_angle = min(30, max(-30, steer_ang))
init_len = 1.0 / len(W1.cam_loc)
for yaw_ang in W1.cam_loc:
cam_length = yaw_ang + steer_angle
length = np.multiply(init_len, steer_angle)
delta = length / np.sum(cam_length)
W1.prio = init_len + delta
Now, I want to publish the value in ROS via the publisher I have defined. I created a new topic and have used the 'Float32' type from the 'std_msgs.msg' pack since the 'priority' is a float value. My attempt for the publisher is as follows:
def vehicle_status_callback(vehicle_status):
odometry_weight_model(
vehicle_status.velocity,
vehicle_status.orientation,
vehicle_status.control.steer,
)
def main():
"""Run the view prioritization"""
rospy.init_node("view_prioritization", anonymous=True)
role_name = rospy.get_param("~role_name", "ego_vehicle")
sensor_definition_file = rospy.get_param("~sensor_definition_file")
with open(sensor_definition_file) as sensor_data:
multi_sensors = json.load(sensor_data)
for i in multi_sensors["sensors"]:
W1.cam_loc.append(i["yaw"])
rospy.Subscriber(
"/carla/ego_vehicle/vehicle_status",
CarlaEgoVehicleStatus,
vehicle_status_callback,
)
rospy.spin()
pub = rospy.Publisher(
"/carla/ego_vehicle/camera/rgb/view/priority",
Float32,
queue_size=10,)
cam_prio = Float32()
cam_prio.data = W1.prio
pub.publish(cam_prio.data)
if __name__ == "__main__":
try:
main()
except rospy.ROSInterruptException:
pass
However, this structure does not return a value in the node. An idea that I have come across is to write the publisher in the init() method and call the publisher in the callback, but I am unsure how to execute this. Any help would be greatly appreciated.