validate multi-dof trajectories before execution #713
Conversation
|
I will give it a run early next week |
rhaschke
force-pushed
the
validate-multi-dof-traj
branch
from
3529272
to
33afea5
Compare
|
sorry rhaschke I have check this and I thought I had replied. |
| { | ||
| // There is nothing to check | ||
| continue; |
There was a problem hiding this comment.
i prefer this style of breaking early (in this case with continue) rather than wrap all the functionality with increasing depths of indentation. why did you change it? it makes the PR harder to review
There was a problem hiding this comment.
Having both, single-dof and multi-dof joint trajectories, we cannot simply break out anymore. We have to check for both trajectory types.
| const std::vector<geometry_msgs::Transform>& transforms = | ||
| trajectory.multi_dof_joint_trajectory.points.front().transforms; | ||
| const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names; | ||
| const std::size_t n = joint_names.size(); |
There was a problem hiding this comment.
don't use one letter variable names - how about num_joints? I think it would actually be cleaner to use joint_names.size() throughout
There was a problem hiding this comment.
It was called n before... Using joint_names.size() is an extra function call each time.
There was a problem hiding this comment.
It happens often that when we add new features or fixes to MoveIt we ask the contributor or assigned maintainer to touch up that part of the code. I also think that there's no need to put the size value into a temporary variable.
Calling a function like this won't have any measurable overhead. After a quick look at the code I could only find 2 occurences of this n being used.
The same temporary variable trick is done 2 times in this function, make sure however you decide to address this you handle both.
There was a problem hiding this comment.
I handled both of them ;-)
| ROS_ERROR_STREAM_NAMED("traj_execution", "Unknown joint in trajectory: " << joint_names[i]); | ||
| return false; | ||
| } | ||
|
|
There was a problem hiding this comment.
add comment to this section describing process being taken
| if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_) | ||
| { | ||
| ROS_ERROR_STREAM_NAMED("traj_execution", | ||
| "\nInvalid Trajectory: start point deviates from current robot state more than " |
There was a problem hiding this comment.
clarify this error is coming from the new Multi DOF Traj checks
| return false; | ||
| } | ||
|
|
||
| for (std::size_t i = 0; i < n; ++i) | ||
| for (std::size_t i = 0, end = joint_names.size(); i < end; ++i) |
There was a problem hiding this comment.
Where is end defined? Is this some C++ magic?
There was a problem hiding this comment.
Just right after i=0 in the for loop's init condition. No magic there ;-)
This is a follow up on #691, actually implementing the missing functionality.
@TroyCordie could you please test this patch on your setup?