Skip to content

Commit

Permalink
Code review
Browse files Browse the repository at this point in the history
  • Loading branch information
AndyZe committed Oct 24, 2023
1 parent adc2968 commit d8e47de
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -73,14 +73,18 @@ class SmoothingBaseClass

/**
* Smooth an array of joint position deltas
* @param position_vector array of joint position commands
* @param positiona array of joint position commands
* @param velocities array of joint velocity commands
* @param accelerations array of joint acceleration commands
* @return True if initialization was successful
*/
virtual bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) = 0;

/**
* Reset to a given joint state
* @param joint_positions reset the filters to these joint positions
* @param positions reset the filters to these joint positions
* @param velocities reset the filters to these joint velocities (if applicable)
* @param accelerations reset the filters to these joint accelerations (if applicable)
* @return True if reset was successful
*/
virtual bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
Expand Down
10 changes: 5 additions & 5 deletions moveit_ros/moveit_servo/src/utils/common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -135,21 +135,21 @@ trajectory_msgs::msg::JointTrajectory composeTrajectoryMessage(const servo::Para

if (servo_params.publish_joint_positions)
{
for (size_t i = 0; i < num_joints; i++)
for (size_t i = 0; i < num_joints; ++i)
{
point.positions[i] = joint_state.positions[i];
}
}
if (servo_params.publish_joint_velocities)
{
for (size_t i = 0; i < num_joints; i++)
for (size_t i = 0; i < num_joints; ++i)
{
point.velocities[i] = joint_state.velocities[i];
}
}
if (servo_params.publish_joint_accelerations)
{
for (size_t i = 0; i < num_joints; i++)
for (size_t i = 0; i < num_joints; ++i)
{
point.accelerations[i] = joint_state.accelerations[i];
}
Expand All @@ -167,14 +167,14 @@ std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& s
multi_array.data.resize(num_joints);
if (servo_params.publish_joint_positions)
{
for (size_t i = 0; i < num_joints; i++)
for (size_t i = 0; i < num_joints; ++i)
{
multi_array.data[i] = joint_state.positions[i];
}
}
else if (servo_params.publish_joint_velocities)
{
for (size_t i = 0; i < num_joints; i++)
for (size_t i = 0; i < num_joints; ++i)
{
multi_array.data[i] = joint_state.velocities[i];
}
Expand Down

0 comments on commit d8e47de

Please sign in to comment.