51 std::filesystem::path ros_controllers_yaml_path = package_path / MOVEIT_CONTROLLERS_YAML;
52 std::ifstream input_stream(ros_controllers_yaml_path);
53 if (!input_stream.good())
55 RCLCPP_WARN_STREAM(*
logger_,
"Does not exist " << ros_controllers_yaml_path);
64 YAML::Node doc = YAML::Load(input_stream);
65 YAML::Node controllers = doc[
"moveit_simple_controller_manager"];
67 std::vector<std::string> controller_names;
71 for (
const std::string& controller_name : controller_names)
73 const YAML::Node& cnode = controllers[controller_name];
74 if (!cnode.IsDefined())
76 RCLCPP_WARN_STREAM(*
logger_,
"Configuration for controller " << controller_name <<
" does not exist! Ignoring.");
86 YAML::Node trajectory_execution = doc[
"trajectory_execution"];
87 if (trajectory_execution.IsDefined() && trajectory_execution.IsMap())
89 for (
const auto& kv : trajectory_execution)
95 catch (YAML::ParserException& e)
97 RCLCPP_ERROR_STREAM(*
logger_, e.what());
108 control_setting.
name_ = name;
110 if (control_setting.
type_.empty())
112 RCLCPP_ERROR_STREAM(*
logger_,
"Couldn't parse type for controller " << name <<
" in moveit_controllers.yaml");
116 for (
const std::string parameter : {
"action_ns",
"default" })
118 if (controller_node[parameter].IsDefined())
120 control_setting.
parameters_[parameter] = controller_node[parameter].as<std::string>();
124 const YAML::Node& joints_node = controller_node[
"joints"];
126 if (joints_node.IsSequence())
128 control_setting.
joints_ = joints_node.as<std::vector<std::string>>();
130 else if (joints_node.IsDefined())
132 control_setting.
joints_.push_back(joints_node.as<std::string>());
134 if (control_setting.
joints_.empty())
136 RCLCPP_ERROR_STREAM(*
logger_,
"Couldn't parse joints for controller " << name <<
" in moveit_controllers.yaml");
149 emitter << YAML::Comment(
"MoveIt uses this configuration for controller management");
150 emitter << YAML::Newline;
151 emitter << YAML::BeginMap;
155 emitter << YAML::Key <<
"trajectory_execution" << YAML::Value;
156 emitter << YAML::BeginMap;
159 emitter << YAML::Key << kv.first << YAML::Value << kv.second;
161 emitter << YAML::EndMap;
164 emitter << YAML::Key <<
"moveit_controller_manager" << YAML::Value
165 <<
"moveit_simple_controller_manager/MoveItSimpleControllerManager";
166 emitter << YAML::Newline;
167 emitter << YAML::Newline;
169 emitter << YAML::Key <<
"moveit_simple_controller_manager" << YAML::Value;
170 emitter << YAML::BeginMap;
172 emitter << YAML::Key <<
"controller_names";
173 emitter << YAML::Value;
174 emitter << YAML::BeginSeq;
179 emitter << YAML::EndSeq;
180 emitter << YAML::Newline;
181 emitter << YAML::Newline;
185 emitter << YAML::Key << controller.name_;
186 emitter << YAML::Value;
187 emitter << YAML::BeginMap;
189 emitter << YAML::Key <<
"type" << YAML::Value << controller.type_;
192 emitter << YAML::Key <<
"joints";
193 emitter << YAML::Value;
194 emitter << YAML::BeginSeq;
197 for (
const std::string& joint : controller.joints_)
201 emitter << YAML::EndSeq;
203 for (
const auto& pair : controller.parameters_)
205 emitter << YAML::Key << pair.first;
206 emitter << YAML::Value << pair.second;
209 emitter << YAML::EndMap;
212 emitter << YAML::EndMap;
214 emitter << YAML::EndMap;