Skip to content

Commit

Permalink
simple_charging_dock: fix the issue of possible mismatch in joint names
Browse files Browse the repository at this point in the history
  • Loading branch information
xianglunkai committed Jun 13, 2024
1 parent 3bfffab commit 30fcd32
Showing 1 changed file with 6 additions and 2 deletions.
8 changes: 6 additions & 2 deletions nav2_docking/opennav_docking/src/simple_charging_dock.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -284,19 +284,23 @@ void SimpleChargingDock::jointStateCallback(const sensor_msgs::msg::JointState::
{
double velocity = 0.0;
double effort = 0.0;
size_t match_count = 0;
for (size_t i = 0; i < state->name.size(); ++i) {
for (auto & name : stall_joint_names_) {
if (state->name[i] == name) {
// Tracking this joint
velocity += abs(state->velocity[i]);
effort += abs(state->effort[i]);
++match_count;
}
}
}

// Take average
effort /= stall_joint_names_.size();
velocity /= stall_joint_names_.size();
if (match_count > 0) {
effort /= stall_joint_names_.size();
velocity /= stall_joint_names_.size();
}

is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_);
}
Expand Down

0 comments on commit 30fcd32

Please sign in to comment.