Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[JTC] Renaming variables, reordering trajectory checks and fixing open-loop mode by adding last commanded time. #1032

Draft
wants to merge 7 commits into
base: master
Choose a base branch
from
Draft

[JTC] Renaming variables, reordering trajectory checks and fixing open-loop mode by adding last commanded time. #1032

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
8b8a373
Increasing code readability in JTC
destogl Feb 9, 2024
b578e59
Fixup last_commanded_time.
destogl Feb 9, 2024
c64264d
Merge remote-tracking branch 'origin/master' into cleaning-jtc
christophfroehlich Feb 12, 2024
ca31d58
Variable renaming in the test utils
christophfroehlich Feb 12, 2024
8e577c4
Merge branch 'master' into cleaning-jtc
bmagyar Feb 19, 2024
3ba1136
Merge branch 'master' into cleaning-jtc
christophfroehlich Nov 2, 2024
b1b8161
Fix merge conflict
christophfroehlich Nov 2, 2024
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
7 changes: 5 additions & 2 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
std::shared_ptr<ParamListener> param_listener_;
Params params_;

// 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.
interpolation_methods::InterpolationMethod interpolation_method_{
interpolation_methods::DEFAULT_INTERPOLATION};
Expand Down Expand Up @@ -171,9 +174,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
131 changes: 73 additions & 58 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,18 +125,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 @@ -148,27 +148,32 @@ controller_interface::return_type JointTrajectoryController::update(
{
bool first_sample = false;
// 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)
{
traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_);
if (fabs(last_commanded_time_.seconds()) < std::numeric_limits<float>::epsilon())
{
last_commanded_time_ = time;
}
current_trajectory_->set_point_before_trajectory_msg(
last_commanded_time_, last_commanded_state_);
}
else
{
traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_);
current_trajectory_->set_point_before_trajectory_msg(time, state_current_);
}
}

// find segment for current timestamp
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
const bool valid_point = traj_external_point_ptr_->sample(
const bool valid_point = current_trajectory_->sample(
time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);

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 @@ -180,7 +185,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();

// have we reached the end, are not holding position, and is a timeout configured?
// Check independently of other tolerances
Expand All @@ -190,8 +195,8 @@ controller_interface::return_type JointTrajectoryController::update(
{
RCLCPP_WARN(get_node()->get_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 @@ -273,8 +278,9 @@ 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_ = state_desired_;
last_commanded_time_ = time;
}

if (active_goal)
Expand Down Expand Up @@ -303,8 +309,8 @@ controller_interface::return_type JointTrajectoryController::update(

RCLCPP_WARN(get_node()->get_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 @@ -322,8 +328,8 @@ controller_interface::return_type JointTrajectoryController::update(

RCLCPP_INFO(get_node()->get_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 @@ -341,8 +347,8 @@ controller_interface::return_type JointTrajectoryController::update(

RCLCPP_WARN(get_node()->get_logger(), 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 @@ -351,16 +357,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(get_node()->get_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(get_node()->get_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 @@ -572,13 +578,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 @@ -885,9 +891,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 All @@ -906,6 +911,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
read_state_from_state_interfaces(state_current_);
read_state_from_state_interfaces(last_commanded_state_);
}
last_commanded_time_ = rclcpp::Time(0.0);

// The controller should start by holding position at the beginning of active state
add_new_trajectory_msg(set_hold_position());
Expand Down Expand Up @@ -984,7 +990,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 @@ -1018,7 +1024,7 @@ bool JointTrajectoryController::reset()
}
}

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

return true;
}
Expand Down Expand Up @@ -1314,6 +1320,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 @@ -1326,33 +1333,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;
}

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)
// CHECK: if provided trajectory has points
if (trajectory.points.empty())
{
auto trajectory_end_time = trajectory_start_time;
for (const auto & p : trajectory.points)
{
trajectory_end_time += p.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_ERROR(get_node()->get_logger(), "Empty trajectory received.");
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 @@ -1367,12 +1362,7 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}

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

// 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 @@ -1388,9 +1378,32 @@ bool JointTrajectoryController::validate_trajectory_msg(
}
}

// CHECK: if trajectory end time is in the past (if start time defined)
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;
}
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 @@ -1404,6 +1417,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 @@ -1446,7 +1461,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 @@ -1479,7 +1494,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 @@ -1532,7 +1547,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
Loading