A new version of a robot operating system (ROS-2) has been developed to address the real-time and fault constraints of distributed robotics applications. However, current implementations lack strong real-time scheduling and the optimization of response time for various tasks and applications. This may lead to inconsistent system behavior and may affect system performance. This article presents a new and efficient heuristic scheduling algorithm for the ROS-2 framework to improve resource mapping and system scheduling for robotics applications. The proposed scheduling design includes grouping callbacks into executors, assigning executor priority, and sequencing callbacks inside executors. The timing constraints and functional properties are expressed in the formal design model, and real-time scheduling is performed with respect to the timing constraints. The benefits of the proposed solution are demonstrated by a set of experimental results. Using this algorithm, it is shown that the number of executors needed to schedule a set of callbacks is minimized and the system schedulability is maximized for loaded and overloaded cases.