Skip to content

Commit

Permalink
enable ReflowComments to also use ColumnLimit on comments (ros-contro…
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor authored May 30, 2023
1 parent 1303920 commit ce60d2f
Show file tree
Hide file tree
Showing 12 changed files with 212 additions and 196 deletions.
2 changes: 1 addition & 1 deletion .clang-format
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,5 @@ ConstructorInitializerIndentWidth: 0
ContinuationIndentWidth: 2
DerivePointerAlignment: false
PointerAlignment: Middle
ReflowComments: false
ReflowComments: true
IncludeBlocks: Preserve
Original file line number Diff line number Diff line change
Expand Up @@ -166,20 +166,22 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
geometry_msgs::msg::Wrench ft_values_;

/**
* @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values
*/
* @brief Read values from hardware interfaces and set corresponding fields of state_current and
* ft_values
*/
void read_state_from_hardware(
trajectory_msgs::msg::JointTrajectoryPoint & state_current,
geometry_msgs::msg::Wrench & ft_values);

/**
* @brief Set fields of state_reference with values from controllers exported position and velocity references
*/
* @brief Set fields of state_reference with values from controllers exported position and
* velocity references
*/
void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state);

/**
* @brief Write values from state_command to claimed hardware interfaces
*/
* @brief Write values from state_command to claimed hardware interfaces
*/
void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command);
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,13 +43,17 @@ struct AdmittanceTransforms
{
// transformation from force torque sensor frame to base link frame at reference joint angles
Eigen::Isometry3d ref_base_ft_;
// transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles
// transformation from force torque sensor frame to base link frame at reference + admittance
// offset joint angles
Eigen::Isometry3d base_ft_;
// transformation from control frame to base link frame at reference + admittance offset joint angles
// transformation from control frame to base link frame at reference + admittance offset joint
// angles
Eigen::Isometry3d base_control_;
// transformation from end effector frame to base link frame at reference + admittance offset joint angles
// transformation from end effector frame to base link frame at reference + admittance offset
// joint angles
Eigen::Isometry3d base_tip_;
// transformation from center of gravity frame to base link frame at reference + admittance offset joint angles
// transformation from center of gravity frame to base link frame at reference + admittance offset
// joint angles
Eigen::Isometry3d base_cog_;
// transformation from world frame to base link frame
Eigen::Isometry3d world_base_;
Expand Down Expand Up @@ -111,20 +115,20 @@ class AdmittanceRule
controller_interface::return_type reset(const size_t num_joints);

/**
* Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does
* not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation
* are calculated without an error
* \param[in] current_joint_state current joint state of the robot
* \param[in] reference_joint_state input joint state reference
* \param[out] success true if no calls to the kinematics interface fail
* Calculate all transforms needed for admittance control using the loader kinematics plugin. If
* the transform does not exist in the kinematics model, then TF will be used for lookup. The
* return value is true if all transformation are calculated without an error \param[in]
* current_joint_state current joint state of the robot \param[in] reference_joint_state input
* joint state reference \param[out] success true if no calls to the kinematics interface fail
*/
bool get_all_transforms(
const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state);

/**
* Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field
* members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated
* Updates parameter_ struct if any parameters have changed since last update. Parameter dependent
* Eigen field members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes,
* damping_) are also updated
*/
void apply_parameters_update();

Expand All @@ -136,7 +140,8 @@ class AdmittanceRule
* \param[in] measured_wrench most recent measured wrench from force torque sensor
* \param[in] reference_joint_state input joint state reference
* \param[in] period time in seconds since last controller update
* \param[out] desired_joint_state joint state reference after the admittance offset is applied to the input reference
* \param[out] desired_joint_state joint state reference after the admittance offset is applied to
* the input reference
*/
controller_interface::return_type update(
const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
Expand All @@ -148,7 +153,8 @@ class AdmittanceRule
/**
* Set fields of `state_message` from current admittance controller state.
*
* \param[out] state_message message containing target position/vel/accel, wrench, and actual robot state, among other things
* \param[out] state_message message containing target position/vel/accel, wrench, and actual
* robot state, among other things
*/
const control_msgs::msg::AdmittanceControllerState & get_controller_state();

Expand All @@ -159,22 +165,21 @@ class AdmittanceRule

protected:
/**
* Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input
* is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin
* calls failed.
* \param[in] admittance_state contains all the information needed to calculate the admittance offset
* \param[in] dt controller period
* Calculates the admittance rule from given the robot's current joint angles. The admittance
* controller state input is updated with the new calculated values. A boolean value is returned
* indicating if any of the kinematics plugin calls failed. \param[in] admittance_state contains
* all the information needed to calculate the admittance offset \param[in] dt controller period
* \param[out] success true if no calls to the kinematics interface fail
*/
bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt);

/**
* Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`,
* the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`.
* The `wrench_world_` estimate includes gravity compensation
* \param[in] measured_wrench most recent measured wrench from force torque sensor
* \param[in] sensor_world_rot rotation matrix from world frame to sensor frame
* \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame
* Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement
* `measured_wrench`, the sensor to base frame rotation `sensor_world_rot`, and the center of
* gravity frame to base frame rotation `cog_world_rot`. The `wrench_world_` estimate includes
* gravity compensation \param[in] measured_wrench most recent measured wrench from force torque
* sensor \param[in] sensor_world_rot rotation matrix from world frame to sensor frame \param[in]
* cog_world_rot rotation matrix from world frame to center of gravity frame
*/
void process_wrench_measurements(
const geometry_msgs::msg::Wrench & measured_wrench,
Expand Down
12 changes: 6 additions & 6 deletions diff_drive_controller/test/test_diff_drive_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,12 +48,12 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr
}

/**
* @brief wait_for_twist block until a new twist is received.
* Requires that the executor is not spinned elsewhere between the
* message publication and the call to this function
*
* @return true if new twist msg was received, false if timeout
*/
* @brief wait_for_twist block until a new twist is received.
* Requires that the executor is not spinned elsewhere between the
* message publication and the call to this function
*
* @return true if new twist msg was received, false if timeout
*/
bool wait_for_twist(
rclcpp::Executor & executor,
const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500))
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -87,8 +87,8 @@ class ForwardControllersBase : public controller_interface::ControllerInterface
*
* It is expected that error handling of exceptions is done.
*
* \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and their values are
* allowed, controller_interface::CallbackReturn::ERROR otherwise.
* \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and
* their values are allowed, controller_interface::CallbackReturn::ERROR otherwise.
*/
virtual controller_interface::CallbackReturn read_parameters() = 0;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -123,8 +123,8 @@ SegmentTolerances get_segment_tolerances(Params const & params)
* \param state_error State error to check.
* \param joint_idx Joint index for the state error
* \param state_tolerance State tolerance of joint to check \p state_error against.
* \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true
* \return True if \p state_error fulfills \p state_tolerance.
* \param show_errors If the joint that violate its tolerance should be output to console. NOT
* REALTIME if true \return True if \p state_error fulfills \p state_tolerance.
*/
inline bool check_state_tolerance_per_joint(
const trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,26 +60,29 @@ class Trajectory
/// containing trajectory.
/**
* Sampling trajectory at given \p sample_time.
* If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or acceleration respectively.
* Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment.
* If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or
* acceleration respectively. Deduction assumes that the provided velocity or acceleration have to
* be reached at the time defined in the segment.
*
* Specific case returns for start_segment_itr and end_segment_itr:
* - Sampling before the trajectory start:
* start_segment_itr = begin(), end_segment_itr = begin()
* - Sampling exactly on a point of the trajectory:
* start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr
* start_segment_itr = iterator where point is, end_segment_itr = iterator after
* start_segment_itr
* - Sampling between points:
* start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr
* start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after
* start_segment_itr
* - Sampling after entire trajectory:
* start_segment_itr = --end(), end_segment_itr = end()
* - Sampling empty msg or before the time given in set_point_before_trajectory_msg()
* return false
*
* \param[in] sample_time Time at which trajectory will be sampled.
* \param[in] interpolation_method Specify whether splines, another method, or no interpolation at all.
* \param[out] expected_state Calculated new at \p sample_time.
* \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See description above.
* \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See description above.
* \param[in] interpolation_method Specify whether splines, another method, or no interpolation at
* all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr
* Iterator to the start segment for given \p sample_time. See description above. \param[out]
* end_segment_itr Iterator to the end segment for given \p sample_time. See description above.
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool sample(
Expand All @@ -91,14 +94,16 @@ class Trajectory
/**
* Do interpolation between 2 states given a time in between their respective timestamps
*
* The start and end states need not necessarily be specified all the way to the acceleration level:
* The start and end states need not necessarily be specified all the way to the acceleration
* level:
* - If only \b positions are specified, linear interpolation will be used.
* - If \b positions and \b velocities are specified, a cubic spline will be used.
* - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used.
* - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be
* used.
*
* If start and end states have different specifications
* (eg. start is position-only, end is position-velocity), the lowest common specification will be used
* (position-only in the example).
* (eg. start is position-only, end is position-velocity), the lowest common specification will be
* used (position-only in the example).
*
* \param[in] time_a Time at which the segment state equals \p state_a.
* \param[in] state_a State at \p time_a.
Expand Down Expand Up @@ -153,9 +158,9 @@ class Trajectory
};

/**
* \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 indices.
* If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated mapping vector is
* <tt>"{2, 1}"</tt>.
* \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2
* indices. If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated
* mapping vector is <tt>"{2, 1}"</tt>.
*/
template <class T>
inline std::vector<size_t> mapping(const T & t1, const T & t2)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -552,7 +552,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
const double EPS = 1e-6;
/**
* @brief check if position error of revolute joints are normalized if not configured so
*/
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
{
rclcpp::executors::MultiThreadedExecutor executor;
Expand Down Expand Up @@ -659,7 +659,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)

/**
* @brief check if position error of revolute joints are normalized if configured so
*/
*/
TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
{
rclcpp::executors::MultiThreadedExecutor executor;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,12 +61,12 @@ class TestableJointTrajectoryController
}

/**
* @brief wait_for_trajectory block until a new JointTrajectory is received.
* Requires that the executor is not spinned elsewhere between the
* message publication and the call to this function
*
* @return true if new JointTrajectory msg was received, false if timeout
*/
* @brief wait_for_trajectory block until a new JointTrajectory is received.
* Requires that the executor is not spinned elsewhere between the
* message publication and the call to this function
*
* @return true if new JointTrajectory msg was received, false if timeout
*/
bool wait_for_trajectory(
rclcpp::Executor & executor,
const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
Expand Down Expand Up @@ -293,7 +293,8 @@ class TrajectoryControllerTest : public ::testing::Test
/**
* delay_btwn_points - delay between each points
* points - vector of trajectories. One point per controlled joint
* joint_names - names of joints, if empty, will use joint_names_ up to the number of provided points
* joint_names - names of joints, if empty, will use joint_names_ up to the number of provided
* points
*/
void publish(
const builtin_interfaces::msg::Duration & delay_btwn_points,
Expand Down
Loading

0 comments on commit ce60d2f

Please sign in to comment.