Skip to content

Commit

Permalink
fixed format
Browse files Browse the repository at this point in the history
  • Loading branch information
Maciej Bednarczyk committed Nov 16, 2022
1 parent 4b9f37b commit c3d68a9
Show file tree
Hide file tree
Showing 6 changed files with 95 additions and 94 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,10 @@

#include <memory>
#include <string>
#include <vector>
#include <unordered_map>
#include <vector>

#include "control_msgs/msg/dynamic_joint_state.hpp"
#include "controller_interface/controller_interface.hpp"
#include "gpio_controllers/visibility_control.h"
#include "rclcpp/subscription.hpp"
Expand All @@ -28,7 +29,6 @@
#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_publisher.h"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "control_msgs/msg/dynamic_joint_state.hpp"

namespace gpio_controllers
{
Expand Down
48 changes: 24 additions & 24 deletions gpio_controllers/include/gpio_controllers/visibility_control.h
Original file line number Diff line number Diff line change
Expand Up @@ -19,31 +19,31 @@
// https://gcc.gnu.org/wiki/Visibility

#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__ ((dllexport))
#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__ ((dllimport))
#else
#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport)
#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport)
#endif
#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY
#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT
#else
#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT
#endif
#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC
#define GPIO_COMMAND_CONTROLLER_LOCAL
#ifdef __GNUC__
#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport))
#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport))
#else
#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__ ((visibility("default")))
#define GPIO_COMMAND_CONTROLLER_IMPORT
#if __GNUC__ >= 4
#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__ ((visibility("default")))
#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__ ((visibility("hidden")))
#else
#define GPIO_COMMAND_CONTROLLER_PUBLIC
#define GPIO_COMMAND_CONTROLLER_LOCAL
#endif
#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE
#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport)
#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport)
#endif
#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY
#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT
#else
#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT
#endif
#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC
#define GPIO_COMMAND_CONTROLLER_LOCAL
#else
#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default")))
#define GPIO_COMMAND_CONTROLLER_IMPORT
#if __GNUC__ >= 4
#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default")))
#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden")))
#else
#define GPIO_COMMAND_CONTROLLER_PUBLIC
#define GPIO_COMMAND_CONTROLLER_LOCAL
#endif
#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE
#endif

#endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_
120 changes: 64 additions & 56 deletions gpio_controllers/src/gpio_command_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,9 @@ CallbackReturn GpioCommandController::on_init()
{
auto_declare<std::vector<std::string>>("gpios", std::vector<std::string>());
gpio_names_ = get_node()->get_parameter("gpios").as_string_array();
for(std::string &gpio : gpio_names_)
auto_declare<std::vector<std::string>>("command_interfaces."
+ gpio, std::vector<std::string>());
for (std::string & gpio : gpio_names_)
auto_declare<std::vector<std::string>>(
"command_interfaces." + gpio, std::vector<std::string>());
}
catch (const std::exception & e)
{
Expand All @@ -59,56 +59,61 @@ CallbackReturn GpioCommandController::on_init()
CallbackReturn GpioCommandController::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
gpio_names_ = get_node()->get_parameter("gpios").as_string_array();
gpio_names_ = get_node()->get_parameter("gpios").as_string_array();

if (gpio_names_.empty()){
RCLCPP_ERROR(get_node()->get_logger(), "'gpios' parameter was empty");
return CallbackReturn::ERROR;
}
if (gpio_names_.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "'gpios' parameter was empty");
return CallbackReturn::ERROR;
}

for(std::string &gpio : gpio_names_){
auto interfaces = get_node()->get_parameter("command_interfaces." + gpio).as_string_array();
if (interfaces.empty()){
RCLCPP_ERROR(get_node()->get_logger(),
"'command_interfaces.%s' parameter was empty", gpio.c_str());
return CallbackReturn::ERROR;
}
if ( !interface_names_.insert( std::make_pair( gpio, interfaces) ).second ) {
RCLCPP_ERROR(get_node()->get_logger(),
"Trying to override existing gpio setup. Wrong controller parameters.");
return CallbackReturn::ERROR;
}
for (std::string & gpio : gpio_names_)
{
auto interfaces = get_node()->get_parameter("command_interfaces." + gpio).as_string_array();
if (interfaces.empty())
{
RCLCPP_ERROR(
get_node()->get_logger(), "'command_interfaces.%s' parameter was empty", gpio.c_str());
return CallbackReturn::ERROR;
}

for(const auto & gpio : gpio_names_){
for(const auto & interface_name: interface_names_[gpio]){
interface_types_.push_back(gpio + "/" + interface_name);
}
if (!interface_names_.insert(std::make_pair(gpio, interfaces)).second)
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Trying to override existing gpio setup. Wrong controller parameters.");
return CallbackReturn::ERROR;
}
}

try
for (const auto & gpio : gpio_names_)
{
for (const auto & interface_name : interface_names_[gpio])
{
gpios_command_subscriber_ = get_node()->create_subscription<CmdType>(
"~/commands", rclcpp::SystemDefaultsQoS(),
[this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); });
interface_types_.push_back(gpio + "/" + interface_name);
}
}

gpio_state_publisher_ =
get_node()->create_publisher<StateType>(
"~/gpio_states", rclcpp::SystemDefaultsQoS());
try
{
gpios_command_subscriber_ = get_node()->create_subscription<CmdType>(
"~/commands", rclcpp::SystemDefaultsQoS(),
[this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); });

realtime_gpio_state_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<StateType>>(
gpio_state_publisher_);
}
catch (const std::exception & e)
{
// get_node() may throw, logging raw here
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return CallbackReturn::ERROR;
}
gpio_state_publisher_ =
get_node()->create_publisher<StateType>("~/gpio_states", rclcpp::SystemDefaultsQoS());

realtime_gpio_state_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<StateType>>(gpio_state_publisher_);
}
catch (const std::exception & e)
{
// get_node() may throw, logging raw here
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return CallbackReturn::ERROR;
}

RCLCPP_INFO(get_node()->get_logger(), "configure successful");
return CallbackReturn::SUCCESS;
RCLCPP_INFO(get_node()->get_logger(), "configure successful");
return CallbackReturn::SUCCESS;
}

controller_interface::InterfaceConfiguration
Expand All @@ -121,8 +126,8 @@ GpioCommandController::command_interface_configuration() const
return command_interfaces_config;
}

controller_interface::InterfaceConfiguration
GpioCommandController::state_interface_configuration() const
controller_interface::InterfaceConfiguration GpioCommandController::state_interface_configuration()
const
{
controller_interface::InterfaceConfiguration state_interfaces_config;
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
Expand All @@ -141,11 +146,10 @@ CallbackReturn GpioCommandController::on_activate(
command_interfaces_.size() != ordered_interfaces.size())
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Expected %zu command interfaces, got %zu", interface_types_.size(),
get_node()->get_logger(), "Expected %zu command interfaces, got %zu", interface_types_.size(),
ordered_interfaces.size());

for(const auto & interface: interface_types_)
for (const auto & interface : interface_types_)
RCLCPP_ERROR(get_node()->get_logger(), "Got %s", interface.c_str());
return CallbackReturn::ERROR;
}
Expand All @@ -155,9 +159,11 @@ CallbackReturn GpioCommandController::on_activate(
gpio_state_msg.header.stamp = get_node()->now();
gpio_state_msg.joint_names.resize(gpio_names_.size());
gpio_state_msg.interface_values.resize(gpio_names_.size());
for(auto g = 0ul; g < gpio_names_.size(); g++){
for (auto g = 0ul; g < gpio_names_.size(); g++)
{
gpio_state_msg.joint_names[g] = gpio_names_[g];
for(const auto & interface_name: interface_names_[gpio_names_[g]]){
for (const auto & interface_name : interface_names_[gpio_names_[g]])
{
gpio_state_msg.interface_values[g].interface_names.push_back(interface_name);
gpio_state_msg.interface_values[g].values.push_back(std::numeric_limits<double>::quiet_NaN());
}
Expand Down Expand Up @@ -189,10 +195,12 @@ controller_interface::return_type GpioCommandController::update(
gpio_state_msg.header.stamp = get_node()->now();

auto sindex = 0ul;
for(auto g = 0ul; g < gpio_names_.size(); g++){
for(auto i = 0ul; i < interface_names_[gpio_names_[g]].size(); i++){
for (auto g = 0ul; g < gpio_names_.size(); g++)
{
for (auto i = 0ul; i < interface_names_[gpio_names_[g]].size(); i++)
{
gpio_state_msg.interface_values[g].values[i] = state_interfaces_[sindex].get_value();
sindex ++;
sindex++;
}
}

Expand All @@ -211,8 +219,8 @@ controller_interface::return_type GpioCommandController::update(
{
RCLCPP_ERROR_THROTTLE(
get_node()->get_logger(), *get_node()->get_clock(), 1000,
"command size (%zu) does not match number of interfaces (%zu)",
(*gpio_commands)->data.size(), command_interfaces_.size());
"command size (%zu) does not match number of interfaces (%zu)", (*gpio_commands)->data.size(),
command_interfaces_.size());
return controller_interface::return_type::ERROR;
}

Expand Down
11 changes: 2 additions & 9 deletions gpio_controllers/test/test_gpio_command_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ void GpioCommandControllerTest::SetUpController()
state_ifs.emplace_back(gpio_1_1_dig_state_);
state_ifs.emplace_back(gpio_1_2_dig_state_);
state_ifs.emplace_back(gpio_2_ana_state_);

controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
}

Expand All @@ -96,8 +96,7 @@ TEST_F(GpioCommandControllerTest, GpioWithMissingGpioParams)
controller_->get_node()->set_parameter({"gpios", std::vector<std::string>{"gpio1", "gpio2"}});
controller_->get_node()->set_parameter(
{"command_interfaces.gpio1", std::vector<std::string>{"dig.1", "dig.2"}});
controller_->get_node()->set_parameter(
{"command_interfaces.gpio2", std::vector<std::string>()});
controller_->get_node()->set_parameter({"command_interfaces.gpio2", std::vector<std::string>()});
// // activate failed, command interface for 'gpio2' is not set up
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
}
Expand Down Expand Up @@ -138,7 +137,6 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest)
controller_->get_node()->set_parameter(
{"command_interfaces.gpio2", std::vector<std::string>{"ana.1"}});


ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

Expand Down Expand Up @@ -177,11 +175,9 @@ TEST_F(GpioCommandControllerTest, WrongCommandCheckTest)
controller_->get_node()->set_parameter(
{"command_interfaces.gpio2", std::vector<std::string>{"ana.1"}});


ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);


// send command with wrong number of gpios
auto command_ptr = std::make_shared<CmdType>();
command_ptr->data = {0.0, 20.0};
Expand All @@ -207,7 +203,6 @@ TEST_F(GpioCommandControllerTest, NoCommandCheckTest)
controller_->get_node()->set_parameter(
{"command_interfaces.gpio2", std::vector<std::string>{"ana.1"}});


ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

Expand Down Expand Up @@ -235,7 +230,6 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest)
controller_->get_node()->set_parameter(
{"state_interfaces.gpio2", std::vector<std::string>{"ana.1"}});


// default values
ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0);
ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0);
Expand Down Expand Up @@ -289,7 +283,6 @@ TEST_F(GpioCommandControllerTest, StateCallbackTest)
controller_->get_node()->set_parameter(
{"state_interfaces.gpio2", std::vector<std::string>{"ana.1"}});


// default values
ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0);
ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0);
Expand Down
2 changes: 1 addition & 1 deletion gpio_controllers/test/test_gpio_command_controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,9 @@

#include "gmock/gmock.h"

#include "gpio_controllers/gpio_command_controller.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "gpio_controllers/gpio_command_controller.hpp"

using hardware_interface::CommandInterface;
using hardware_interface::StateInterface;
Expand Down
4 changes: 2 additions & 2 deletions gpio_controllers/test/test_load_gpio_command_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@ TEST(TestLoadGpioCommandController, load_controller)
ros2_control_test_assets::minimal_robot_urdf),
executor, "test_controller_manager");

ASSERT_NO_THROW(cm.load_controller(
"test_gpio_command_controller", "gpio_controllers/GpioCommandController"));
ASSERT_NO_THROW(
cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController"));

rclcpp::shutdown();
}

0 comments on commit c3d68a9

Please sign in to comment.