Skip to content

Commit

Permalink
Update comments
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich committed Oct 24, 2023
1 parent e416ef6 commit 608bab3
Show file tree
Hide file tree
Showing 2 changed files with 4 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -1028,6 +1028,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
// those are not nan
trajectory_msgs::msg::JointTrajectoryPoint state;
resize_joint_trajectory_point(state, dof_);
// read from cmd joints only if all joints have command interface
// otherwise it leaves the entries of joints without command interface NaN.
// if no open_loop control, state_current_ is then used for `set_point_before_trajectory_msg` and
// future trajectory sampling will always give NaN for these joints
if (dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state))
{
state_current_ = state;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -398,7 +398,6 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
/**
* @brief check if state topic is consistent with parameters
*/

TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
{
rclcpp::executors::SingleThreadedExecutor executor;
Expand Down Expand Up @@ -499,7 +498,6 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
/**
* @brief same as state_topic_consistency but with #command-joints < #dof
*/

TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof)
{
rclcpp::executors::SingleThreadedExecutor executor;
Expand Down

0 comments on commit 608bab3

Please sign in to comment.