Skip to content

Commit

Permalink
Increasing code readability in JTC
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl committed Feb 9, 2024
1 parent 54b195f commit 34efa25
Show file tree
Hide file tree
Showing 3 changed files with 74 additions and 82 deletions.
6 changes: 4 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 @@ -131,8 +131,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 @@ -184,9 +186,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
7 changes: 3 additions & 4 deletions joint_trajectory_controller/src/cartesian_trajectory_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,7 +214,7 @@ void CartesianTrajectoryGenerator::reference_callback(
assign_value_from_input(
msg->transforms[0].rotation.z, msg->velocities[0].angular.z, params_.joints[5], 5);

add_new_trajectory_msg(new_traj_msg);
topic_callback(new_traj_msg);
}

controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate(
Expand Down Expand Up @@ -259,9 +259,8 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate(
// }
// }

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

subscriber_is_active_ = true;

Expand Down
143 changes: 67 additions & 76 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,31 +133,33 @@ 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
state_current_.time_from_start.set__sec(0);
if (!read_state_from_state_interfaces(state_current_))
{
return controller_interface::return_type::OK;
}

// currently carrying out a trajectory
if (has_active_trajectory())
{
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;

Expand Down Expand Up @@ -196,24 +198,24 @@ controller_interface::return_type JointTrajectoryController::update(
{
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_);
}
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, period,
joint_limiter_);

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 @@ -225,7 +227,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 @@ -235,8 +237,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 @@ -349,8 +351,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 @@ -368,8 +370,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 @@ -387,8 +389,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 @@ -397,16 +399,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 @@ -602,13 +604,13 @@ void JointTrajectoryController::query_state_service(
{
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
const rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01);
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, period, joint_limiter_);
// 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 @@ -953,9 +955,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 @@ -1054,7 +1055,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 @@ -1088,7 +1089,7 @@ bool JointTrajectoryController::reset()
}
}

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

return true;
}
Expand Down Expand Up @@ -1315,32 +1316,13 @@ void JointTrajectoryController::compute_error_for_joint(
// error defined as the difference between current and desired
if (joints_angle_wraparound_[index])
{
RCLCPP_WARN(
get_node()->get_logger(), "Calculating angular distance between sizes: %zu and desired: %zu.",
current.positions.size(), desired.positions.size());
RCLCPP_WARN(
get_node()->get_logger(), "Calculating angular distance between current: %.5f.",
current.positions[index]);
RCLCPP_WARN(
get_node()->get_logger(), "Calculating angular distance between desired: %.5f.",
desired.positions[index]);
// if desired, the shortest_angular_distance is calculated, i.e., the error is
// normalized between -pi<error<pi
error.positions[index] =
angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
}
else
{
RCLCPP_WARN(get_node()->get_logger(), "Calculating for index: %zu.", index);
RCLCPP_WARN(
get_node()->get_logger(), "Calculating angular distance between sizes: %zu and desired: %zu.",
current.positions.size(), desired.positions.size());
RCLCPP_WARN(
get_node()->get_logger(), "Calculating angular distance between current: %.5f.",
current.positions[index]);
RCLCPP_WARN(
get_node()->get_logger(), "Calculating angular distance between desired: %.5f.",
desired.positions[index]);
error.positions[index] = desired.positions[index] - current.positions[index];
}
if (
Expand Down Expand Up @@ -1488,6 +1470,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 @@ -1500,33 +1483,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 @@ -1541,12 +1512,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 @@ -1562,9 +1528,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 @@ -1578,6 +1567,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 @@ -1620,7 +1611,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 @@ -1653,7 +1644,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 @@ -1706,7 +1697,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

0 comments on commit 34efa25

Please sign in to comment.