Publishing a value calculated in a function in ROS

asked 2020-12-12 02:38:03 -0500

Siddhant0712 gravatar image

updated 2022-04-30 13:31:24 -0500

lucasw gravatar image

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.

edit retag flag offensive close merge delete