decision_making_node.py 文件源码

def publish():
    header = Header()
    header.stamp = rospy.Time.now()
    header.frame_id = 'map'

    for role, robot_id in WorldModel.assignments.items():
        if robot_id is not None:
            command = WorldModel.commands[role]

            if command.velocity_control_enable:
                msg = TwistStamped()
                msg.header = header
                msg.twist = command.target_velocity
                pubs_velocity[robot_id].publish(msg)
            else:
                send_pose = WorldModel.tf_listener.transformPose("map",command.target_pose)
                send_pose.header.stamp = rospy.Time.now()
                pubs_position[robot_id].publish(send_pose)

            pubs_kick_velocity[robot_id].publish(Float32(command.kick_power))

            status = AIStatus()
            # TODO(Asit) use navigation_enable instead avoidBall.
            status.avoidBall = command.navigation_enable
            status.do_chip = command.chip_enable
            status.dribble_power = command.dribble_power
            pubs_ai_status[robot_id].publish(status)

            command.reset_adjustments()