Skip to content

Commit

Permalink
re-run pre-commit with clang-format installed and updates CMakelist t…
Browse files Browse the repository at this point in the history
…o delete some tests
  • Loading branch information
flochre committed Aug 2, 2023
1 parent 1281b48 commit 7fa64fb
Show file tree
Hide file tree
Showing 3 changed files with 37 additions and 40 deletions.
10 changes: 0 additions & 10 deletions range_sensor_broadcaster/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -55,19 +55,9 @@ pluginlib_export_plugin_description_file(

if(BUILD_TESTING)
find_package(ament_cmake_gmock REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
find_package(controller_manager REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(ros2_control_test_assets REQUIRED)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()

add_rostest_with_parameters_gmock(test_range_sensor_broadcaster
test/test_range_sensor_broadcaster.cpp
Expand Down
25 changes: 17 additions & 8 deletions range_sensor_broadcaster/src/range_sensor_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,13 @@ namespace range_sensor_broadcaster
{
controller_interface::CallbackReturn RangeSensorBroadcaster::on_init()
{
try {
try
{
param_listener_ = std::make_shared<ParamListener>(get_node());
params_ = param_listener_->get_params();
} catch (const std::exception & e) {
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what());
return CallbackReturn::ERROR;
Expand All @@ -41,24 +44,29 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
params_ = param_listener_->get_params();
if (params_.sensor_name.empty()) {
if (params_.sensor_name.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified.");
return CallbackReturn::ERROR;
}

if (params_.frame_id.empty()) {
if (params_.frame_id.empty())
{
RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided.");
return CallbackReturn::ERROR;
}

range_sensor_ = std::make_unique<semantic_components::RangeSensor>(
semantic_components::RangeSensor(params_.sensor_name));
try {
try
{
// register ft sensor data publisher
sensor_state_publisher_ =
get_node()->create_publisher<sensor_msgs::msg::Range>("~/range", rclcpp::SystemDefaultsQoS());
realtime_publisher_ = std::make_unique<StatePublisher>(sensor_state_publisher_);
} catch (const std::exception & e) {
}
catch (const std::exception & e)
{
fprintf(
stderr, "Exception thrown during publisher creation at configure stage with message : %s \n",
e.what());
Expand Down Expand Up @@ -87,7 +95,7 @@ RangeSensorBroadcaster::command_interface_configuration() const
}

controller_interface::InterfaceConfiguration RangeSensorBroadcaster::state_interface_configuration()
const
const
{
controller_interface::InterfaceConfiguration state_interfaces_config;
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
Expand All @@ -112,7 +120,8 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_deactivate(
controller_interface::return_type RangeSensorBroadcaster::update(
const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
{
if (realtime_publisher_ && realtime_publisher_->trylock()) {
if (realtime_publisher_ && realtime_publisher_->trylock())
{
realtime_publisher_->msg_.header.stamp = time;
range_sensor_->get_values_as_message(realtime_publisher_->msg_);
realtime_publisher_->unlockAndPublish();
Expand Down
42 changes: 20 additions & 22 deletions range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,7 @@ void RangeSensorBroadcasterTest::SetUp()
range_broadcaster_ = std::make_unique<range_sensor_broadcaster::RangeSensorBroadcaster>();
}

void RangeSensorBroadcasterTest::TearDown()
{
range_broadcaster_.reset(nullptr);
}
void RangeSensorBroadcasterTest::TearDown() { range_broadcaster_.reset(nullptr); }

controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster(
std::string broadcaster_name)
Expand All @@ -36,8 +33,8 @@ controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster(
// result = range_broadcaster_->init("test_range_sensor_broadcaster");
result = range_broadcaster_->init(broadcaster_name);

if (controller_interface::return_type::OK == result) {

if (controller_interface::return_type::OK == result)
{
std::vector<hardware_interface::LoanedStateInterface> state_interfaces;
state_interfaces.emplace_back(range_);

Expand All @@ -51,7 +48,8 @@ controller_interface::CallbackReturn RangeSensorBroadcasterTest::configure_broad
std::vector<rclcpp::Parameter> & parameters)
{
// Configure the broadcaster
for (auto parameter : parameters) {
for (auto parameter : parameters)
{
range_broadcaster_->get_node()->set_parameter(parameter);
}

Expand All @@ -71,15 +69,17 @@ void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Ran
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription(subscription);
while (max_sub_check_loop_count--) {
while (max_sub_check_loop_count--)
{
range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
// check if message has been received
if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) {
if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
{
break;
}
}
ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
"controller/broadcaster update loop";
"controller/broadcaster update loop";

// take message from subscription
rclcpp::MessageInfo msg_info;
Expand All @@ -95,8 +95,7 @@ TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Exception)
TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Success)
{
ASSERT_EQ(
init_broadcaster("test_range_sensor_broadcaster"),
controller_interface::return_type::OK);
init_broadcaster("test_range_sensor_broadcaster"), controller_interface::return_type::OK);
}

TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Error_1)
Expand Down Expand Up @@ -127,8 +126,8 @@ TEST_F(RangeSensorBroadcasterTest, Configure_RangeBroadcaster_Success)
init_broadcaster("test_range_sensor_broadcaster");

ASSERT_EQ(
range_broadcaster_->on_configure(
rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS);
range_broadcaster_->on_configure(rclcpp_lifecycle::State()),
controller_interface::CallbackReturn::SUCCESS);
}

TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success)
Expand All @@ -138,8 +137,8 @@ TEST_F(RangeSensorBroadcasterTest, Activate_RangeBroadcaster_Success)
range_broadcaster_->on_configure(rclcpp_lifecycle::State());

ASSERT_EQ(
range_broadcaster_->on_activate(
rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS);
range_broadcaster_->on_activate(rclcpp_lifecycle::State()),
controller_interface::CallbackReturn::SUCCESS);
}

TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success)
Expand All @@ -148,16 +147,15 @@ TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success)

range_broadcaster_->on_configure(rclcpp_lifecycle::State());
ASSERT_EQ(
range_broadcaster_->on_activate(
rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS);
range_broadcaster_->on_activate(rclcpp_lifecycle::State()),
controller_interface::CallbackReturn::SUCCESS);
// range_broadcaster_->get_node()->get_clock()->now();
// auto result = range_broadcaster_->update(rclcpp::Time(0.0), rclcpp::Duration::from_seconds(0.01));
// auto result = range_broadcaster_->update(rclcpp::Time(0.0),
// rclcpp::Duration::from_seconds(0.01));
auto result = range_broadcaster_->update(
range_broadcaster_->get_node()->get_clock()->now(), rclcpp::Duration::from_seconds(0.01));

ASSERT_EQ(
result,
controller_interface::return_type::OK);
ASSERT_EQ(result, controller_interface::return_type::OK);
}

TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success)
Expand Down

0 comments on commit 7fa64fb

Please sign in to comment.