Feature/safety position controller #10
Conversation
hector_ros_controllers/params/safety_position_controller_parameters.yaml
Outdated
Show resolved
Hide resolved
hector_ros_controllers/params/safety_position_controller_parameters.yaml
Outdated
Show resolved
Hide resolved
| bool SafetyPositionController::gather_interface_indices() | ||
| { | ||
| bool success = true; | ||
| for ( size_t i = 0; i < params_.joints.size(); ++i ) { | ||
| state_interface_index_[i] = -1; | ||
| for ( size_t s = 0; s < state_interfaces_.size(); ++s ) { | ||
| auto name = state_interfaces_[s].get_name(); | ||
| auto interface_name = state_interfaces_[s].get_interface_name(); | ||
| if ( state_interfaces_[s].get_name() == params_.joints[i] + "/position" && | ||
| state_interfaces_[s].get_interface_name() == "position" ) { | ||
| state_interface_index_[i] = static_cast<int>( s ); | ||
| break; | ||
| } | ||
| } | ||
| if ( state_interface_index_[i] < 0 ) | ||
| RCLCPP_WARN( get_node()->get_logger(), "No state interface 'position' found for joint '%s'.", | ||
| params_.joints[i].c_str() ); | ||
| success &= ( state_interface_index_[i] >= 0 ); | ||
| } | ||
| return success; | ||
| } |
There was a problem hiding this comment.
This propably can be omitted. State interfaces will be loaded in the same order as they are provided in state_interface_configuration() . If indices are required in a separate array, this can be handled in the respective loop from the state interface configuration.
| bool success = true; | ||
| for ( size_t i = 0; i < params_.joints.size(); ++i ) { | ||
| if ( !std::isnan( commands[i] ) ) { | ||
| success &= command_interfaces_[i].set_value( commands[i] ); |
There was a problem hiding this comment.
This should also get an opt first and check for a value. Otherwise compiler complains && it is already implemented properly for reading operations
There was a problem hiding this comment.
In this case, set_value() returns a bool, so using &= works as intended and doesn’t trigger any compiler warnings. The optional/value check is only required when reading from interfaces (to ensure a value is available), not when setting them. I haven't encountered any compiler warnings.
| const auto &limit = in_compliant_mode_ | ||
| ? params_.current_limits.joints_map[params_.joints[i]].compliant_limit | ||
| : params_.current_limits.joints_map[params_.joints[i]].stiff_limit; | ||
| success &= command_interfaces_[i + params_.joints.size()].set_value( limit ); |
There was a problem hiding this comment.
This should also get an opt first and check for a value. Otherwise compiler complains && it is already implemented properly for reading operations
This PR introduces the SafetyPositionController, a chainable ROS 2 controller that ensures safe joint position commands.
Key features