Skip to content

Commit

Permalink
[JTC] Renaming variables, reordering trajectory checks (#1032)
Browse files Browse the repository at this point in the history 
Co-authored-by: Christoph Froehlich <[email protected]>
Co-authored-by: Sai Kishor Kothakota <[email protected]>
  • Loading branch information
@destogl @christophfroehlich @saikishor
3 people authored Feb 16, 2025
1 parent d67c2f3 commit fc56126
Show file tree
Hide file tree
Showing 3 changed files with 67 additions and 58 deletions.
5 changes: 3 additions & 2 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

rclcpp::Time traj_time_;

// variables for storing internal data for open-loop control
trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
rclcpp::Time last_commanded_time_;
/// Specify interpolation method. Default to splines.
Expand Down Expand Up @@ -157,9 +158,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

rclcpp::Service<control_msgs::srv::QueryTrajectoryState>::SharedPtr query_state_srv_;

std::shared_ptr<Trajectory> traj_external_point_ptr_ = nullptr;
std::shared_ptr<Trajectory> current_trajectory_ = nullptr;
realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
traj_msg_external_point_ptr_;
new_trajectory_msg_;

std::shared_ptr<trajectory_msgs::msg::JointTrajectory> hold_position_msg_ptr_ = nullptr;

Expand Down
116 changes: 62 additions & 54 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -161,18 +161,18 @@ controller_interface::return_type JointTrajectoryController::update(
// don't update goal after we sampled the trajectory to avoid any racecondition
const auto active_goal = *rt_active_goal_.readFromRT();

// Check if a new external message has been received from nonRT threads
auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg();
auto new_external_msg = traj_msg_external_point_ptr_.readFromRT();
// Check if a new trajectory message has been received from Non-RT threads
const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg();
auto new_external_msg = new_trajectory_msg_.readFromRT();
// Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
if (
current_external_msg != *new_external_msg &&
current_trajectory_msg != *new_external_msg &&
(*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false)
{
fill_partial_goal(*new_external_msg);
sort_to_local_joint_order(*new_external_msg);
// TODO(denis): Add here integration of position and velocity
traj_external_point_ptr_->update(*new_external_msg);
current_trajectory_->update(*new_external_msg);
}

// current state update
Expand All @@ -185,21 +185,21 @@ controller_interface::return_type JointTrajectoryController::update(
bool first_sample = false;
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
// if sampling the first time, set the point before you sample
if (!traj_external_point_ptr_->is_sampled_already())
if (!current_trajectory_->is_sampled_already())
{
first_sample = true;
if (params_.open_loop_control)
{
if (last_commanded_time_.seconds() == 0.0)
if (std::abs(last_commanded_time_.seconds()) < std::numeric_limits<float>::epsilon())
{
last_commanded_time_ = time;
}
traj_external_point_ptr_->set_point_before_trajectory_msg(
current_trajectory_->set_point_before_trajectory_msg(
last_commanded_time_, last_commanded_state_, joints_angle_wraparound_);
}
else
{
traj_external_point_ptr_->set_point_before_trajectory_msg(
current_trajectory_->set_point_before_trajectory_msg(
time, state_current_, joints_angle_wraparound_);
}
traj_time_ = time;
Expand All @@ -210,17 +210,17 @@ controller_interface::return_type JointTrajectoryController::update(
}

// Sample expected state from the trajectory
traj_external_point_ptr_->sample(
current_trajectory_->sample(
traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);

// Sample setpoint for next control cycle
const bool valid_point = traj_external_point_ptr_->sample(
const bool valid_point = current_trajectory_->sample(
traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr,
end_segment_itr, false);

if (valid_point)
{
const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start();
const rclcpp::Time traj_start = current_trajectory_->time_from_start();
// this is the time instance
// - started with the first segment: when the first point will be reached (in the future)
// - later: when the point of the current segment was reached
Expand All @@ -232,7 +232,7 @@ controller_interface::return_type JointTrajectoryController::update(
bool tolerance_violated_while_moving = false;
bool outside_goal_tolerance = false;
bool within_goal_time = true;
const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end();
const bool before_last_point = end_segment_itr != current_trajectory_->end();
auto active_tol = active_tolerances_.readFromRT();

// have we reached the end, are not holding position, and is a timeout configured?
Expand All @@ -243,8 +243,8 @@ controller_interface::return_type JointTrajectoryController::update(
{
RCLCPP_WARN(logger, "Aborted due to command timeout");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
new_trajectory_msg_.reset();
new_trajectory_msg_.initRT(set_hold_position());
}

// Check state/goal tolerance
Expand Down Expand Up @@ -324,7 +324,7 @@ controller_interface::return_type JointTrajectoryController::update(
assign_interface_from_point(joint_command_interface_[3], tmp_command_);
}

// store the previous command. Used in open-loop control mode
// store the previous command and time used in open-loop control mode
last_commanded_state_ = command_next_;
last_commanded_time_ = time;
}
Expand Down Expand Up @@ -355,8 +355,8 @@ controller_interface::return_type JointTrajectoryController::update(

RCLCPP_WARN(logger, "Aborted due to state tolerance violation");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
new_trajectory_msg_.reset();
new_trajectory_msg_.initRT(set_hold_position());
}
// check goal tolerance
else if (!before_last_point)
Expand All @@ -374,8 +374,8 @@ controller_interface::return_type JointTrajectoryController::update(

RCLCPP_INFO(logger, "Goal reached, success!");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_success_trajectory_point());
new_trajectory_msg_.reset();
new_trajectory_msg_.initRT(set_success_trajectory_point());
}
else if (!within_goal_time)
{
Expand All @@ -393,8 +393,8 @@ controller_interface::return_type JointTrajectoryController::update(

RCLCPP_WARN(logger, "%s", error_string.c_str());

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
new_trajectory_msg_.reset();
new_trajectory_msg_.initRT(set_hold_position());
}
}
}
Expand All @@ -403,16 +403,16 @@ controller_interface::return_type JointTrajectoryController::update(
// we need to ensure that there is no pending goal -> we get a race condition otherwise
RCLCPP_ERROR(logger, "Holding position due to state tolerance violation");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
new_trajectory_msg_.reset();
new_trajectory_msg_.initRT(set_hold_position());
}
else if (
!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false)
{
RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position...");

traj_msg_external_point_ptr_.reset();
traj_msg_external_point_ptr_.initRT(set_hold_position());
new_trajectory_msg_.reset();
new_trajectory_msg_.initRT(set_hold_position());
}
// else, run another cycle while waiting for outside_goal_tolerance
// to be satisfied (will stay in this state until new message arrives)
Expand Down Expand Up @@ -622,13 +622,13 @@ void JointTrajectoryController::query_state_service(
if (has_active_trajectory())
{
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
response->success = traj_external_point_ptr_->sample(
response->success = current_trajectory_->sample(
static_cast<rclcpp::Time>(request->time), interpolation_method_, state_requested,
start_segment_itr, end_segment_itr);
// If the requested sample time precedes the trajectory finish time respond as failure
if (response->success)
{
if (end_segment_itr == traj_external_point_ptr_->end())
if (end_segment_itr == current_trajectory_->end())
{
RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time.");
response->success = false;
Expand Down Expand Up @@ -932,9 +932,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
}
}

traj_external_point_ptr_ = std::make_shared<Trajectory>();
traj_msg_external_point_ptr_.writeFromNonRT(
std::shared_ptr<trajectory_msgs::msg::JointTrajectory>());
current_trajectory_ = std::make_shared<Trajectory>();
new_trajectory_msg_.writeFromNonRT(std::shared_ptr<trajectory_msgs::msg::JointTrajectory>());

subscriber_is_active_ = true;

Expand Down Expand Up @@ -1033,7 +1032,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(

subscriber_is_active_ = false;

traj_external_point_ptr_.reset();
current_trajectory_.reset();

return CallbackReturn::SUCCESS;
}
Expand Down Expand Up @@ -1061,7 +1060,7 @@ bool JointTrajectoryController::reset()
}
}

traj_external_point_ptr_.reset();
current_trajectory_.reset();

return true;
}
Expand Down Expand Up @@ -1354,6 +1353,7 @@ bool JointTrajectoryController::validate_trajectory_point_field(
bool JointTrajectoryController::validate_trajectory_msg(
const trajectory_msgs::msg::JointTrajectory & trajectory) const
{
// CHECK: Partial joint goals
// If partial joints goals are not allowed, goal should specify all controller joints
if (!params_.allow_partial_joints_goal)
{
Expand All @@ -1366,36 +1366,21 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}

// CHECK: if joint names are provided
if (trajectory.joint_names.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory.");
return false;
}

// CHECK: if provided trajectory has points
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 const trajectory_end_time =
trajectory_start_time + trajectory.points.back().time_from_start;
if (trajectory_end_time < get_node()->now())
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Received trajectory with non-zero start time (%f) that ends in the past (%f)",
trajectory_start_time.seconds(), trajectory_end_time.seconds());
return false;
}
}

// CHECK: If joint names are matching the joints defined for the controller
for (size_t i = 0; i < trajectory.joint_names.size(); ++i)
{
const std::string & incoming_joint_name = trajectory.joint_names[i];
Expand All @@ -1410,6 +1395,7 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}

// CHECK: if trajectory ends with non-zero velocity (when option is disabled)
if (!params_.allow_nonzero_velocity_at_trajectory_end)
{
for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
Expand All @@ -1425,9 +1411,29 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}

// CHECK: if trajectory end time is in the past (if start time defined)
const rclcpp::Time trajectory_start_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 const trajectory_end_time =
trajectory_start_time + trajectory.points.back().time_from_start;
if (trajectory_end_time < get_node()->now())
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Received trajectory with non-zero start time (%f) that ends in the past (%f)",
trajectory_start_time.seconds(), trajectory_end_time.seconds());
return false;
}
}

rclcpp::Duration previous_traj_time(0ms);
for (size_t i = 0; i < trajectory.points.size(); ++i)
{
// CHECK: if time of points in the trajectory is monotonous
if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time))
{
RCLCPP_ERROR(
Expand All @@ -1441,6 +1447,8 @@ bool JointTrajectoryController::validate_trajectory_msg(

const size_t joint_count = trajectory.joint_names.size();
const auto & points = trajectory.points;

// CHECK: if all required data are provided in the trajectory
// This currently supports only position, velocity and acceleration inputs
if (params_.allow_integration_in_goal_trajectories)
{
Expand Down Expand Up @@ -1490,7 +1498,7 @@ bool JointTrajectoryController::validate_trajectory_msg(
void JointTrajectoryController::add_new_trajectory_msg(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)
{
traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);
new_trajectory_msg_.writeFromNonRT(traj_msg);
}

void JointTrajectoryController::preempt_active_goal()
Expand Down Expand Up @@ -1523,7 +1531,7 @@ JointTrajectoryController::set_success_trajectory_point()
{
// set last command to be repeated at success, no matter if it has nonzero velocity or
// acceleration
hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back();
hold_position_msg_ptr_->points[0] = current_trajectory_->get_trajectory_msg()->points.back();
hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0);

// set flag, otherwise tolerances will be checked with success_trajectory_point too
Expand Down Expand Up @@ -1576,7 +1584,7 @@ void JointTrajectoryController::resize_joint_trajectory_point_command(

bool JointTrajectoryController::has_active_trajectory() const
{
return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg();
return current_trajectory_ != nullptr && current_trajectory_->has_trajectory_msg();
}

void JointTrajectoryController::update_pids()
Expand Down
4 changes: 2 additions & 2 deletions joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,12 +185,12 @@ class TestableJointTrajectoryController

bool has_trivial_traj() const
{
return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false;
return has_active_trajectory() && current_trajectory_->has_nontrivial_msg() == false;
}

bool has_nontrivial_traj()
{
return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg();
return has_active_trajectory() && current_trajectory_->has_nontrivial_msg();
}

double get_cmd_timeout() { return cmd_timeout_; }
Expand Down

0 comments on commit fc56126

Please sign in to comment.