Skip to content

Commit

Permalink
Switched to generate_parameter_library
Browse files Browse the repository at this point in the history
  • Loading branch information
@guihomework
guihomework committed Mar 7, 2023
1 parent c046598 commit e6d3e27
Show file tree
Hide file tree
Showing 10 changed files with 155 additions and 724 deletions.
33 changes: 11 additions & 22 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,29 +42,13 @@ target_include_directories(control_toolbox PUBLIC
ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY")

########################
# Parameter handler
########################
add_library(parameter_handler SHARED
src/parameter_handler.cpp
)

target_include_directories(parameter_handler PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/control_toolbox>
)
ament_target_dependencies(parameter_handler
rcl_interfaces
rclcpp
rcutils
)

########################
# Control filters
########################
find_package(filters REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(generate_parameter_library REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
Expand All @@ -73,6 +57,7 @@ set(CONTROL_FILTERS_INCLUDE_DEPENDS
filters
geometry_msgs
pluginlib
generate_parameter_library
rclcpp
tf2_geometry_msgs
tf2
Expand All @@ -86,7 +71,11 @@ target_include_directories(gravity_compensation PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(gravity_compensation parameter_handler)
generate_parameter_library(
gravity_compensation_filter_parameters
src/control_filters/gravity_compensation_filter_parameters.yaml
)
target_link_libraries(gravity_compensation gravity_compensation_filter_parameters)
ament_target_dependencies(gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS})

##########
Expand All @@ -109,7 +98,7 @@ if(BUILD_TESTING)

## Control Filters
ament_add_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp)
target_link_libraries(test_gravity_compensation gravity_compensation)
target_link_libraries(test_gravity_compensation gravity_compensation gravity_compensation_filter_parameters)
ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS})

endif()
Expand All @@ -119,13 +108,13 @@ install(
DIRECTORY include/
DESTINATION include/control_toolbox
)
install(TARGETS control_toolbox parameter_handler
install(TARGETS control_toolbox
EXPORT export_control_toolbox
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

install(TARGETS gravity_compensation
install(TARGETS gravity_compensation gravity_compensation_filter_parameters
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
Expand All @@ -142,7 +131,7 @@ ament_export_include_directories(
ament_export_libraries(
control_toolbox
gravity_compensation
parameter_handler
gravity_compensation_filter_parameters
)

ament_package()
4 changes: 2 additions & 2 deletions control_filters.xml
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<class_libraries>
<library path="gravity_compensation">
<class name="control_filters/GravityCompensationWrench"
type="control_filters::GravityCompensation<geometry_msgs::msg::WrenchStamped>"
base_class_type="filters::FilterBase<geometry_msgs::msg::WrenchStamped>">
type="control_filters::GravityCompensation&lt;geometry_msgs::msg::WrenchStamped&gt;"
base_class_type="filters::FilterBase&lt;geometry_msgs::msg::WrenchStamped&gt;">
<description>
This is a gravity compensation filter working with geometry_msgs::WrenchStamped.
It subtracts the influence of a force caused by the gravitational force from force
Expand Down
116 changes: 36 additions & 80 deletions include/control_filters/gravity_compensation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,80 +19,16 @@
#include <string>
#include <vector>

#include "control_toolbox/parameter_handler.hpp"
#include "gravity_compensation_filter_parameters.hpp"
#include "filters/filter_base.hpp"
#include "geometry_msgs/msg/vector3_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

namespace control_filters
{
class GravityCompensationParameters : public control_toolbox::ParameterHandler
{
public:
explicit GravityCompensationParameters(const std::string & params_prefix)
: control_toolbox::ParameterHandler(params_prefix, 0, 0, 4, 3)
{
add_string_parameter("world_frame", false);
add_string_parameter("sensor_frame", false);
add_string_parameter("force_frame", false);

add_double_parameter("CoG.x", true);
add_double_parameter("CoG.y", true);
add_double_parameter("CoG.z", true);
add_double_parameter("force", true);
}

bool check_if_parameters_are_valid() override
{
bool ret = true;

// Check if any string parameter is empty
ret = !empty_parameter_in_list(string_parameters_);

for (size_t i = 0; i < 4; ++i)
{
if (std::isnan(double_parameters_[i].second))
{
RCUTILS_LOG_ERROR_NAMED(
logger_name_.c_str(), "Parameter '%s' has to be set",
double_parameters_[i].first.name.c_str());
ret = false;
}
}

return ret;
}

void update_storage() override
{
world_frame_ = string_parameters_[0].second;
RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "World frame: %s", world_frame_.c_str());
sensor_frame_ = string_parameters_[0].second;
RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Sensor frame: %s", sensor_frame_.c_str());
force_frame_ = string_parameters_[0].second;
RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Force frame: %s", force_frame_.c_str());

cog_.vector.x = double_parameters_[0].second;
RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG X is %e", cog_.vector.x);
cog_.vector.y = double_parameters_[1].second;
RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG Y is %e", cog_.vector.y);
cog_.vector.z = double_parameters_[2].second;
RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "CoG Z is %e", cog_.vector.z);

force_z_ = double_parameters_[3].second;
RCUTILS_LOG_INFO_NAMED(logger_name_.c_str(), "Force is %e", force_z_);
}

// Frames for Transformation of Gravity / CoG Vector
std::string world_frame_;
std::string sensor_frame_;
std::string force_frame_;

// Storage for Calibration Values
geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame)
double force_z_; // Gravitational Force
};

template <typename T>
class GravityCompensation : public filters::FilterBase<T>
Expand All @@ -113,13 +49,30 @@ class GravityCompensation : public filters::FilterBase<T>
*/
bool update(const T & data_in, T & data_out) override;

protected:
void compute_internal_params()
{
cog_.vector.x = parameters_.CoG.pos[0];
cog_.vector.y = parameters_.CoG.pos[1];
cog_.vector.z = parameters_.CoG.pos[2];
// for now always consider the force is along z
force_z_ = parameters_.CoG.force[2];
};

private:
rclcpp::Clock::SharedPtr clock_;
std::shared_ptr<rclcpp::Logger> logger_;
std::unique_ptr<GravityCompensationParameters> parameters_;
std::shared_ptr<gravity_compensation_filter::ParamListener> parameter_handler_;
gravity_compensation_filter::Params parameters_;

// Callback for updating dynamic parameters
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_;
// Frames for Transformation of Gravity / CoG Vector
std::string world_frame_;
std::string sensor_frame_;
std::string force_frame_;

// Storage for Calibration Values
geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt Sensor Frame)
double force_z_; // Gravitational Force

// Filter objects
std::unique_ptr<tf2_ros::Buffer> p_tf_Buffer_;
Expand All @@ -146,22 +99,25 @@ bool GravityCompensation<T>::configure()

logger_.reset(
new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_)));
parameters_.reset(new GravityCompensationParameters(this->param_prefix_));

parameters_->initialize(this->params_interface_, logger_->get_name());

parameters_->declare_parameters();

if (!parameters_->get_parameters())
// Initialize the parameters
try
{
parameter_handler_ =
std::make_shared<gravity_compensation_filter::ParamListener>(this->params_interface_);
}
catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what());
parameter_handler_.reset();
return false;
}

// Add callback to dynamically update parameters
on_set_callback_handle_ = this->params_interface_->add_on_set_parameters_callback(
[this](const std::vector<rclcpp::Parameter> & parameters) {
return parameters_->set_parameter_callback(parameters);
});
catch (rclcpp::exceptions::InvalidParameterValueException & ex) {
RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what());
parameter_handler_.reset();
return false;
}
parameters_ = parameter_handler_->get_params();
compute_internal_params();

return true;
}
Expand Down
Loading

0 comments on commit e6d3e27

Please sign in to comment.