Skip to content

Commit

Permalink
added some cpplint and clangformatting changes
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor committed Jul 9, 2023
1 parent 6e92e76 commit 7422843
Show file tree
Hide file tree
Showing 9 changed files with 50 additions and 35 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -68,11 +68,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase
bool is_in_chained_mode() const final;

CONTROLLER_INTERFACE_PUBLIC
virtual bool toggle_references_from_subscribers(bool enable) final;
bool toggle_references_from_subscribers(bool enable) final;

protected:
/// Virtual method that each chainable controller should implement to export its read-only chainable
/// interfaces.
/// Virtual method that each chainable controller should implement to export its read-only
/// chainable interfaces.
/**
* Each chainable controller implements this methods where all its state(read only) interfaces are
* exported. The method has the same meaning as `export_estimated_interfaces` method from
Expand All @@ -82,8 +82,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase
*/
virtual std::vector<hardware_interface::StateInterface> on_export_estimated_interfaces() = 0;

/// Virtual method that each chainable controller should implement to export its read/write chainable
/// interfaces.
/// Virtual method that each chainable controller should implement to export its read/write
/// chainable interfaces.
/**
* Each chainable controller implements this methods where all input (command) interfaces are
* exported. The method has the same meaning as `export_command_interface` method from
Expand Down Expand Up @@ -141,7 +141,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase
/// A flag marking if a chainable controller is currently preceded by another controller.
bool in_chained_mode_ = false;

/// A flag marking whether to use references from subscribers or from the interfaces as input commands
/// A flag marking whether to use references from subscribers or from the interfaces as input
/// commands
bool use_references_from_subscribers_ = false;
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
* @return false
*/
CONTROLLER_INTERFACE_PUBLIC
virtual bool toggle_references_from_subscribers(bool enable) final;
bool toggle_references_from_subscribers(bool enable) final;
};

using ControllerInterfaceSharedPtr = std::shared_ptr<ControllerInterface>;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
CONTROLLER_INTERFACE_PUBLIC
virtual bool is_in_chained_mode() const = 0;

/// A method to enable the useage of references from subscribers
/// A method to enable the usage of references from subscribers
/**
* Enable or disable the retrieval of references for a chainable controller. This methods helps
* to toggle between the usage of reference from the subscribers or the external interfaces to
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -389,7 +389,7 @@ class ControllerManager : public rclcpp::Node
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it);

///Sets if the parsed vector of controller's reference interfacs are available to use or not
/// Sets if the parsed vector of controller's reference interfacs are available to use or not
/**
* Sets the availability status of the reference interfaces of the given list of controllers
* @param[in] controllers list
Expand Down
15 changes: 9 additions & 6 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,9 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const

/// Checks if an interface belongs to a controller based on its prefix.
/**
* A State/Command interface can be provided by a controller in which case is called "estimated/reference"
* interface.
* This means that the @interface_name starts with the name of a controller.
* A State/Command interface can be provided by a controller in which case is called
* "estimated/reference" interface. This means that the @interface_name starts with the name of a
* controller.
*
* \param[in] interface_name to be found in the map.
* \param[in] controllers list of controllers to compare their names to interface's prefix.
Expand Down Expand Up @@ -2131,7 +2131,8 @@ void ControllerManager::propagate_deactivation_of_chained_mode(
ControllersListIterator following_ctrl_it;
if (is_interface_a_chained_interface(ctrl_itf_name, controllers, following_ctrl_it))
{
// If the preceding controller's estimated interfaces are in use by other controllers, then maintain the chained mode
// If the preceding controller's estimated interfaces are in use by other controllers,
// then maintain the chained mode
if (is_controller_estimated_interfaces_inuse_by_other_controllers(
following_ctrl_it->info.name, controllers, deactivate_request_))
{
Expand All @@ -2140,7 +2141,8 @@ void ControllerManager::propagate_deactivation_of_chained_mode(
to_use_references_from_subscribers_.end(), following_ctrl_it->info.name);
if (found_it == to_use_references_from_subscribers_.end())
{
// In this case we check if the interface is state only and then add to use references_from_subscribers list
// In this case we check if the interface is state only and then add to use
// references_from_subscribers list
to_use_references_from_subscribers_.push_back(following_ctrl_it->info.name);
RCLCPP_DEBUG(
get_logger(), "Adding controller '%s' in 'use references from subscriber' request.",
Expand Down Expand Up @@ -2391,7 +2393,8 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
// strictness ==
// controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN)
// {
// // insert to the begin of activate request list to be activated before preceding controller
// // insert to the begin of activate request list to be activated before preceding
// controller
// activate_request_.insert(activate_request_.begin(), preceding_ctrl_name);
// }
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,8 @@ controller_interface::return_type TestChainableController::update_and_write_comm
{
estimated_interfaces_data_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT;
}
// If there is no command interface and if there is a state interface then just forward the same value as in the state interface
// If there is no command interface and if there is a state interface then just forward the same
// value as in the state interface
for (size_t i = 0; i < estimated_interface_names_.size() && i < state_interfaces_.size() &&
command_interfaces_.empty();
++i)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ class TestChainableController : public controller_interface::ChainableController
void set_estimated_interface_names(const std::vector<std::string> & estimated_interface_names);

CONTROLLER_MANAGER_PUBLIC
void set_imu_sensor_name(const std::string &name);
void set_imu_sensor_name(const std::string & name);

size_t internal_counter;
controller_interface::InterfaceConfiguration cmd_iface_cfg_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -928,7 +928,8 @@ TEST_P(
ASSERT_TRUE(diff_drive_controller->is_in_chained_mode());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id());
// SensorFusionController continues to stay in the chained mode as it is still using the estimated interfaces
// SensorFusionController continues to stay in the chained mode as it is still using the estimated
// interfaces
ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id());
Expand All @@ -938,7 +939,8 @@ TEST_P(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
robot_localization_controller->get_state().id());

// Deactivate the robot localization controller and see that the sensor fusion controller is still active but not in the chained mode
// Deactivate the robot localization controller and see that the sensor fusion controller is still
// active but not in the chained mode
DeactivateAndCheckController(
robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 8u);
EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode());
Expand Down Expand Up @@ -972,7 +974,8 @@ TEST_P(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
robot_localization_controller->get_state().id());

// Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active but not in the chained mode
// Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active
// but not in the chained mode
DeactivateAndCheckController(sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 14u);
EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode());
EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode());
Expand All @@ -989,7 +992,8 @@ TEST_P(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
robot_localization_controller->get_state().id());

// Deactivate the diff_drive_controller as all it's following controllers that uses it's interfaces are deactivated
// Deactivate the diff_drive_controller as all it's following controllers that uses it's
// interfaces are deactivated
DeactivateAndCheckController(
diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 20u);
EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode());
Expand Down Expand Up @@ -1190,7 +1194,8 @@ TEST_P(
ActivateAndCheckController(robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 1u);
ActivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 1u);

// Verify that the other preceding controller is deactivated (diff_drive_controller_two) and other depending controllers are active
// Verify that the other preceding controller is deactivated (diff_drive_controller_two) and other
// depending controllers are active
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
diff_drive_controller_two->get_state().id());
Expand Down Expand Up @@ -1494,10 +1499,10 @@ TEST_P(
{controller_interface::return_type::OK, std::future_status::ready,
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}}};

// Test switch 'from chained mode' when controllers are deactivated and possible combination of disabling
// controllers that use reference/estimated interfaces of the other controller.
// This is important to check that deactivation is not trigger irrespective of the type (reference/estimated)
// interface that is shared among the other controllers
// Test switch 'from chained mode' when controllers are deactivated and possible combination of
// disabling controllers that use reference/estimated interfaces of the other controller. This is
// important to check that deactivation is not trigger irrespective of the type
// (reference/estimated) interface that is shared among the other controllers

// PositionController is deactivated --> DiffDrive controller still continues in chained mode
// As the DiffDriveController is in chained mode, right now we tend to also deactivate
Expand All @@ -1511,7 +1516,8 @@ TEST_P(
ASSERT_TRUE(sensor_fusion_controller->is_in_chained_mode());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id());
// SensorFusionController continues to stay in the chained mode as it is still using the estimated interfaces
// SensorFusionController continues to stay in the chained mode as it is still using the estimated
// interfaces
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id());
EXPECT_EQ(
Expand All @@ -1521,7 +1527,8 @@ TEST_P(
robot_localization_controller->get_state().id());

// DiffDrive (preceding) controller is activated --> PID controller in chained mod
// Let's try to deactivate the diff_drive_control, it should fail as there are still other controllers that use it's resources
// Let's try to deactivate the diff_drive_control, it should fail as there are still other
// controllers that use it's resources
DeactivateController(
DIFF_DRIVE_CONTROLLER, expected.at(test_param.strictness).return_type,
expected.at(test_param.strictness).future_status);
Expand All @@ -1542,8 +1549,8 @@ TEST_P(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
robot_localization_controller->get_state().id());

// Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the robot
// localization controller is still active
// Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the
// robot localization controller is still active
DeactivateAndCheckController(
sensor_fusion_controller, SENSOR_FUSION_CONTROLLER, {}, 0u, true,
expected.at(test_param.strictness).return_type,
Expand All @@ -1562,7 +1569,8 @@ TEST_P(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
robot_localization_controller->get_state().id());

// Deactivate the robot localization controller and see that the sensor fusion controller is still active but not in the chained mode
// Deactivate the robot localization controller and see that the sensor fusion controller is still
// active but not in the chained mode
DeactivateAndCheckController(
robot_localization_controller, ROBOT_LOCALIZATION_CONTROLLER, {}, 0u);
EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode());
Expand Down Expand Up @@ -1596,7 +1604,8 @@ TEST_P(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
robot_localization_controller->get_state().id());

// Deactivate the odometry publisher controller and now the diff_drive should continue active but not in chained mode
// Deactivate the odometry publisher controller and now the diff_drive should continue active but
// not in chained mode
DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 0u);
EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode());
EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode());
Expand All @@ -1613,7 +1622,8 @@ TEST_P(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
robot_localization_controller->get_state().id());

// Deactivate the diff_drive_controller as all it's following controllers that uses it's interfaces are deactivated
// Deactivate the diff_drive_controller as all it's following controllers that uses it's
// interfaces are deactivated
DeactivateAndCheckController(
diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 0u);
EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode());
Expand Down
4 changes: 2 additions & 2 deletions joint_limits_interface/test/joint_limits_urdf_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,10 @@

/// \author Adolfo Rodriguez Tsouroukdissian

#include <gtest/gtest.h>

#include <joint_limits_interface/joint_limits_urdf.hpp>

#include <gtest/gtest.h>

#include <string>

class JointLimitsUrdfTest : public ::testing::Test
Expand Down

0 comments on commit 7422843

Please sign in to comment.