declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description='Full path to the ROS2 parameters file to use for all launched nodes')
from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node, SetParameter
from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python import get_package_share_directory
from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.actions import GroupAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch.substitutions import TextSubstitution from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace
defgenerate_launch_description():
# args that can be set from the command line or a default will be used background_r_launch_arg = DeclareLaunchArgument( "background_r", default_value=TextSubstitution(text="0") ) background_g_launch_arg = DeclareLaunchArgument( "background_g", default_value=TextSubstitution(text="255") ) background_b_launch_arg = DeclareLaunchArgument( "background_b", default_value=TextSubstitution(text="0") ) chatter_ns_launch_arg = DeclareLaunchArgument( "chatter_ns", default_value=TextSubstitution(text="my/chatter/ns") )
# include another launch file launch_include = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( get_package_share_directory('demo_nodes_cpp'), 'launch/topics/talker_listener.launch.py')) ) # include another launch file in the chatter_ns namespace launch_include_with_namespace = GroupAction( actions=[ # push-ros-namespace to set namespace of included nodes PushRosNamespace(LaunchConfiguration('chatter_ns')), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( get_package_share_directory('demo_nodes_cpp'), 'launch/topics/talker_listener.launch.py')) ), ] )
# start a turtlesim_node in the turtlesim1 namespace turtlesim_node = Node( package='turtlesim', namespace='turtlesim1', executable='turtlesim_node', name='sim' )
# start another turtlesim_node in the turtlesim2 namespace # and use args to set parameters turtlesim_node_with_parameters = Node( package='turtlesim', namespace='turtlesim2', executable='turtlesim_node', name='sim', parameters=[{ "background_r": LaunchConfiguration('background_r'), "background_g": LaunchConfiguration('background_g'), "background_b": LaunchConfiguration('background_b'), }] )
# perform remap so both turtles listen to the same command topic forward_turtlesim_commands_to_second_turtlesim_node = Node( package='turtlesim', executable='mimic', name='mimic', remappings=[ ('/input/pose', '/turtlesim1/turtle1/pose'), ('/output/cmd_vel', '/turtlesim2/turtle1/cmd_vel'), ] )
from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node
import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ThisLaunchFileDir
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from nav2_common.launch import RewrittenYaml
defgenerate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup')
# Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative # https://github.com/ros/geometry2/issues/32 # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
# Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file}
DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), description='Full path to map yaml file to load'),
DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack'),
DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description='Full path to the ROS2 parameters file to use'),
// Node that consumes messages. structConsumer : public rclcpp::Node { Consumer(const std::string & name, const std::string & input) : Node(name, rclcpp::NodeOptions().use_intra_process_comms(true)) { // Create a subscription on the input topic which prints on receipt of new messages. sub_ = this->create_subscription<std_msgs::msg::Int32>( input, 10, [](std_msgs::msg::Int32::UniquePtr msg) { printf( " Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data, reinterpret_cast<std::uintptr_t>(msg.get())); }); }
/** * A small convenience function for converting a thread ID to a string **/ std::string string_thread_id() { auto hashed = std::hash<std::thread::id>()(std::this_thread::get_id()); return std::to_string(hashed); }
/* For this example, we will be creating a publishing node like the one in minimal_publisher. * We will have a single subscriber node running 2 threads. Each thread loops at different speeds, and * just repeats what it sees from the publisher to the screen. */
classDualThreadedNode : public rclcpp::Node { public: DualThreadedNode() : Node("DualThreadedNode") { /* These define the callback groups * They don't really do much on their own, but they have to exist in order to * assign callbacks to them. They're also what the executor looks for when trying to run multiple threads */ callback_group_subscriber1_ = this->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); callback_group_subscriber2_ = this->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive);
// Each of these callback groups is basically a thread // Everything assigned to one of them gets bundled into the same thread auto sub1_opt = rclcpp::SubscriptionOptions(); sub1_opt.callback_group = callback_group_subscriber1_; auto sub2_opt = rclcpp::SubscriptionOptions(); sub2_opt.callback_group = callback_group_subscriber2_;
subscription1_ = this->create_subscription<std_msgs::msg::String>( "topic", rclcpp::QoS(10), // std::bind is sort of C++'s way of passing a function // If you're used to function-passing, skip these comments std::bind( &DualThreadedNode::subscriber1_cb, // First parameter is a reference to the function this, // What the function should be bound to std::placeholders::_1), // At this point we're not positive of all the // parameters being passed // So we just put a generic placeholder // into the binder // (since we know we need ONE parameter) sub1_opt); // This is where we set the callback group. // This subscription will run with callback group subscriber1
private: /** * Simple function for generating a timestamp * Used for somewhat ineffectually demonstrating that the multithreading doesn't cripple performace */ std::string timing_string() { rclcpp::Time time = this->now(); return std::to_string(time.nanoseconds()); }
/** * Every time the Publisher publishes something, all subscribers to the topic get poked * This function gets called when Subscriber1 is poked (due to the std::bind we used when defining it) */ voidsubscriber1_cb(const std_msgs::msg::String::SharedPtr msg) { auto message_received_at = timing_string();
// Extract current thread RCLCPP_INFO( this->get_logger(), "THREAD %s => Heard '%s' at %s", string_thread_id().c_str(), msg->data.c_str(), message_received_at.c_str()); }
/** * This function gets called when Subscriber2 is poked * Since it's running on a separate thread than Subscriber 1, it will run at (more-or-less) the same time! */ voidsubscriber2_cb(const std_msgs::msg::String::SharedPtr msg) { auto message_received_at = timing_string();
// You MUST use the MultiThreadedExecutor to use, well, multiple threads rclcpp::executors::MultiThreadedExecutor executor; auto pubnode = std::make_shared<PublisherNode>(); auto subnode = std::make_shared<DualThreadedNode>(); // This contains BOTH subscriber callbacks. // They will still run on different threads // One Node. Two callbacks. Two Threads executor.add_node(pubnode); executor.add_node(subnode); executor.spin(); rclcpp::shutdown(); return0; }
using std::chrono::seconds; using std::chrono::milliseconds; using std::chrono::nanoseconds; usingnamespace std::chrono_literals;
using examples_rclcpp_cbg_executor::PingNode; using examples_rclcpp_cbg_executor::PongNode; using examples_rclcpp_cbg_executor::configure_thread; using examples_rclcpp_cbg_executor::get_thread_time; using examples_rclcpp_cbg_executor::ThreadPriority;
/// The main function composes a Ping node and a Pong node in one OS process /// and runs the experiment. See README.md for an architecture diagram. intmain(int argc, char * argv[]) { rclcpp::init(argc, argv);
// Create two executors within this process. rclcpp::executors::SingleThreadedExecutor high_prio_executor; rclcpp::executors::SingleThreadedExecutor low_prio_executor;
// Create Ping node instance and add it to high-prio executor. auto ping_node = std::make_shared<PingNode>(); high_prio_executor.add_node(ping_node);
// Create Pong node instance and add it the one of its callback groups // to the high-prio executor and the other to the low-prio executor. auto pong_node = std::make_shared<PongNode>(); high_prio_executor.add_callback_group( pong_node->get_high_prio_callback_group(), pong_node->get_node_base_interface()); low_prio_executor.add_callback_group( pong_node->get_low_prio_callback_group(), pong_node->get_node_base_interface());
rclcpp::Logger logger = pong_node->get_logger();
// Create a thread for each of the two executors ... auto high_prio_thread = std::thread( [&]() { high_prio_executor.spin(); }); auto low_prio_thread = std::thread( [&]() { low_prio_executor.spin(); });
// ... and configure them accordinly as high and low prio and pin them to the // first CPU. Hence, the two executors compete about this computational resource. constint CPU_ZERO = 0; bool ret = configure_thread(high_prio_thread, ThreadPriority::HIGH, CPU_ZERO); if (!ret) { RCLCPP_WARN(logger, "Failed to configure high priority thread, are you root?"); } ret = configure_thread(low_prio_thread, ThreadPriority::LOW, CPU_ZERO); if (!ret) { RCLCPP_WARN(logger, "Failed to configure low priority thread, are you root?"); }
// Creating the threads immediately started them. // Therefore, get start CPU time of each thread now. nanoseconds high_prio_thread_begin = get_thread_time(high_prio_thread); nanoseconds low_prio_thread_begin = get_thread_time(low_prio_thread);
const std::chrono::seconds EXPERIMENT_DURATION = 10s; RCLCPP_INFO_STREAM( logger, "Running experiment from now on for " << EXPERIMENT_DURATION.count() << " seconds ..."); std::this_thread::sleep_for(EXPERIMENT_DURATION);
// Get end CPU time of each thread ... nanoseconds high_prio_thread_end = get_thread_time(high_prio_thread); nanoseconds low_prio_thread_end = get_thread_time(low_prio_thread);
// ... and stop the experiment. rclcpp::shutdown(); high_prio_thread.join(); low_prio_thread.join();
ping_node->print_statistics(EXPERIMENT_DURATION);
// Print CPU times. int64_t high_prio_thread_duration_ms = std::chrono::duration_cast<milliseconds>( high_prio_thread_end - high_prio_thread_begin).count(); int64_t low_prio_thread_duration_ms = std::chrono::duration_cast<milliseconds>( low_prio_thread_end - low_prio_thread_begin).count(); RCLCPP_INFO( logger, "High priority executor thread ran for %" PRId64 "ms.", high_prio_thread_duration_ms); RCLCPP_INFO( logger, "Low priority executor thread ran for %" PRId64 "ms.", low_prio_thread_duration_ms);
