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.

Sign up for GitHub

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

Jump to bottom

add interface for warning, error and report #13

Closed
wants to merge 6 commits into from
Closed

add interface for warning, error and report #13

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
ff092df
add interface for warning, error and report
mamueluth Jan 11, 2024
ce485b2
add emergency stop
mamueluth Jan 11, 2024
d3d8cdb
create std interfaces for errors and warnings
mamueluth Jan 11, 2024
2a8d6f6
adjust to single interface per report, simplify to use value for report
mamueluth Jan 12, 2024
28a9a9f
finalize everything (comments, tests, cleanup)
mamueluth Jan 22, 2024
b4c9597
Update .clang-format
mamueluth Jan 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion .clang-format
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,5 +12,4 @@ DerivePointerAlignment: false
PointerAlignment: Middle
ReflowComments: true
IncludeBlocks: Preserve
InsertBraces: true
...
4 changes: 4 additions & 0 deletions hardware_interface/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,10 @@ if(BUILD_TESTING)
target_link_libraries(test_component_interfaces hardware_interface)
ament_target_dependencies(test_component_interfaces ros2_control_test_assets)

ament_add_gmock(test_error_warning_codes test/test_error_warning_codes.cpp)
target_link_libraries(test_error_warning_codes hardware_interface)
ament_target_dependencies(test_error_warning_codes ros2_control_test_assets)

ament_add_gmock(test_component_parser test/test_component_parser.cpp)
target_link_libraries(test_component_parser hardware_interface)
ament_target_dependencies(test_component_parser ros2_control_test_assets)
Expand Down
88 changes: 88 additions & 0 deletions hardware_interface/include/hardware_interface/actuator_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,10 @@
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp"
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
Expand Down Expand Up @@ -103,6 +106,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
info_ = hardware_info;
import_state_interface_descriptions(info_);
import_command_interface_descriptions(info_);
create_report_interfaces();
return CallbackReturn::SUCCESS;
};

Expand Down Expand Up @@ -134,6 +138,44 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
}
}

void create_report_interfaces()
{
// EMERGENCY STOP
InterfaceInfo emergency_interface_info;
emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL;
emergency_interface_info.data_type = "bool";
InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info);
emergency_stop_ = std::make_shared<StateInterface>(emergency_interface_descr);

// ERROR
// create error signal interface
InterfaceInfo error_interface_info;
error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME;
error_interface_info.data_type = "std::vector<uint8_t>";
InterfaceDescription error_interface_descr(info_.name, error_interface_info);
error_signal_ = std::make_shared<StateInterface>(error_interface_descr);
// create error signal report message interface
InterfaceInfo error_msg_interface_info;
error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME;
error_msg_interface_info.data_type = "std::vector<std::string>";
InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info);
error_signal_message_ = std::make_shared<StateInterface>(error_msg_interface_descr);

// WARNING
// create warning signal interface
InterfaceInfo warning_interface_info;
warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME;
warning_interface_info.data_type = "std::vector<uint8_t>";
InterfaceDescription warning_interface_descr(info_.name, warning_interface_info);
warning_signal_ = std::make_shared<StateInterface>(warning_interface_descr);
// create warning signal report message interface
InterfaceInfo warning_msg_interface_info;
warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME;
warning_msg_interface_info.data_type = "std::vector<std::string>";
InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
warning_signal_message_ = std::make_shared<StateInterface>(warning_msg_interface_descr);
}

/// Exports all state interfaces for this hardware interface.
/**
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
Expand Down Expand Up @@ -170,6 +212,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
actuator_states_.insert(std::make_pair(name, std::make_shared<StateInterface>(descr)));
state_interfaces.push_back(actuator_states_.at(name));
}

// export warning signal interfaces
state_interfaces.push_back(emergency_stop_);
state_interfaces.push_back(error_signal_);
state_interfaces.push_back(error_signal_message_);
state_interfaces.push_back(warning_signal_);
state_interfaces.push_back(warning_signal_message_);

return state_interfaces;
}
/// Exports all command interfaces for this hardware interface.
Expand Down Expand Up @@ -310,13 +360,51 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
return actuator_commands_.at(interface_name)->get_value();
}

void set_emergency_stop(const double & emergency_stop)
{
emergency_stop_->set_value(emergency_stop);
}

double get_emergency_stop() const { return emergency_stop_->get_value(); }

void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }

double get_warning_code() const { return warning_signal_->get_value(); }

void set_warning_message(const double & error_message)
{
warning_signal_message_->set_value(error_message);
}

double get_warning_message() const { return warning_signal_message_->get_value(); }

void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }

double get_error_code(const std::string & error_signal) const
{
return error_signal_->get_value();
}

void set_error_message(const double & error_message)
{
error_signal_message_->set_value(error_message);
}

double get_error_message() const { return error_signal_message_->get_value(); }

protected:
HardwareInfo info_;
std::map<std::string, InterfaceDescription> joint_state_interfaces_;
std::map<std::string, InterfaceDescription> joint_command_interfaces_;

private:
std::map<std::string, std::shared_ptr<StateInterface>> actuator_states_;
std::shared_ptr<StateInterface> emergency_stop_;
std::shared_ptr<StateInterface> error_signal_;
std::shared_ptr<StateInterface> error_signal_message_;
std::shared_ptr<StateInterface> warning_signal_;
std::shared_ptr<StateInterface> warning_signal_message_;

std::map<std::string, std::shared_ptr<CommandInterface>> actuator_commands_;

rclcpp_lifecycle::State lifecycle_state_;
Expand Down
3 changes: 2 additions & 1 deletion hardware_interface/include/hardware_interface/handle.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <string>
#include <utility>
#include <variant>
#include <vector>

#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/macros.hpp"
Expand All @@ -27,7 +28,7 @@
namespace hardware_interface
{

typedef std::variant<double> HANDLE_DATATYPE;
typedef std::variant<bool, double, std::vector<int8_t>, std::vector<uint8_t>> HANDLE_DATATYPE;

/// A handle used to get and set a value on a given interface.
class Handle
Expand Down
69 changes: 69 additions & 0 deletions hardware_interface/include/hardware_interface/sensor_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,9 @@
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_error_signals.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_warning_signals.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
Expand Down Expand Up @@ -102,6 +104,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
{
info_ = hardware_info;
import_state_interface_descriptions(info_);
create_report_interfaces();
return CallbackReturn::SUCCESS;
};

Expand All @@ -119,6 +122,37 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
}
}

void create_report_interfaces()
{
// ERROR
// create error signal interface
InterfaceInfo error_interface_info;
error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME;
error_interface_info.data_type = "std::vector<uint8_t>";
InterfaceDescription error_interface_descr(info_.name, error_interface_info);
error_signal_ = std::make_shared<StateInterface>(error_interface_descr);
// create error signal report message interface
InterfaceInfo error_msg_interface_info;
error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME;
error_msg_interface_info.data_type = "std::vector<std::string>";
InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info);
error_signal_message_ = std::make_shared<StateInterface>(error_msg_interface_descr);

// WARNING
// create warning signal interface
InterfaceInfo warning_interface_info;
warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME;
warning_interface_info.data_type = "std::vector<uint8_t>";
InterfaceDescription warning_interface_descr(info_.name, warning_interface_info);
warning_signal_ = std::make_shared<StateInterface>(warning_interface_descr);
// create warning signal report message interface
InterfaceInfo warning_msg_interface_info;
warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME;
warning_msg_interface_info.data_type = "std::vector<std::string>";
InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info);
warning_signal_message_ = std::make_shared<StateInterface>(warning_msg_interface_descr);
}

/// Exports all state interfaces for this hardware interface.
/**
* Default implementation for exporting the StateInterfaces. The StateInterfaces are created
Expand Down Expand Up @@ -157,6 +191,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
state_interfaces.push_back(sensor_states_.at(name));
}

// export warning signal interfaces
state_interfaces.push_back(error_signal_);
state_interfaces.push_back(error_signal_message_);
state_interfaces.push_back(warning_signal_);
state_interfaces.push_back(warning_signal_message_);

return state_interfaces;
}

Expand Down Expand Up @@ -200,13 +240,42 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
return sensor_states_.at(interface_name)->get_value();
}

void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }

double get_warning_code() const { return warning_signal_->get_value(); }

void set_warning_message(const double & error_message)
{
warning_signal_message_->set_value(error_message);
}

double get_warning_message() const { return warning_signal_message_->get_value(); }

void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }

double get_error_code(const std::string & error_signal) const
{
return error_signal_->get_value();
}

void set_error_message(const double & error_message)
{
error_signal_message_->set_value(error_message);
}

double get_error_message() const { return error_signal_message_->get_value(); }

protected:
HardwareInfo info_;

std::map<std::string, InterfaceDescription> sensor_state_interfaces_;

private:
std::map<std::string, std::shared_ptr<StateInterface>> sensor_states_;
std::shared_ptr<StateInterface> error_signal_;
std::shared_ptr<StateInterface> error_signal_message_;
std::shared_ptr<StateInterface> warning_signal_;
std::shared_ptr<StateInterface> warning_signal_message_;

rclcpp_lifecycle::State lifecycle_state_;
};
Expand Down
Loading
Loading