Skip to content

Commit

Permalink
properly separate the class definitions and declarations into separat…
Browse files Browse the repository at this point in the history 
…e files
  • Loading branch information
@saikishor
saikishor committed Aug 8, 2023
1 parent fe635c9 commit 3c92eaf
Show file tree
Hide file tree
Showing 3 changed files with 210 additions and 181 deletions.
196 changes: 33 additions & 163 deletions example_12/controllers/include/passthrough_controller/passthrough_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,187 +21,58 @@
#include <string>
#include <vector>

#include <controller_interface/chainable_controller_interface.hpp>
#include "controller_interface/helpers.hpp"
#include "controller_interface/chainable_controller_interface.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "std_msgs/msg/float64_multi_array.hpp"
// auto-generated by generate_parameter_library
#include "passthrough_controller_parameters.hpp"

#include "passthrough_controller/visibility_control.h"

/**
* PassthroughController is a simple chainable controller that exposes reference interfaces equal to
* the number of it's command interfaces. This controller simply forwards the information commanded
* to it's reference interface to it's own command interfaces without any modifications.
*/
namespace passthrough_controller
{
using DataType = std_msgs::msg::Float64MultiArray;
class PassthroughController : public controller_interface::ChainableControllerInterface
{
public:
PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
PassthroughController() {}

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
controller_interface::CallbackReturn on_init() override
{
try
{
param_listener_ = std::make_shared<ParamListener>(get_node());
params_ = param_listener_->get_params();
}
catch (const std::exception & e)
{
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
}
return controller_interface::CallbackReturn::SUCCESS;
}

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override
{
params_ = param_listener_->get_params();
command_interface_names_ = params_.interfaces;

joints_cmd_sub_ = this->get_node()->create_subscription<DataType>(
"~/commands", rclcpp::SystemDefaultsQoS(),
[this](const DataType::SharedPtr msg)
{
// check if message is correct size, if not ignore
if (msg->data.size() == command_interface_names_.size())
{
rt_buffer_ptr_.writeFromNonRT(msg);
}
else
{
RCLCPP_ERROR(
this->get_node()->get_logger(),
"Invalid command received of %zu size, expected %zu size", msg->data.size(),
command_interface_names_.size());
}
});

// pre-reserve command interfaces
command_interfaces_.reserve(command_interface_names_.size());

RCLCPP_INFO(this->get_node()->get_logger(), "configure successful");

// The names should be in the same order as for command interfaces for easier matching
reference_interface_names_ = command_interface_names_;
// for any case make reference interfaces size of command interfaces
reference_interfaces_.resize(
reference_interface_names_.size(), std::numeric_limits<double>::quiet_NaN());

return controller_interface::CallbackReturn::SUCCESS;
}

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override
{
// check if we have all resources defined in the "points" parameter
// also verify that we *only* have the resources defined in the "points" parameter
// ATTENTION(destogl): Shouldn't we use ordered interface all the time?
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
ordered_interfaces;
if (
!controller_interface::get_ordered_interfaces(
command_interfaces_, command_interface_names_, std::string(""), ordered_interfaces) ||
command_interface_names_.size() != ordered_interfaces.size())
{
RCLCPP_ERROR(
this->get_node()->get_logger(), "Expected %zu command interfaces, got %zu",
command_interface_names_.size(), ordered_interfaces.size());
return controller_interface::CallbackReturn::ERROR;
}

// reset command buffer if a command came through callback when controller was inactive
rt_buffer_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<DataType>>(nullptr);

RCLCPP_INFO(this->get_node()->get_logger(), "activate successful");

std::fill(
reference_interfaces_.begin(), reference_interfaces_.end(),
std::numeric_limits<double>::quiet_NaN());

return controller_interface::CallbackReturn::SUCCESS;
}

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override
{
// reset command buffer
rt_buffer_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<DataType>>(nullptr);
return controller_interface::CallbackReturn::SUCCESS;
}

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration() const override
{
controller_interface::InterfaceConfiguration command_interfaces_config;
command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
command_interfaces_config.names = command_interface_names_;

return command_interfaces_config;
}

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration() const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
controller_interface::return_type update_reference_from_subscribers(
const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override
{
auto joint_commands = rt_buffer_ptr_.readFromRT();
// message is valid
if (!(!joint_commands || !(*joint_commands)))
{
if (reference_interfaces_.size() != (*joint_commands)->data.size())
{
RCLCPP_ERROR_THROTTLE(
get_node()->get_logger(), *(get_node()->get_clock()), 1000,
"command size (%zu) does not match number of reference interfaces (%zu)",
(*joint_commands)->data.size(), reference_interfaces_.size());
return controller_interface::return_type::ERROR;
}
reference_interfaces_ = (*joint_commands)->data;
}

return controller_interface::return_type::OK;
}
PASSTHROUGH_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_init() override;

protected:
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override
{
std::vector<hardware_interface::CommandInterface> reference_interfaces;
PASSTHROUGH_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration() const override;

PASSTHROUGH_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration() const override;

for (size_t i = 0; i < reference_interface_names_.size(); ++i)
{
reference_interfaces.push_back(hardware_interface::CommandInterface(
get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i]));
}
PASSTHROUGH_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;

return reference_interfaces;
}
PASSTHROUGH_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
bool on_set_chained_mode(bool /* chained_mode */) override { return true; }
PASSTHROUGH_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;

PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
PASSTHROUGH_CONTROLLER_PUBLIC
bool on_set_chained_mode(bool chained_mode) override;

PASSTHROUGH_CONTROLLER_PUBLIC
controller_interface::return_type update_and_write_commands(
const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override
{
for (size_t i = 0; i < command_interfaces_.size(); ++i)
{
if (!std::isnan(reference_interfaces_[i]))
{
command_interfaces_[i].set_value(reference_interfaces_[i]);
}
}

return controller_interface::return_type::OK;
}
const rclcpp::Time & time, const rclcpp::Duration & period) override;

protected:
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;

controller_interface::return_type update_reference_from_subscribers(
const rclcpp::Time & time, const rclcpp::Duration & period) override;

std::shared_ptr<ParamListener> param_listener_;
Params params_;
Expand All @@ -213,7 +84,6 @@ class PassthroughController : public controller_interface::ChainableControllerIn

std::vector<std::string> command_interface_names_;
};

} // namespace passthrough_controller

#endif // PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_
32 changes: 16 additions & 16 deletions example_12/controllers/include/passthrough_controller/visibility_control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,30 +20,30 @@

#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport))
#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport))
#define PASSTHROUGH_CONTROLLER_EXPORT __attribute__((dllexport))
#define PASSTHROUGH_CONTROLLER_IMPORT __attribute__((dllimport))
#else
#define PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport)
#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport)
#define PASSTHROUGH_CONTROLLER_EXPORT __declspec(dllexport)
#define PASSTHROUGH_CONTROLLER_IMPORT __declspec(dllimport)
#endif
#ifdef PASSTHROUGH_CONTROLLER__VISIBILITY_BUILDING_DLL
#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT
#ifdef PASSTHROUGH_CONTROLLER_BUILDING_DLL
#define PASSTHROUGH_CONTROLLER_PUBLIC PASSTHROUGH_CONTROLLER_EXPORT
#else
#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT
#define PASSTHROUGH_CONTROLLER_PUBLIC PASSTHROUGH_CONTROLLER_IMPORT
#endif
#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC_TYPE PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL
#define PASSTHROUGH_CONTROLLER_PUBLIC_TYPE PASSTHROUGH_CONTROLLER_PUBLIC
#define PASSTHROUGH_CONTROLLER_LOCAL
#else
#define PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default")))
#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT
#define PASSTHROUGH_CONTROLLER_EXPORT __attribute__((visibility("default")))
#define PASSTHROUGH_CONTROLLER_IMPORT
#if __GNUC__ >= 4
#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default")))
#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
#define PASSTHROUGH_CONTROLLER_PUBLIC __attribute__((visibility("default")))
#define PASSTHROUGH_CONTROLLER_LOCAL __attribute__((visibility("hidden")))
#else
#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL
#define PASSTHROUGH_CONTROLLER_PUBLIC
#define PASSTHROUGH_CONTROLLER_LOCAL
#endif
#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC_TYPE
#define PASSTHROUGH_CONTROLLER_PUBLIC_TYPE
#endif

#endif // PASSTHROUGH_CONTROLLER__VISIBILITY_CONTROL_H_
Loading

0 comments on commit 3c92eaf

Please sign in to comment.