Skip to content

Commit c5344aa

Browse files
carlossvgcarlossvg
committed
Split nodes in real-time and non-realtime parts
- Creates a real-time callback group - Creates a real-time loop function using rclcpp::waitset
1 parent dff4bf9 commit c5344aa

File tree

10 files changed

+394
-7
lines changed

10 files changed

+394
-7
lines changed

‎pendulum_bringup/launch/pendulum_bringup.launch.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,7 @@ def generate_launch_description():
7474
# Node definitions
7575
pendulum_demo_runner = Node(
7676
package='pendulum_demo',
77-
executable='pendulum_demo',
77+
executable='pendulum_demo_waitset',
7878
output='screen',
7979
parameters=[param_file],
8080
arguments=[

‎pendulum_bringup/launch/pendulum_bringup_rviz.launch.py‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
# Copyright 2019 Carlos San Vicente
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import os
16+
17+
from launch import LaunchDescription
18+
from launch.actions import DeclareLaunchArgument
19+
from launch.conditions import IfCondition
20+
import launch.substitutions
21+
from launch.substitutions import LaunchConfiguration
22+
from launch_ros.actions import Node
23+
from launch_ros.substitutions import FindPackageShare
24+
25+
26+
def generate_launch_description():
27+
# Get the bringup directory
28+
bringup_dir = FindPackageShare('pendulum_bringup').find('pendulum_bringup')
29+
30+
# Set robot description parameters
31+
urdf_file = os.path.join(bringup_dir, 'urdf', 'pendulum.urdf')
32+
with open(urdf_file, 'r') as infp:
33+
robot_desc = infp.read()
34+
rsp_params = {'robot_description': robot_desc}
35+
36+
# Set parameter file path
37+
param_file_path = os.path.join(bringup_dir, 'params', 'pendulum.param.yaml')
38+
param_file = launch.substitutions.LaunchConfiguration('params', default=[param_file_path])
39+
40+
# Set rviz config path
41+
rviz_cfg_path = os.path.join(bringup_dir, 'rviz/pendulum.rviz')
42+
43+
# Create the launch configuration variables
44+
autostart_param = DeclareLaunchArgument(
45+
name='autostart',
46+
default_value='True',
47+
description='Automatically start lifecycle nodes')
48+
priority_param = DeclareLaunchArgument(
49+
name='priority',
50+
default_value='0',
51+
description='Set process priority')
52+
cpu_affinity_param = DeclareLaunchArgument(
53+
name='cpu-affinity',
54+
default_value='0',
55+
description='Set process CPU affinity')
56+
with_lock_memory_param = DeclareLaunchArgument(
57+
name='lock-memory',
58+
default_value='False',
59+
description='Lock the process memory')
60+
lock_memory_size_param = DeclareLaunchArgument(
61+
name='lock-memory-size',
62+
default_value='0',
63+
description='Set lock memory size in MB')
64+
config_child_threads_param = DeclareLaunchArgument(
65+
name='config-child-threads',
66+
default_value='False',
67+
description='Configure process child threads (typically DDS threads)')
68+
with_rviz_param = DeclareLaunchArgument(
69+
'rviz',
70+
default_value='False',
71+
description='Launch RVIZ2 in addition to other nodes'
72+
)
73+
74+
# Node definitions
75+
76+
robot_state_publisher_runner = Node(
77+
package='robot_state_publisher',
78+
executable='robot_state_publisher',
79+
output='screen',
80+
parameters=[rsp_params],
81+
condition=IfCondition(LaunchConfiguration('rviz'))
82+
)
83+
84+
rviz_runner = Node(
85+
package='rviz2',
86+
executable='rviz2',
87+
name='rviz2',
88+
arguments=['-d', str(rviz_cfg_path)],
89+
condition=IfCondition(LaunchConfiguration('rviz'))
90+
)
91+
92+
pendulum_state_publisher_runner = Node(
93+
package='pendulum_state_publisher',
94+
executable='pendulum_state_publisher',
95+
condition=IfCondition(LaunchConfiguration('rviz'))
96+
)
97+
98+
ld = LaunchDescription()
99+
100+
ld.add_action(autostart_param)
101+
ld.add_action(priority_param)
102+
ld.add_action(cpu_affinity_param)
103+
ld.add_action(with_lock_memory_param)
104+
ld.add_action(lock_memory_size_param)
105+
ld.add_action(config_child_threads_param)
106+
ld.add_action(with_rviz_param)
107+
ld.add_action(robot_state_publisher_runner)
108+
#ld.add_action(pendulum_demo_runner)
109+
ld.add_action(rviz_runner)
110+
ld.add_action(pendulum_state_publisher_runner)
111+
112+
return ld

‎pendulum_controller/include/pendulum_controller/pendulum_controller_node.hpp‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,24 @@ class PendulumControllerNode : public rclcpp_lifecycle::LifecycleNode
5252
const std::string & node_name,
5353
const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
5454

55+
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<pendulum2_msgs::msg::JointCommand>>
56+
get_command_publisher() const
57+
{
58+
return command_pub_;
59+
}
60+
61+
std::shared_ptr<rclcpp::Subscription<pendulum2_msgs::msg::JointState>>
62+
get_state_subscription() const
63+
{
64+
return state_sub_;
65+
}
66+
67+
rclcpp::CallbackGroup::SharedPtr get_realtime_callback_group() const {
68+
return realtime_cb_group_;
69+
}
70+
71+
void realtime_loop();
72+
5573
private:
5674
/// \brief Create teleoperation subscription
5775
void create_teleoperation_subscription();
@@ -108,6 +126,8 @@ class PendulumControllerNode : public rclcpp_lifecycle::LifecycleNode
108126

109127
uint32_t num_missed_deadlines_pub_;
110128
uint32_t num_missed_deadlines_sub_;
129+
130+
rclcpp::CallbackGroup::SharedPtr realtime_cb_group_;
111131
};
112132
} // namespace pendulum_controller
113133
} // namespace pendulum

‎pendulum_controller/src/pendulum_controller_node.cpp‎

Lines changed: 35 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,8 @@ PendulumControllerNode::PendulumControllerNode(
4949
declare_parameter<std::vector<double>>("controller.feedback_matrix",
5050
{-10.0000, -51.5393, 356.8637, 154.4146}))),
5151
num_missed_deadlines_pub_{0U},
52-
num_missed_deadlines_sub_{0U}
52+
num_missed_deadlines_sub_{0U},
53+
realtime_cb_group_(create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false))
5354
{
5455
create_teleoperation_subscription();
5556
create_state_subscription();
@@ -74,6 +75,7 @@ void PendulumControllerNode::create_state_subscription()
7475
std::make_shared<MessagePoolMemoryStrategy<pendulum2_msgs::msg::JointState, 1>>();
7576

7677
rclcpp::SubscriptionOptions state_subscription_options;
78+
state_subscription_options.callback_group = realtime_cb_group_;
7779
state_subscription_options.event_callbacks.deadline_callback =
7880
[this](rclcpp::QOSDeadlineRequestedInfo &) -> void
7981
{
@@ -108,6 +110,7 @@ void PendulumControllerNode::create_state_subscription()
108110
void PendulumControllerNode::create_command_publisher()
109111
{
110112
rclcpp::PublisherOptions command_publisher_options;
113+
command_publisher_options.callback_group = realtime_cb_group_;
111114
command_publisher_options.event_callbacks.deadline_callback =
112115
[this](rclcpp::QOSDeadlineOfferedInfo &) -> void
113116
{
@@ -119,6 +122,37 @@ void PendulumControllerNode::create_command_publisher()
119122
command_publisher_options);
120123
}
121124

125+
void PendulumControllerNode::realtime_loop()
126+
{
127+
rclcpp::WaitSet wait_set;
128+
wait_set.add_subscription(state_sub_);
129+
130+
while (rclcpp::ok()) {
131+
// TODO(carlosvg): set timeout to double update period
132+
const auto wait_result = wait_set.wait(std::chrono::seconds(5));
133+
134+
if (wait_result.kind() == rclcpp::WaitResultKind::Ready) {
135+
// TODO(carlosvg): add timer
136+
if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) {
137+
pendulum2_msgs::msg::JointState msg;
138+
rclcpp::MessageInfo msg_info;
139+
if (state_sub_->take(msg, msg_info)) {
140+
// TODO(carlosvg): replace msg handling with direct function calls
141+
std::shared_ptr<void> message = std::make_shared<pendulum2_msgs::msg::JointState>(msg);
142+
state_sub_->handle_message(message, msg_info);
143+
}
144+
}
145+
} else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) {
146+
if (this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
147+
// TODO(carlosvg): transition to error, or increment count
148+
RCLCPP_INFO(get_logger(), "Wait-set timeout");
149+
}
150+
} else {
151+
RCLCPP_INFO(get_logger(), "Wait-set failed.");
152+
}
153+
}
154+
}
155+
122156
void PendulumControllerNode::log_controller_state()
123157
{
124158
const auto state = controller_.get_state();

‎pendulum_demo/CMakeLists.txt‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,13 @@ target_link_libraries(${PENDULUM_DEMO_EXE}
2222
pendulum_controller::pendulum_controller
2323
pendulum_driver::pendulum_driver)
2424

25+
set(PENDULUM_DEMO_WAITSET_EXE pendulum_demo_waitset)
26+
27+
add_executable(${PENDULUM_DEMO_WAITSET_EXE} "src/pendulum_demo_waitset.cpp")
28+
target_link_libraries(${PENDULUM_DEMO_WAITSET_EXE}
29+
pendulum_controller::pendulum_controller
30+
pendulum_driver::pendulum_driver)
31+
2532
if(BUILD_TESTING)
2633
find_package(ament_cmake_gtest REQUIRED)
2734
find_package(ament_lint_auto REQUIRED)
@@ -47,7 +54,7 @@ install(
4754
DESTINATION share/${PROJECT_NAME}
4855
)
4956

50-
install(TARGETS ${PENDULUM_DEMO_EXE}
57+
install(TARGETS ${PENDULUM_DEMO_EXE} ${PENDULUM_DEMO_WAITSET_EXE}
5158
LIBRARY DESTINATION lib
5259
ARCHIVE DESTINATION lib
5360
RUNTIME DESTINATION lib/${PROJECT_NAME}

‎pendulum_demo/src/pendulum_demo.cpp‎

Lines changed: 24 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,31 @@ int main(int argc, char * argv[])
6868
pendulum::utils::autostart(*driver_node_ptr);
6969
}
7070

71-
exec.spin();
71+
auto controller_rt_cb = controller_node_ptr->get_realtime_callback_group();
72+
auto driver_rt_cb = driver_node_ptr->get_realtime_callback_group();
73+
74+
auto controller_command_publisher = controller_node_ptr->get_command_publisher();
75+
auto controller_state_subscription = controller_node_ptr->get_state_subscription();
76+
auto driver_state_publisher = driver_node_ptr->get_state_publisher();
77+
auto driver_command_publisher= driver_node_ptr->get_command_subscription();
78+
auto driver_state_timer= driver_node_ptr->get_state_timer();
79+
80+
auto thread = std::thread([&exec]() {
81+
// spin will block until work comes in, execute work as it becomes available, and keep blocking.
82+
// It will only be interrupted by Ctrl-C.
83+
exec.spin();
84+
});
85+
// exec.spin();
86+
87+
// Create a static executor
88+
rclcpp::executors::StaticSingleThreadedExecutor exec_rt;
89+
exec_rt.add_callback_group(controller_rt_cb, controller_node_ptr->get_node_base_interface());
90+
exec_rt.add_callback_group(driver_rt_cb, driver_node_ptr->get_node_base_interface());
91+
92+
exec_rt.spin();
93+
7294
rclcpp::shutdown();
95+
thread.join();
7396
} catch (const std::exception & e) {
7497
RCLCPP_INFO(rclcpp::get_logger("pendulum_demo"), e.what());
7598
ret = 2;

‎pendulum_demo/src/pendulum_demo_waitset.cpp‎

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
// Copyright 2019 Carlos San Vicente
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <memory>
16+
#include <utility>
17+
#include <string>
18+
19+
#include "rclcpp/rclcpp.hpp"
20+
21+
#include "pendulum_driver/pendulum_driver_node.hpp"
22+
#include "pendulum_controller/pendulum_controller_node.hpp"
23+
#include "pendulum_utils/process_settings.hpp"
24+
#include "pendulum_utils/lifecycle_autostart.hpp"
25+
26+
int main(int argc, char * argv[])
27+
{
28+
pendulum::utils::ProcessSettings settings;
29+
if (!settings.init(argc, argv)) {
30+
return EXIT_FAILURE;
31+
}
32+
33+
int32_t ret = 0;
34+
35+
try {
36+
/*
37+
// configure process real-time settings
38+
if (settings.configure_child_threads) {
39+
// process child threads created by ROS nodes will inherit the settings
40+
settings.configure_process();
41+
}
42+
*/
43+
44+
rclcpp::init(argc, argv);
45+
46+
// Create a static executor
47+
rclcpp::executors::StaticSingleThreadedExecutor exec;
48+
49+
// Create pendulum controller node
50+
using pendulum::pendulum_controller::PendulumControllerNode;
51+
const auto controller_node_ptr =
52+
std::make_shared<PendulumControllerNode>("pendulum_controller");
53+
54+
exec.add_node(controller_node_ptr->get_node_base_interface());
55+
56+
// Create pendulum simulation
57+
using pendulum::pendulum_driver::PendulumDriverNode;
58+
const auto driver_node_ptr = std::make_shared<PendulumDriverNode>("pendulum_driver");
59+
60+
exec.add_node(driver_node_ptr->get_node_base_interface());
61+
62+
/*
63+
// configure process real-time settings
64+
if (!settings.configure_child_threads) {
65+
// process child threads created by ROS nodes will NOT inherit the settings
66+
settings.configure_process();
67+
}
68+
*/
69+
70+
// TODO(carlosvg): protect shared variables, using try-lock or std::atomic
71+
// TODO(carlosvg): configure threads individually
72+
auto executor_thread = std::thread(
73+
[&exec]() {
74+
exec.spin();
75+
});
76+
auto driver_realtime_thread = std::thread(
77+
[&driver_node_ptr, &settings]() {
78+
settings.configure_process();
79+
driver_node_ptr->realtime_loop();
80+
});
81+
auto controller_realtime_thread = std::thread(
82+
[&controller_node_ptr, &settings]() {
83+
settings.configure_process();
84+
controller_node_ptr->realtime_loop();
85+
});
86+
87+
if (settings.auto_start_nodes) {
88+
pendulum::utils::autostart(*controller_node_ptr);
89+
pendulum::utils::autostart(*driver_node_ptr);
90+
}
91+
92+
// TODO(carlosvg): add wait loop or experiment duration option
93+
const std::chrono::seconds EXPERIMENT_DURATION = std::chrono::seconds(10);
94+
std::this_thread::sleep_for(EXPERIMENT_DURATION);
95+
96+
rclcpp::shutdown();
97+
executor_thread.join();
98+
driver_realtime_thread.join();
99+
controller_realtime_thread.join();
100+
101+
} catch (const std::exception & e) {
102+
RCLCPP_INFO(rclcpp::get_logger("pendulum_demo"), e.what());
103+
ret = 2;
104+
} catch (...) {
105+
RCLCPP_INFO(
106+
rclcpp::get_logger("pendulum_demo"), "Unknown exception caught. "
107+
"Exiting...");
108+
ret = -1;
109+
}
110+
return ret;
111+
}

0 commit comments

Comments
 (0)