Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

add ros parameters file to node context #60

Merged
merged 7 commits into from
Mar 3, 2021
Merged

add ros parameters file to node context #60

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
944432c
use upstream yaml parser
Karsten1987 Feb 17, 2021
3fc2a5f
uncrustify
Karsten1987 Feb 17, 2021
1047c22
add conditional
Karsten1987 Feb 18, 2021
7a8a62b
manually add parameters to node context
Karsten1987 Feb 24, 2021
59db1df
linters
Karsten1987 Feb 24, 2021
b46c5c8
Load controllers in the example launch files
ahcorde Mar 2, 2021
b1f5184
Merge pull request #64 from ros-simulation/fix_params_examples
Karsten1987 Mar 2, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions gazebo_ros2_control/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ find_package(gazebo_ros REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rcl_yaml_param_parser REQUIRED)
find_package(urdf REQUIRED)
find_package(yaml_cpp_vendor REQUIRED)

Expand Down Expand Up @@ -37,6 +38,7 @@ ament_target_dependencies(${PROJECT_NAME}
hardware_interface
pluginlib
rclcpp
rcl_yaml_param_parser
urdf
yaml_cpp_vendor
)
Expand All @@ -49,6 +51,7 @@ ament_target_dependencies(gazebo_hardware_plugins
gazebo_dev
hardware_interface
rclcpp
rcl_yaml_param_parser
)

## Install
Expand Down
1 change: 1 addition & 0 deletions gazebo_ros2_control/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rcl_yaml_param_parser</depend>
<depend>std_msgs</depend>
<depend>urdf</depend>
<depend>yaml_cpp_vendor</depend>
Expand Down
264 changes: 62 additions & 202 deletions gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@
#include "pluginlib/class_loader.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/parameter_map.hpp"
#include "rcl_yaml_param_parser/parser.h"

#include "hardware_interface/resource_manager.hpp"
#include "hardware_interface/component_parser.hpp"
Expand Down Expand Up @@ -180,6 +182,8 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element

if (sdf->HasElement("parameters")) {
impl_->param_file_ = sdf->GetElement("parameters")->Get<std::string>();
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");
Expand Down Expand Up @@ -214,214 +218,70 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
new pluginlib::ClassLoader<gazebo_ros2_control::GazeboSystemInterface>(
"gazebo_ros2_control",
"gazebo_ros2_control::GazeboSystemInterface"));
} 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<gazebo_ros2_control::GazeboSystemInterface>(
impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));

rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast<rclcpp::Node>(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));
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<gazebo_ros2_control::GazeboSystemInterface>(
impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));

rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast<rclcpp::Node>(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;
}

impl_->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();

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<void(YAML::Node, const std::string &,
std::shared_ptr<rclcpp::Node>, const std::string &)>
feed_yaml_to_node_rec =
[&](YAML::Node yaml_node, const std::string & key,
std::shared_ptr<rclcpp::Node> 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<std::string>();

// 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<std::string>();
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<std::string> val;
for (auto yaml_node_it : yaml_node) {
std::string name = yaml_node_it.as<std::string>();
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<std::string>();
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<std::string> start_controllers = {};
std::vector<std::string> stop_controllers = {};

YAML::Node root_node = YAML::LoadFile(impl_->param_file_);
for (auto yaml : root_node) {
auto controller_manager_node_name = yaml.first.as<std::string>();
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<std::string>();

if (controller_name == "update_rate") {
float udpate_rate = controller_manager_params_it.second.as<float>();
// Decide the plugin control period
impl_->control_period_ = rclcpp::Duration(
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(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<std::string>();
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;
}
}
resource_manager_->import_component(std::move(gazeboSystem));
}

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) {
impl_->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();

// 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_);

// parse the parameters file
rcl_params_t * params_struct = rcl_yaml_node_struct_init(rcutils_get_default_allocator());
auto ret = rcl_parse_yaml_file(impl_->param_file_.c_str(), params_struct);
if (!ret) {
RCLCPP_ERROR(
impl_->model_nh_->get_logger(), "Failed to create robot simulation interface loader: %s ",
ex.what());
impl_->model_nh_->get_logger(), "Unable to parse yaml config in file %s",
impl_->param_file_.c_str());
return;
}
auto parameter_map = rclcpp::parameter_map_from(params_struct);
rcl_yaml_node_struct_fini(params_struct);
auto it = parameter_map.find("/controller_manager");
if (it != parameter_map.end()) {
impl_->controller_manager_->set_parameters(it->second);
} else {
RCLCPP_INFO(
impl_->model_nh_->get_logger(),
"No parameters for /controller_manager found, setting defaults");
}
Karsten1987 marked this conversation as resolved.
Show resolved Hide resolved

Karsten1987 marked this conversation as resolved.
Show resolved Hide resolved
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_ =
Expand Down