Skip to content

Commit

Permalink
JTC trajectory end time validation fix (#1090)
Browse files Browse the repository at this point in the history
(cherry picked from commit b51e4c2)
  • Loading branch information
henrygerardmoore authored and christophfroehlich committed May 22, 2024
1 parent 12a2ef2 commit fe85da4
Show file tree
Hide file tree
Showing 2 changed files with 19 additions and 11 deletions.
19 changes: 8 additions & 11 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1361,17 +1361,20 @@ bool JointTrajectoryController::validate_trajectory_msg(
return false;
}

if (trajectory.points.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received.");
return false;
}

const auto trajectory_start_time = static_cast<rclcpp::Time>(trajectory.header.stamp);
// If the starting time it set to 0.0, it means the controller should start it now.
// Otherwise we check if the trajectory ends before the current time,
// in which case it can be ignored.
if (trajectory_start_time.seconds() != 0.0)
{
auto trajectory_end_time = trajectory_start_time;
for (const auto & p : trajectory.points)
{
trajectory_end_time += p.time_from_start;
}
auto const trajectory_end_time =
trajectory_start_time + trajectory.points.back().time_from_start;
if (trajectory_end_time < get_node()->now())
{
RCLCPP_ERROR(
Expand All @@ -1396,12 +1399,6 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}

if (trajectory.points.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received.");
return false;
}

if (!params_.allow_nonzero_velocity_at_trajectory_end)
{
for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
Expand Down
11 changes: 11 additions & 0 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1398,6 +1398,17 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message)
traj_msg = good_traj_msg;
traj_msg.points.push_back(traj_msg.points.front());
EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));

// End time in the past
traj_msg = good_traj_msg;
traj_msg.header.stamp = rclcpp::Time(1);
EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg));

// End time in the future
traj_msg = good_traj_msg;
traj_msg.header.stamp = traj_controller_->get_node()->now();
traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(10);
EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg));
}

/**
Expand Down

0 comments on commit fe85da4

Please sign in to comment.