Skip to content

Commit

Permalink
[backport] Fix #730 (#736)
Browse files Browse the repository at this point in the history
* Fix file name for include guard (backport #681)

(cherry picked from commit c619aac)

Co-authored-by: Christoph Fröhlich <[email protected]>

* Activate AdmittanceControllerTestParameterizedInvalidParameters (#711) (#733)

* [JTC] Re-enabling test, bugfixing and hardening. Adding a parameter to define when trajectories with non-zero velocity at the end are used. (backport #705) (#706)

* Enable effort rejection test

---------

Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com>
  • Loading branch information
christophfroehlich and mergify[bot] authored Aug 5, 2023
1 parent 17e14e3 commit 6adb847
Show file tree
Hide file tree
Showing 15 changed files with 552 additions and 1,050 deletions.
4 changes: 4 additions & 0 deletions .github/workflows/rolling-binary-build-main.yml
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,13 @@ on:
workflow_dispatch:
branches:
- master
- '*feature*'
- '*feature/**'
pull_request:
branches:
- master
- '*feature*'
- '*feature/**'
push:
branches:
- master
Expand Down
4 changes: 4 additions & 0 deletions .github/workflows/rolling-binary-build-testing.yml
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,13 @@ on:
workflow_dispatch:
branches:
- master
- '*feature*'
- '*feature/**'
pull_request:
branches:
- master
- '*feature*'
- '*feature/**'
push:
branches:
- master
Expand Down
4 changes: 4 additions & 0 deletions .github/workflows/rolling-semi-binary-build-main.yml
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,13 @@ on:
workflow_dispatch:
branches:
- master
- '*feature*'
- '*feature/**'
pull_request:
branches:
- master
- '*feature*'
- '*feature/**'
push:
branches:
- master
Expand Down
4 changes: 4 additions & 0 deletions .github/workflows/rolling-semi-binary-build-testing.yml
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,13 @@ on:
workflow_dispatch:
branches:
- master
- '*feature*'
- '*feature/**'
pull_request:
branches:
- master
- '*feature*'
- '*feature/**'
push:
branches:
- master
Expand Down
16 changes: 12 additions & 4 deletions admittance_controller/test/test_admittance_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,10 +67,18 @@ INSTANTIATE_TEST_SUITE_P(
// wrong length selected axes
std::make_tuple(
std::string("admittance.selected_axes"),
rclcpp::ParameterValue(std::vector<double>() = {1, 2, 3})),
// invalid robot description
std::make_tuple(
std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot"))));
rclcpp::ParameterValue(std::vector<double>() = {1, 2, 3}))
// invalid robot description.
// TODO(anyone): deactivated, because SetUpController returns SUCCESS here?
// ,std::make_tuple(
// std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))
));

// Test on_init returns ERROR when a parameter is invalid
TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, invalid_parameters)
{
ASSERT_EQ(SetUpController(), controller_interface::return_type::ERROR);
}

TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success)
{
Expand Down
12 changes: 9 additions & 3 deletions admittance_controller/test/test_admittance_controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@

#include "gmock/gmock.h"

#include "6d_robot_description.hpp"
#include "admittance_controller/admittance_controller.hpp"
#include "control_msgs/msg/admittance_controller_state.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
Expand All @@ -38,6 +37,7 @@
#include "rclcpp/utilities.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "semantic_components/force_torque_sensor.hpp"
#include "test_asset_6d_robot_description.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "trajectory_msgs/msg/joint_trajectory.hpp"

Expand Down Expand Up @@ -454,9 +454,15 @@ class AdmittanceControllerTestParameterizedInvalidParameters
static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); }

protected:
void SetUpController()
controller_interface::return_type SetUpController()
{
AdmittanceControllerTest::SetUpController("test_admittance_controller");
auto param_name = std::get<0>(GetParam());
auto param_value = std::get<1>(GetParam());
std::vector<rclcpp::Parameter> parameter_overrides;
rclcpp::Parameter param(param_name, param_value);
parameter_overrides.push_back(param);
return AdmittanceControllerTest::SetUpController(
"test_admittance_controller", parameter_overrides);
}
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_
#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_
#ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_
#define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_

#include <string>

Expand Down Expand Up @@ -310,4 +310,4 @@ const auto valid_6d_robot_srdf =

} // namespace ros2_control_test_assets

#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_
#endif // TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_
14 changes: 7 additions & 7 deletions joint_trajectory_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -78,13 +78,13 @@ if(BUILD_TESTING)
ros2_control_test_assets
)

# TODO(andyz): Disabled due to flakiness
# ament_add_gmock(test_trajectory_actions
# test/test_trajectory_actions.cpp
# )
# target_link_libraries(test_trajectory_actions
# joint_trajectory_controller
# )
ament_add_gmock(test_trajectory_actions
test/test_trajectory_actions.cpp
)
set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300)
target_link_libraries(test_trajectory_actions
joint_trajectory_controller
)
endif()


Expand Down
5 changes: 5 additions & 0 deletions joint_trajectory_controller/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -159,6 +159,11 @@ open_loop_control (boolean)

If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits<double>::quiet_NaN()) or state values when the hardware is started.

allow_nonzero_velocity_at_trajectory_end (boolean)
If false, the last velocity point has to be zero or the goal will be rejected.

Default: true

constraints (structure)
Default values for tolerances if no explicit values are states in JointTrajectory message.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,11 @@
#include <vector>

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

namespace parameter_traits
namespace joint_trajectory_controller
{
tl::expected<void, std::string> command_interface_type_combinations(
rclcpp::Parameter const & parameter)
Expand Down Expand Up @@ -94,6 +95,6 @@ tl::expected<void, std::string> state_interface_type_combinations(
return {};
}

} // namespace parameter_traits
} // namespace joint_trajectory_controller

#endif // JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_
29 changes: 27 additions & 2 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
return CallbackReturn::ERROR;
}

// TODO(christophfroehlich): remove deprecation warning
if (params_.allow_nonzero_velocity_at_trajectory_end)
{
RCLCPP_WARN(
get_node()->get_logger(),
"[Deprecated]: \"allow_nonzero_velocity_at_trajectory_end\" is set to "
"true. The default behavior will change to false.");
}

return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -136,7 +145,9 @@ controller_interface::return_type JointTrajectoryController::update(
{
error.positions[index] = desired.positions[index] - current.positions[index];
}
if (has_velocity_state_interface_ && has_velocity_command_interface_)
if (
has_velocity_state_interface_ &&
(has_velocity_command_interface_ || has_effort_command_interface_))
{
error.velocities[index] = desired.velocities[index] - current.velocities[index];
}
Expand Down Expand Up @@ -234,7 +245,7 @@ controller_interface::return_type JointTrajectoryController::update(
const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time();
const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;

time_difference = get_node()->now().seconds() - traj_end.seconds();
time_difference = time.seconds() - traj_end.seconds();

if (time_difference > default_tolerances_.goal_time_tolerance)
{
Expand Down Expand Up @@ -1410,6 +1421,20 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}

if (!params_.allow_nonzero_velocity_at_trajectory_end)
{
for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
{
if (trajectory.points.back().velocities.at(i) != 0.)
{
RCLCPP_ERROR(
get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f",
trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i));
return false;
}
}
}

rclcpp::Duration previous_traj_time(0ms);
for (size_t i = 0; i < trajectory.points.size(); ++i)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ joint_trajectory_controller:
validation: {
unique<>: null,
subset_of<>: [["position", "velocity", "acceleration", "effort",]],
command_interface_type_combinations: null,
"joint_trajectory_controller::command_interface_type_combinations": null,
}
}
state_interfaces: {
Expand All @@ -32,7 +32,7 @@ joint_trajectory_controller:
validation: {
unique<>: null,
subset_of<>: [["position", "velocity", "acceleration",]],
state_interface_type_combinations: null,
"joint_trajectory_controller::state_interface_type_combinations": null,
}
}
allow_partial_joints_goal: {
Expand Down Expand Up @@ -74,6 +74,11 @@ joint_trajectory_controller:
one_of<>: [["splines", "none"]],
}
}
allow_nonzero_velocity_at_trajectory_end: {
type: bool,
default_value: true,
description: "If false, the last velocity point has to be zero or the goal will be rejected",
}
gains:
__map_joints:
p: {
Expand Down
Loading

0 comments on commit 6adb847

Please sign in to comment.