Skip to content

Commit

Permalink
Rename read_state_from_hardware method
Browse files Browse the repository at this point in the history
Co-authored-by: Dr. Denis <[email protected]>
  • Loading branch information
christophfroehlich and destogl committed Nov 15, 2023
1 parent f1eaa49 commit 34ec02e
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -261,7 +261,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error);

void read_state_from_hardware(JointTrajectoryPoint & state);
void read_state_from_state_interfaces(JointTrajectoryPoint & state);

/** Assign values from the command interfaces as state.
* This is only possible if command AND state interfaces exist for the same type,
Expand Down
10 changes: 5 additions & 5 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,7 @@ controller_interface::return_type JointTrajectoryController::update(

// current state update
state_current_.time_from_start.set__sec(0);
read_state_from_hardware(state_current_);
read_state_from_state_interfaces(state_current_);

// currently carrying out a trajectory
if (has_active_trajectory())
Expand Down Expand Up @@ -397,7 +397,7 @@ controller_interface::return_type JointTrajectoryController::update(
return controller_interface::return_type::OK;
}

void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state)
void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state)
{
auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
Expand Down Expand Up @@ -965,9 +965,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
else
{
// Initialize current state storage from hardware
read_state_from_hardware(state_current_);
read_state_from_hardware(state_desired_);
read_state_from_hardware(last_commanded_state_);
read_state_from_state_interfaces(state_current_);
read_state_from_state_interfaces(state_desired_);
read_state_from_state_interfaces(last_commanded_state_);
}

// Should the controller start by holding position at the beginning of active state?
Expand Down

0 comments on commit 34ec02e

Please sign in to comment.