54 std::filesystem::path ros_controllers_yaml_path = package_path / CONTROLLERS_YAML;
55 std::ifstream input_stream(ros_controllers_yaml_path);
56 if (!input_stream.good())
58 RCLCPP_WARN_STREAM(*
logger_,
"Does not exist " << ros_controllers_yaml_path);
65 YAML::Node config = YAML::Load(input_stream);
68 const YAML::Node& cm_node = config[
"controller_manager"][
"ros__parameters"];
69 if (!cm_node.IsDefined())
73 for (
const auto& it : cm_node)
75 if (!it.second.IsMap())
78 const auto& type_node = it.second[
"type"];
79 if (!type_node.IsDefined())
85 controller.
name_ = it.first.as<std::string>();
86 controller.
type_ = type_node.as<std::string>();
88 if (controller.
type_ !=
"joint_state_broadcaster/JointStateBroadcaster")
97 const YAML::Node& controller_node = config[controller.name_][
"ros__parameters"];
98 if (!controller_node.IsDefined() || !controller_node.IsMap())
102 auto jnode = controller_node[
"joints"];
103 if (jnode.IsDefined() && jnode.IsSequence())
107 for (std::size_t i = 0; i < jnode.size(); i++)
109 controller.joints_.push_back(jnode[i].as<std::string>());
113 if (controller.joints_.empty())
115 std::string one_joint;
117 if (!one_joint.empty())
119 controller.joints_.push_back(one_joint);
124 catch (YAML::ParserException& e)
126 RCLCPP_ERROR_STREAM(*
logger_, e.what());
140 emitter << YAML::Comment(
"This config file is used by ros2_control");
141 emitter << YAML::BeginMap;
144 emitter << YAML::Key <<
"controller_manager";
145 emitter << YAML::Value;
146 emitter << YAML::BeginMap;
148 emitter << YAML::Key <<
"ros__parameters";
149 emitter << YAML::Value;
150 emitter << YAML::BeginMap;
152 emitter << YAML::Key <<
"update_rate" << YAML::Value << 100;
153 emitter << YAML::Comment(
"Hz");
154 emitter << YAML::Newline;
155 emitter << YAML::Newline;
160 emitter << YAML::Key << ci.name_;
161 emitter << YAML::Value;
162 emitter << YAML::BeginMap;
164 emitter << YAML::Key <<
"type" << YAML::Value << ci.type_;
166 emitter << YAML::Newline << YAML::Newline;
167 emitter << YAML::EndMap;
170 emitter << YAML::Key <<
"joint_state_broadcaster";
171 emitter << YAML::Value;
172 emitter << YAML::BeginMap;
174 emitter << YAML::Key <<
"type" << YAML::Value <<
"joint_state_broadcaster/JointStateBroadcaster";
176 emitter << YAML::EndMap;
178 emitter << YAML::EndMap;
180 emitter << YAML::EndMap;
181 emitter << YAML::Newline;
182 emitter << YAML::Newline;
187 emitter << YAML::Key << ci.name_;
188 emitter << YAML::Value;
189 emitter << YAML::BeginMap;
191 emitter << YAML::Key <<
"ros__parameters";
192 emitter << YAML::Value;
193 emitter << YAML::BeginMap;
195 if (ci.type_ !=
"position_controllers/GripperActionController")
197 emitter << YAML::Key <<
"joints" << YAML::Value << ci.joints_;
201 emitter << YAML::Key <<
"joint" << YAML::Value << ci.joints_[0];
204 if (ci.type_ ==
"joint_trajectory_controller/JointTrajectoryController")
207 emitter << YAML::Key <<
"command_interfaces" << YAML::Value << interfaces.
command_interfaces;
208 emitter << YAML::Key <<
"state_interfaces" << YAML::Value << interfaces.
state_interfaces;
209 emitter << YAML::Key <<
"allow_nonzero_velocity_at_trajectory_end" << YAML::Value <<
true;
212 emitter << YAML::EndMap;
214 emitter << YAML::EndMap;
216 emitter << YAML::EndMap;