From ce60d2f1858331172ea719f9671057d417be6241 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 30 May 2023 08:16:31 +0200 Subject: [PATCH] enable ReflowComments to also use ColumnLimit on comments (#625) --- .clang-format | 2 +- .../admittance_controller.hpp | 14 +- .../admittance_controller/admittance_rule.hpp | 55 +++-- .../test/test_diff_drive_controller.cpp | 12 +- .../forward_controllers_base.hpp | 4 +- .../tolerances.hpp | 4 +- .../trajectory.hpp | 35 +-- .../test/test_trajectory_controller.cpp | 4 +- .../test/test_trajectory_controller_utils.hpp | 15 +- .../steering_odometry.hpp | 217 +++++++++--------- .../src/steering_odometry.cpp | 34 +-- .../test/test_tricycle_controller.cpp | 12 +- 12 files changed, 212 insertions(+), 196 deletions(-) diff --git a/.clang-format b/.clang-format index ab8d077577..9e92cf780a 100644 --- a/.clang-format +++ b/.clang-format @@ -10,5 +10,5 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false +ReflowComments: true IncludeBlocks: Preserve diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index e4aef2cf3d..6ff6cdae7a 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -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); }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 0f0aabb063..36c027491c 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -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_; @@ -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(); @@ -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, @@ -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(); @@ -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, diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 236f34e9a2..adf4e79afc 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -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)) diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 3e153d7e2e..d60245f328 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -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; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index de8f060b1a..e540b06e51 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -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, diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 7dac4ddbe1..230d0198f7 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -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( @@ -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. @@ -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 "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is - * "{2, 1}". + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 + * indices. If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated + * mapping vector is "{2, 1}". */ template inline std::vector mapping(const T & t1, const T & t2) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 74c17dd03d..b1188eede7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -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; @@ -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; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c85eb59f58..8f5f64d57f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -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}) @@ -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, diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 002db32354..51cac3c9a9 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -33,197 +33,200 @@ const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; /** - * \brief The Odometry class handles odometry readings - * (2D pose and velocity with related timestamp) - */ + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ class SteeringOdometry { public: /** - * \brief Constructor - * Timestamp will get the current time value - * Value will be set to zero - * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean - * - */ + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + * + */ explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); /** - * \brief Initialize the odometry - * \param time Current time - */ + * \brief Initialize the odometry + * \param time Current time + */ void init(const rclcpp::Time & time); /** - * \brief Updates the odometry class with latest wheels position - * \param traction_wheel_pos traction wheel position [rad] - * \param steer_pos Front Steer position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_pos traction wheel position [rad] + * \param steer_pos Front Steer position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_position( const double traction_wheel_pos, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_pos Right traction wheel velocity [rad] - * \param left_traction_wheel_pos Left traction wheel velocity [rad] - * \param front_steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel velocity [rad] + * \param left_traction_wheel_pos Left traction wheel velocity [rad] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_position( const double right_traction_wheel_pos, const double left_traction_wheel_pos, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_pos Right traction wheel position [rad] - * \param left_traction_wheel_pos Left traction wheel position [rad] - * \param right_steer_pos Right steer wheel position [rad] - * \param left_steer_pos Left steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel position [rad] + * \param left_traction_wheel_pos Left traction wheel position [rad] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_position( const double right_traction_wheel_pos, const double left_traction_wheel_pos, const double right_steer_pos, const double left_steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param traction_wheel_vel Traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_vel Traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_velocity( const double traction_wheel_vel, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] - * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] - * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param right_steer_pos Right steer wheel position [rad] - * \param left_steer_pos Left steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double right_steer_pos, const double left_steer_pos, const double dt); /** - * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time - */ + * \brief Updates the odometry class with latest velocity command + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ void update_open_loop(const double linear, const double angular, const double dt); /** - * \brief Set odometry type - * \param type odometry type - */ + * \brief Set odometry type + * \param type odometry type + */ void set_odometry_type(const unsigned int type); /** - * \brief heading getter - * \return heading [rad] - */ + * \brief heading getter + * \return heading [rad] + */ double get_heading() const { return heading_; } /** - * \brief x position getter - * \return x position [m] - */ + * \brief x position getter + * \return x position [m] + */ double get_x() const { return x_; } /** - * \brief y position getter - * \return y position [m] - */ + * \brief y position getter + * \return y position [m] + */ double get_y() const { return y_; } /** - * \brief linear velocity getter - * \return linear velocity [m/s] - */ + * \brief linear velocity getter + * \return linear velocity [m/s] + */ double get_linear() const { return linear_; } /** - * \brief angular velocity getter - * \return angular velocity [rad/s] - */ + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ double get_angular() const { return angular_; } /** - * \brief Sets the wheel parameters: radius, separation and wheelbase - */ + * \brief Sets the wheel parameters: radius, separation and wheelbase + */ void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); /** - * \brief Velocity rolling window size setter - * \param velocity_rolling_window_size Velocity rolling window size - */ + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); /** - * \brief Calculates inverse kinematics for the desired linear and angular velocities - * \param Vx Desired linear velocity [m/s] - * \param theta_dot Desired angular velocity [rad/s] - * \return Tuple of velocity commands and steering commands - */ + * \brief Calculates inverse kinematics for the desired linear and angular velocities + * \param Vx Desired linear velocity [m/s] + * \param theta_dot Desired angular velocity [rad/s] + * \return Tuple of velocity commands and steering commands + */ std::tuple, std::vector> get_commands(double Vx, double theta_dot); /** - * \brief Reset poses, heading, and accumulators - */ + * \brief Reset poses, heading, and accumulators + */ void reset_odometry(); private: /** - * \brief Uses precomputed linear and angular velocities to compute dometry and update accumulators - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by previous odometry method - * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by previous odometry method - */ + * \brief Uses precomputed linear and angular velocities to compute dometry and update + * accumulators \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) + * computed by previous odometry method \param angular Angular velocity [rad] (angular + * displacement, i.e. m/s * dt) computed by previous odometry method + */ bool update_odometry(const double linear_velocity, const double angular, const double dt); /** - * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders - * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders - */ + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by + * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed + * by encoders + */ void integrate_runge_kutta_2(double linear, double angular); /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders - * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders - */ + * \brief Integrates the velocities (linear and angular) using exact method + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by + * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed + * by encoders + */ void integrate_exact(double linear, double angular); /** - * \brief Calculates steering angle from the desired translational and rotational velocity - * \param Vx Linear velocity [m] - * \param theta_dot Angular velocity [rad] - */ + * \brief Calculates steering angle from the desired translational and rotational velocity + * \param Vx Linear velocity [m] + * \param theta_dot Angular velocity [rad] + */ double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); /** - * \brief Reset linear and angular accumulators - */ + * \brief Reset linear and angular accumulators + */ void reset_accumulators(); /// Current timestamp: diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 28cd7fc80d..c480dc1253 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -1,17 +1,17 @@ /********************************************************************* -* Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. + * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. *********************************************************************/ /* @@ -303,10 +303,10 @@ void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) } /** - * \brief Other possible integration method provided by the class - * \param linear - * \param angular - */ + * \brief Other possible integration method provided by the class + * \param linear + * \param angular + */ void SteeringOdometry::integrate_exact(double linear, double angular) { if (fabs(angular) < 1e-6) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index a1f09ddaf8..1d6edf261a 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -52,12 +52,12 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle } /** - * @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))