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.

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

Fix MoveIt Servo compilation on macOS #555

Merged
merged 13 commits into from
Jul 27, 2021
33 changes: 28 additions & 5 deletions moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,6 @@ constexpr size_t ROS_QUEUE_SIZE = 2;

using SetParameterCallbackType = std::function<rcl_interfaces::msg::SetParametersResult(const rclcpp::Parameter&)>;

// Helper template for declaring and getting ros param
template <typename T>
void declareOrGetParam(T& output_value, const std::string& param_name, const rclcpp::Node::SharedPtr& node,
const rclcpp::Logger& logger, const T default_value = T{});

// ROS params to be read. See the yaml file in /config for a description of each.
struct ServoParameters
{
Expand Down Expand Up @@ -155,4 +150,32 @@ struct ServoParameters
rcl_interfaces::msg::SetParametersResult setParametersCallback(std::vector<rclcpp::Parameter> parameters);
};

// Helper template for declaring and getting ros param
// Must be declared here to ensure template class is built for required templates when included.
// (The CPP file can't just be included because not everything there is templated, so you'd get duplicate symbols)
template <typename T>
void declareOrGetParam(T& output_value, const std::string& param_name, const rclcpp::Node::SharedPtr& node,
const rclcpp::Logger& logger, const T default_value = T{})
{
try
{
if (node->has_parameter(param_name))
{
node->get_parameter<T>(param_name, output_value);
}
else
{
output_value = node->declare_parameter<T>(param_name, default_value);
}
}
catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
{
RCLCPP_WARN_STREAM(logger, "InvalidParameterTypeException(" << param_name << "): " << e.what());
RCLCPP_ERROR_STREAM(logger, "Error getting parameter \'" << param_name << "\', check parameter type in YAML file");
throw e;
}

RCLCPP_INFO_STREAM(logger, "Found parameter - " << param_name << ": " << output_value);
}

} // namespace moveit_servo
25 changes: 0 additions & 25 deletions moveit_ros/moveit_servo/src/servo_parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,31 +47,6 @@ using namespace std::placeholders; // for _1, _2 etc.

namespace moveit_servo
{
template <typename T>
void declareOrGetParam(T& output_value, const std::string& param_name, const rclcpp::Node::SharedPtr& node,
const rclcpp::Logger& logger, const T default_value)
{
try
{
if (node->has_parameter(param_name))
{
node->get_parameter<T>(param_name, output_value);
}
else
{
output_value = node->declare_parameter<T>(param_name, default_value);
}
}
catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
{
RCLCPP_WARN_STREAM(logger, "InvalidParameterTypeException(" << param_name << "): " << e.what());
RCLCPP_ERROR_STREAM(logger, "Error getting parameter \'" << param_name << "\', check parameter type in YAML file");
throw e;
}

RCLCPP_INFO_STREAM(logger, "Found parameter - " << param_name << ": " << output_value);
}

rcl_interfaces::msg::SetParametersResult
ServoParameters::setParametersCallback(std::vector<rclcpp::Parameter> parameters)
{
Expand Down