Skip to content

Small improvement in remapping #393

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

Merged
merged 2 commits into from
Jul 31, 2023
Merged

Small improvement in remapping #393

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e8b4eb7
Small improvement in remapping
Jul 27, 2022
0193629
Merge branch 'master' into andyz/small_jtc_realtime_improvement
bmagyar Jul 31, 2023
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
8 changes: 6 additions & 2 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 @@ -1136,8 +1136,12 @@ void JointTrajectoryController::sort_to_local_joint_order(
get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size());
return to_remap;
}
std::vector<double> output;
output.resize(mapping.size(), 0.0);
static std::vector<double> output(dof_, 0.0);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If it's safe to assume dof_ never changes, this can get simpler.

dof_ changes here:

controller_interface::CallbackReturn JointTrajectoryController::on_configure(
  const rclcpp_lifecycle::State &)
{
  const auto logger = get_node()->get_logger();

  // update parameters
  joint_names_ = get_node()->get_parameter("joints").as_string_array();
  if ((dof_ > 0) && (joint_names_.size() != dof_))
  {
    RCLCPP_ERROR(
      logger,
      "The JointTrajectoryController does not support restarting with a different number of DOF");
    // TODO(andyz): update vector lengths if num. joints did change and re-initialize them so we
    // can continue
    return CallbackReturn::FAILURE;
  }

  dof_ = joint_names_.size();

So it could hypothetically change but that would be really unusual

Copy link
Contributor Author

@AndyZe AndyZe Jul 27, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it doesn't make any sense to allow dof_ to change since the yaml config file contains a list of joint names to control. Example config file here: https://github.com/ros-planning/moveit_resources/blob/ros2/panda_moveit_config/config/ros2_controllers.yaml

Agree, @destogl or @bmagyar ?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes! Please also see #394

// Only resize if necessary since it's an expensive operation
if (output.size() != mapping.size())
{
output.resize(mapping.size(), 0.0);
}
for (size_t index = 0; index < mapping.size(); ++index)
{
auto map_index = mapping[index];
Expand Down