Skip to content

Commit 6a6cf15

Browse files
destogldestogl
committed
Correct jtc after applying review comments.
1 parent e3d8963 commit 6a6cf15

File tree

5 files changed

+45
-31
lines changed

5 files changed

+45
-31
lines changed

‎joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -115,7 +115,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
115115

116116
// To reduce number of variables and to make the code shorter the interfaces are ordered in types
117117
// as the following constants
118-
constexpr static auto allowed_interface_types_ = {
118+
const std::vector<std::string> allowed_interface_types_ = {
119119
hardware_interface::HW_IF_POSITION,
120120
hardware_interface::HW_IF_VELOCITY,
121121
hardware_interface::HW_IF_ACCELERATION,

‎joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -152,8 +152,10 @@ inline bool check_state_tolerance_per_joint(
152152
{
153153
using std::abs;
154154
const double error_position = state_error.positions[joint_idx];
155-
const double error_velocity = state_error.velocities[joint_idx];
156-
const double error_acceleration = state_error.accelerations[joint_idx];
155+
const double error_velocity = state_error.velocities.empty() ?
156+
0.0 : state_error.velocities[joint_idx];
157+
const double error_acceleration = state_error.accelerations.empty() ?
158+
0.0 : state_error.accelerations[joint_idx];
157159

158160
const bool is_valid =
159161
!(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) &&

‎joint_trajectory_controller/src/joint_trajectory_controller.cpp‎

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -138,12 +138,16 @@ JointTrajectoryController::update()
138138
error.positions[index] = angles::shortest_angular_distance(
139139
current.positions[index], desired.positions[index]);
140140
if (check_if_interface_type_exists(
141-
state_interface_types_, hardware_interface::HW_IF_VELOCITY))
141+
state_interface_types_, hardware_interface::HW_IF_VELOCITY) &&
142+
check_if_interface_type_exists(
143+
command_interface_types_, hardware_interface::HW_IF_VELOCITY))
142144
{
143145
error.velocities[index] = desired.velocities[index] - current.velocities[index];
144146
}
145147
if (check_if_interface_type_exists(
146-
state_interface_types_, hardware_interface::HW_IF_ACCELERATION))
148+
state_interface_types_, hardware_interface::HW_IF_ACCELERATION) &&
149+
check_if_interface_type_exists(
150+
command_interface_types_, hardware_interface::HW_IF_ACCELERATION))
147151
{
148152
error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
149153
}
@@ -241,7 +245,7 @@ JointTrajectoryController::update()
241245
{
242246
assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations);
243247
}
244-
// TODO(anyone): Add here "if using_closed_loop_hw_interface_adapter (see ROS')
248+
// TODO(anyone): Add here "if using_closed_loop_hw_interface_adapter (see ROS1)
245249
// if (check_if_interface_type_exist(
246250
// command_interface_types_, hardware_interface::HW_IF_EFFORT)) {
247251
// assign_interface_from_point(joint_command_interface_[3], state_desired.effort);
@@ -744,7 +748,9 @@ void JointTrajectoryController::publish_state(
744748
state_publisher_->msg_.desired.accelerations = desired_state.accelerations;
745749
state_publisher_->msg_.actual.positions = current_state.positions;
746750
state_publisher_->msg_.error.positions = state_error.positions;
747-
if (check_if_interface_type_exists(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) {
751+
if (check_if_interface_type_exists(
752+
state_interface_types_, hardware_interface::HW_IF_VELOCITY))
753+
{
748754
state_publisher_->msg_.actual.velocities = current_state.velocities;
749755
state_publisher_->msg_.error.velocities = state_error.velocities;
750756
}

‎joint_trajectory_controller/test/test_trajectory_controller.cpp‎

Lines changed: 27 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -120,28 +120,30 @@ TEST_P(TrajectoryControllerTestParameterized, configure) {
120120
executor.cancel();
121121
}
122122

123-
// TEST_P(TrajectoryControllerTestParameterized, activate) {
124-
// SetUpTrajectoryController();
125-
//
126-
// rclcpp::executors::MultiThreadedExecutor executor;
127-
// executor.add_node(traj_controller_->get_node()->get_node_base_interface());
128-
// const auto future_handle_ = std::async(std::launch::async, spin, &executor);
129-
//
130-
// traj_controller_->configure();
131-
// ASSERT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_INACTIVE);
132-
//
133-
// auto cmd_interface_config = traj_controller_->command_interface_configuration();
134-
// ASSERT_EQ(cmd_interface_config.names.size(), joint_names_.size()*command_interface_types_.size());
135-
//
136-
// auto state_interface_config = traj_controller_->state_interface_configuration();
137-
// ASSERT_EQ(state_interface_config.names.size(), joint_names_.size()*state_interface_types_.size());
138-
//
139-
// ActivateTrajectoryController();
140-
// ASSERT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_ACTIVE);
141-
//
142-
// executor.cancel();
143-
// std::cout << "Test end " << std::endl;
144-
// }
123+
TEST_P(TrajectoryControllerTestParameterized, activate) {
124+
SetUpTrajectoryController();
125+
126+
rclcpp::executors::MultiThreadedExecutor executor;
127+
executor.add_node(traj_controller_->get_node()->get_node_base_interface());
128+
129+
traj_controller_->configure();
130+
ASSERT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_INACTIVE);
131+
132+
auto cmd_interface_config = traj_controller_->command_interface_configuration();
133+
ASSERT_EQ(
134+
cmd_interface_config.names.size(),
135+
joint_names_.size() * command_interface_types_.size());
136+
137+
auto state_interface_config = traj_controller_->state_interface_configuration();
138+
ASSERT_EQ(
139+
state_interface_config.names.size(),
140+
joint_names_.size() * state_interface_types_.size());
141+
142+
ActivateTrajectoryController();
143+
ASSERT_EQ(traj_controller_->get_current_state().id(), State::PRIMARY_STATE_ACTIVE);
144+
145+
executor.cancel();
146+
}
145147

146148
// TODO(bmagyar): This seems to be implemented. Can we delete this?
147149
// TEST_F(TestTrajectoryController, activation) {
@@ -287,6 +289,8 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) {
287289
EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD);
288290
EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD);
289291
EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD);
292+
293+
executor.cancel();
290294
}
291295

292296
TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) {
@@ -356,6 +360,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
356360

357361
state = traj_controller_->configure();
358362
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
363+
359364
executor.cancel();
360365
}
361366

‎joint_trajectory_controller/test/test_trajectory_controller_utils.hpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include <memory>
1919
#include <string>
20+
#include <tuple>
2021
#include <utility>
2122
#include <vector>
2223

@@ -323,13 +324,13 @@ class TrajectoryControllerTest : public ::testing::Test
323324
ASSERT_TRUE(state_msg);
324325
for (size_t i = 0; i < expected_actual.positions.size(); ++i) {
325326
SCOPED_TRACE("Joint " + std::to_string(i));
326-
// TODO: add checking for velocties and accelerations
327+
// TODO(anyone): add checking for velocties and accelerations
327328
EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta);
328329
}
329330

330331
for (size_t i = 0; i < expected_desired.positions.size(); ++i) {
331332
SCOPED_TRACE("Joint " + std::to_string(i));
332-
// TODO: add checking for velocties and accelerations
333+
// TODO(anyone): add checking for velocties and accelerations
333334
EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta);
334335
}
335336
}

0 commit comments

Comments
 (0)