101 void initialize(
const rclcpp::Node::SharedPtr& node)
override
104 if (!
node_->has_parameter(makeParameterName(PARAM_BASE_NAME,
"controller_names")))
106 RCLCPP_ERROR_STREAM(getLogger(),
"No controller_names specified.");
109 rclcpp::Parameter controller_names_param;
110 node_->get_parameter(makeParameterName(PARAM_BASE_NAME,
"controller_names"), controller_names_param);
111 if (controller_names_param.get_type() != rclcpp::ParameterType::PARAMETER_STRING_ARRAY)
113 RCLCPP_ERROR(getLogger(),
"Parameter controller_names should be specified as a string array");
116 std::vector<std::string> controller_names = controller_names_param.as_string_array();
118 for (
const std::string& controller_name : controller_names)
122 std::string action_ns;
123 const std::string& action_ns_param = makeParameterName(PARAM_BASE_NAME, controller_name,
"action_ns");
124 if (!
node_->get_parameter(action_ns_param, action_ns))
126 RCLCPP_ERROR_STREAM(getLogger(),
"No action namespace specified for controller `"
127 << controller_name <<
"` through parameter `" << action_ns_param <<
'`');
132 if (!
node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name,
"type"), type))
134 RCLCPP_ERROR_STREAM(getLogger(),
"No type specified for controller " << controller_name);
138 std::vector<std::string> controller_joints;
139 if (!
node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name,
"joints"), controller_joints) ||
140 controller_joints.empty())
142 RCLCPP_ERROR_STREAM(getLogger(),
"No joints specified for controller " << controller_name);
146 ActionBasedControllerHandleBasePtr new_handle;
147 if (type ==
"GripperCommand")
150 const std::string& max_effort_param = makeParameterName(PARAM_BASE_NAME, controller_name,
"max_effort");
151 if (!node->get_parameter(max_effort_param, max_effort))
153 RCLCPP_INFO_STREAM(getLogger(),
"Max effort set to 0.0");
157 new_handle = std::make_shared<GripperControllerHandle>(
node_, controller_name, action_ns, max_effort);
158 bool parallel_gripper =
false;
159 if (
node_->get_parameter(makeParameterName(PARAM_BASE_NAME,
"parallel"), parallel_gripper) && parallel_gripper)
161 if (controller_joints.size() != 2)
163 RCLCPP_ERROR_STREAM(getLogger(),
"Parallel Gripper requires exactly two joints, "
164 << controller_joints.size() <<
" are specified");
168 ->setParallelJawGripper(controller_joints[0], controller_joints[1]);
172 std::string command_joint;
173 if (!
node_->get_parameter(makeParameterName(PARAM_BASE_NAME,
"command_joint"), command_joint))
174 command_joint = controller_joints[0];
180 node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME,
"allow_failure"), allow_failure,
false);
183 RCLCPP_INFO_STREAM(getLogger(),
"Added GripperCommand controller for " << controller_name);
186 else if (type ==
"FollowJointTrajectory")
188 new_handle = std::make_shared<FollowJointTrajectoryControllerHandle>(
node_, controller_name, action_ns);
189 RCLCPP_INFO_STREAM(getLogger(),
"Added FollowJointTrajectory controller for " << controller_name);
194 RCLCPP_ERROR_STREAM(getLogger(),
"Unknown controller type: " << type);
204 node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name,
"default"), state.
default_,
false);
210 for (
const std::string& controller_joint : controller_joints)
211 new_handle->addJoint(controller_joint);
215 RCLCPP_ERROR_STREAM(getLogger(),
"Caught unknown exception while parsing controller information");