Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,16 @@
#define JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_

#include <string>
#include <vector>

#include "parameter_traits/parameter_traits.hpp"
#include "rsl/algorithm.hpp"
#include "tl_expected/expected.hpp"

namespace parameter_traits
{
Result command_interface_type_combinations(rclcpp::Parameter const & parameter)
tl::expected<void, std::string> command_interface_type_combinations(
rclcpp::Parameter const & parameter)
{
auto const & interface_types = parameter.as_string_array();

Expand All @@ -31,59 +35,63 @@ Result command_interface_type_combinations(rclcpp::Parameter const & parameter)
// 2. position [velocity, [acceleration]]

if (
contains<std::string>(interface_types, "velocity") && interface_types.size() > 1 &&
!contains<std::string>(interface_types, "position"))
rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
interface_types.size() > 1 &&
!rsl::contains<std::vector<std::string>>(interface_types, "position"))
{
return ERROR(
return tl::make_unexpected(
"'velocity' command interface can be used either alone or 'position' "
"interface has to be present");
}

if (
contains<std::string>(interface_types, "acceleration") &&
(!contains<std::string>(interface_types, "velocity") &&
!contains<std::string>(interface_types, "position")))
rsl::contains<std::vector<std::string>>(interface_types, "acceleration") &&
(!rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
!rsl::contains<std::vector<std::string>>(interface_types, "position")))
{
return ERROR(
return tl::make_unexpected(
"'acceleration' command interface can only be used if 'velocity' and "
"'position' interfaces are present");
}

if (contains<std::string>(interface_types, "effort") && interface_types.size() > 1)
if (
rsl::contains<std::vector<std::string>>(interface_types, "effort") &&
interface_types.size() > 1)
{
return ERROR("'effort' command interface has to be used alone");
return tl::make_unexpected("'effort' command interface has to be used alone");
}

return OK;
return {};
}

Result state_interface_type_combinations(rclcpp::Parameter const & parameter)
tl::expected<void, std::string> state_interface_type_combinations(
rclcpp::Parameter const & parameter)
{
auto const & interface_types = parameter.as_string_array();

// Valid combinations are
// 1. position [velocity, [acceleration]]

if (
contains<std::string>(interface_types, "velocity") &&
!contains<std::string>(interface_types, "position"))
rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
!rsl::contains<std::vector<std::string>>(interface_types, "position"))
{
return ERROR(
return tl::make_unexpected(
"'velocity' state interface cannot be used if 'position' interface "
"is missing.");
}

if (
contains<std::string>(interface_types, "acceleration") &&
(!contains<std::string>(interface_types, "position") ||
!contains<std::string>(interface_types, "velocity")))
rsl::contains<std::vector<std::string>>(interface_types, "acceleration") &&
(!rsl::contains<std::vector<std::string>>(interface_types, "position") ||
!rsl::contains<std::vector<std::string>>(interface_types, "velocity")))
{
return ERROR(
return tl::make_unexpected(
"'acceleration' state interface cannot be used if 'position' and 'velocity' "
"interfaces are not present.");
}

return OK;
return {};
}

} // namespace parameter_traits
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,15 +55,15 @@ joint_trajectory_controller:
default_value: 50.0,
description: "Rate controller state is published",
validation: {
lower_bounds: [0.1]
gt_eq: [0.1]
}
}
action_monitor_rate: {
type: double,
default_value: 20.0,
description: "Rate status changes will be monitored",
validation: {
lower_bounds: [0.1]
gt_eq: [0.1]
}
}
interpolation_method: {
Expand Down Expand Up @@ -117,7 +117,7 @@ joint_trajectory_controller:
default_value: 0.0,
description: "Time tolerance for achieving trajectory goal before or after commanded time.",
validation: {
lower_bounds: [0.0],
gt_eq: [0.0],
}
}
__map_joints:
Expand Down