diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 91e84dbf..ea201d8c 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -180,11 +180,38 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element if (sdf->HasElement("parameters")) { impl_->param_file_ = sdf->GetElement("parameters")->Get(); + RCLCPP_INFO( + impl_->model_nh_->get_logger(), "Loading parameter file %s\n", impl_->param_file_.c_str()); } else { RCLCPP_ERROR( impl_->model_nh_->get_logger(), "No parameter file provided. Configuration might be wrong"); } + // There's currently no direct way to set parameters to the plugin's node + // So we have to parse the plugin file manually and set it to the node's context. + auto rcl_context = impl_->model_nh_->get_node_base_interface()->get_context()->get_rcl_context(); + std::vector arguments = {"--ros-args", "--params-file", impl_->param_file_.c_str()}; + std::vector argv; + for (const auto & arg : arguments) { + argv.push_back(reinterpret_cast(arg.data())); + } + rcl_arguments_t rcl_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t rcl_ret = rcl_parse_arguments( + static_cast(argv.size()), + argv.data(), rcl_get_default_allocator(), &rcl_args); + rcl_context->global_arguments = rcl_args; + if (rcl_ret != RCL_RET_OK) { + RCLCPP_ERROR(impl_->model_nh_->get_logger(), "parser error %s\n", rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (rcl_arguments_get_param_files_count(&rcl_args) < 1) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "failed to parse yaml file: '%s'\n", + impl_->param_file_.c_str()); + return; + } + // Get the Gazebo simulation period rclcpp::Duration gazebo_period( std::chrono::duration_cast( @@ -214,215 +241,76 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element new pluginlib::ClassLoader( "gazebo_ros2_control", "gazebo_ros2_control::GazeboSystemInterface")); - - for (unsigned int i = 0; i < control_hardware.size(); i++) { - std::string robot_hw_sim_type_str_ = control_hardware[i].hardware_class_type; - auto gazeboSystem = std::unique_ptr( - impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); - - rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast(impl_->model_nh_); - if (!gazeboSystem->initSim( - node_ros2, - impl_->parent_model_, - control_hardware[i], - sdf)) - { - RCLCPP_FATAL( - impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface"); - return; - } - - resource_manager_->import_component(std::move(gazeboSystem)); - } - - impl_->executor_ = std::make_shared(); - - auto spin = [this]() - { - while (rclcpp::ok()) { - impl_->executor_->spin_once(); - } - }; - impl_->thread_executor_spin_ = std::thread(spin); - - // Create the controller manager - RCLCPP_INFO(impl_->model_nh_->get_logger(), "Loading controller_manager"); - impl_->controller_manager_.reset( - new controller_manager::ControllerManager( - std::move(resource_manager_), - impl_->executor_, - "controller_manager")); - impl_->executor_->add_node(impl_->controller_manager_); - - // TODO(anyone): Coded example here. should disable when spawn functionality of - // controller manager is up - auto load_params_from_yaml_node = [](rclcpp::Node::SharedPtr node, - YAML::Node & yaml_node, const std::string & prefix) - { - std::function, const std::string &)> - feed_yaml_to_node_rec = - [&](YAML::Node yaml_node, const std::string & key, - std::shared_ptr node, const std::string & prefix) - { - if (node->get_name() != prefix) { - return; - } - static constexpr char separator = '.'; - if (yaml_node.Type() == YAML::NodeType::Scalar) { - std::string val_str = yaml_node.as(); - - // TODO(ddengster): Do stricter typing for set_parameter value types. - // (ie. Numbers should be converted to int/double/etc instead of strings) - if (!node->has_parameter(key)) { - node->declare_parameter(key); - } - - auto is_number = [](const std::string & str) -> bool { - return str.find_first_not_of("0123456789.-") == std::string::npos; - // @note: bugs with .05 or 15. - }; - - if (is_number(val_str)) { - std::stringstream ss(val_str); - double v = 0.0; - ss >> v; - node->set_parameter(rclcpp::Parameter(key, v)); - } else { - node->set_parameter(rclcpp::Parameter(key, val_str)); - } - return; - } else if (yaml_node.Type() == YAML::NodeType::Map) { - for (auto yaml_node_it : yaml_node) { - std::string newkey = yaml_node_it.first.as(); - if (newkey == prefix || newkey == "ros__parameters") { - newkey = ""; - } else if (!key.empty()) { - newkey = key + separator + newkey; - } - feed_yaml_to_node_rec(yaml_node_it.second, newkey, node, prefix); - } - } else if (yaml_node.Type() == YAML::NodeType::Sequence) { - auto it = yaml_node.begin(); - if (yaml_node.size()) { - if (it->IsScalar()) { - // submit as array of parameters - std::vector val; - for (auto yaml_node_it : yaml_node) { - std::string name = yaml_node_it.as(); - val.push_back(name); - } - if (!node->has_parameter(key)) { - node->declare_parameter(key); - } - node->set_parameter({rclcpp::Parameter(key, val)}); - } else { - size_t index = 0; - for (auto yaml_node_it : yaml_node) { - std::string newkey = std::to_string((index++)); - if (!key.empty()) { - newkey = key + separator + newkey; - } - feed_yaml_to_node_rec(yaml_node_it, newkey, node, prefix); - } - } - } - } - }; - if (node->get_name() != prefix) { - return; - } - feed_yaml_to_node_rec(yaml_node, prefix, node, prefix); - }; - auto load_params_from_yaml = [&](rclcpp::Node::SharedPtr node, - const std::string & yaml_config_file, const std::string & prefix) - { - if (yaml_config_file.empty()) { - throw std::runtime_error("yaml config file path is empty"); - } - YAML::Node root_node = YAML::LoadFile(yaml_config_file); - for (auto yaml : root_node) { - auto nodename = yaml.first.as(); - RCLCPP_DEBUG(impl_->model_nh_->get_logger(), "nodename: %s", nodename.c_str()); - if (nodename == prefix) { - load_params_from_yaml_node(node, yaml.second, prefix); - } - } - }; - - // Start controller, will take effect at the end of the update function - std::vector start_controllers = {}; - std::vector stop_controllers = {}; - - YAML::Node root_node = YAML::LoadFile(impl_->param_file_); - for (auto yaml : root_node) { - auto controller_manager_node_name = yaml.first.as(); - if (controller_manager_node_name == "controller_manager") { - for (auto yaml_node_it : yaml.second) { // ros__parameters - for (auto controller_manager_params_it : yaml_node_it.second) { - auto controller_name = controller_manager_params_it.first.as(); - - if (controller_name == "update_rate") { - float udpate_rate = controller_manager_params_it.second.as(); - // Decide the plugin control period - impl_->control_period_ = rclcpp::Duration( - std::chrono::duration_cast( - std::chrono::duration(1.0 / udpate_rate))); - - // Check the period against the simulation period - if (impl_->control_period_ < gazebo_period) { - RCLCPP_ERROR_STREAM( - impl_->model_nh_->get_logger(), - "Desired controller update period (" << impl_->control_period_.seconds() << - " s) is faster than the gazebo simulation period (" << - gazebo_period.seconds() << " s)."); - } else if (impl_->control_period_ > gazebo_period) { - RCLCPP_WARN_STREAM( - impl_->model_nh_->get_logger(), - " Desired controller update period (" << impl_->control_period_.seconds() << - " s) is slower than the gazebo simulation period (" << - gazebo_period.seconds() << " s)."); - } - } else { - for (auto controller_type_it : controller_manager_params_it.second) { - auto controller_type = controller_type_it.second.as(); - auto controller = impl_->controller_manager_->load_controller( - controller_name, - controller_type); - impl_->controllers_.push_back(controller); - load_params_from_yaml( - controller->get_node(), - impl_->param_file_, - controller_name); - controller->configure(); - start_controllers.push_back(controller_name); - } - } - } - } - break; - } - } - - auto switch_future = std::async( - std::launch::async, - &controller_manager::ControllerManager::switch_controller, - impl_->controller_manager_, - start_controllers, stop_controllers, - 2, true, rclcpp::Duration(0, 0)); // STRICT_: 2 - while (std::future_status::timeout == switch_future.wait_for(std::chrono::milliseconds(100))) { - impl_->controller_manager_->update(); - } - controller_interface::return_type result = switch_future.get(); - if (result != controller_interface::return_type::SUCCESS) { - RCLCPP_ERROR(impl_->model_nh_->get_logger(), "Error initializing the joint_state_controller"); - } } catch (pluginlib::LibraryLoadException & ex) { RCLCPP_ERROR( impl_->model_nh_->get_logger(), "Failed to create robot simulation interface loader: %s ", ex.what()); } + for (unsigned int i = 0; i < control_hardware.size(); i++) { + std::string robot_hw_sim_type_str_ = control_hardware[i].hardware_class_type; + auto gazeboSystem = std::unique_ptr( + impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); + + rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast(impl_->model_nh_); + if (!gazeboSystem->initSim( + node_ros2, + impl_->parent_model_, + control_hardware[i], + sdf)) + { + RCLCPP_FATAL( + impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface"); + return; + } + + resource_manager_->import_component(std::move(gazeboSystem)); + } + + impl_->executor_ = std::make_shared(); + + // Create the controller manager + RCLCPP_INFO(impl_->model_nh_->get_logger(), "Loading controller_manager"); + impl_->controller_manager_.reset( + new controller_manager::ControllerManager( + std::move(resource_manager_), + impl_->executor_, + "controller_manager")); + impl_->executor_->add_node(impl_->controller_manager_); + + if (!impl_->controller_manager_->has_parameter("update_rate")) { + RCLCPP_ERROR_STREAM( + impl_->model_nh_->get_logger(), "controller manager doesn't have an update_rate parameter"); + return; + } + + auto cm_update_rate = impl_->controller_manager_->get_parameter("update_rate").as_int(); + impl_->control_period_ = rclcpp::Duration( + std::chrono::duration_cast( + std::chrono::duration(1.0 / static_cast(cm_update_rate)))); + // Check the period against the simulation period + if (impl_->control_period_ < gazebo_period) { + RCLCPP_ERROR_STREAM( + impl_->model_nh_->get_logger(), + "Desired controller update period (" << impl_->control_period_.seconds() << + " s) is faster than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } else if (impl_->control_period_ > gazebo_period) { + RCLCPP_WARN_STREAM( + impl_->model_nh_->get_logger(), + " Desired controller update period (" << impl_->control_period_.seconds() << + " s) is slower than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } + + auto spin = [this]() + { + while (rclcpp::ok()) { + impl_->executor_->spin_once(); + } + }; + impl_->thread_executor_spin_ = std::thread(spin); + // Listen to the update event. This event is broadcast every simulation iteration. impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( diff --git a/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py b/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py index 89df1466..de6e3e15 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py @@ -18,7 +18,8 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -55,7 +56,29 @@ def generate_launch_description(): '-entity', 'cartpole'], output='screen') + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + output='screen' + ) + + load_effort_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'effort_controllers'], + output='screen' + ) + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_effort_controller], + ) + ), gazebo, node_robot_state_publisher, spawn_entity, diff --git a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py index b6778309..160d20b5 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_position.launch.py @@ -18,7 +18,8 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -55,7 +56,29 @@ def generate_launch_description(): '-entity', 'cartpole'], output='screen') + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + output='screen' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'], + output='screen' + ) + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_joint_trajectory_controller], + ) + ), gazebo, node_robot_state_publisher, spawn_entity, diff --git a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py index b3578571..19c24584 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -18,7 +18,8 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -54,7 +55,29 @@ def generate_launch_description(): '-entity', 'cartpole'], output='screen') + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + output='screen' + ) + + load_velocity_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'velocity_controller'], + output='screen' + ) + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_velocity_controller], + ) + ), gazebo, node_robot_state_publisher, spawn_entity,