Skip to content

Commit

Permalink
refactor: minor changes in fixing failure exit codes
Browse files Browse the repository at this point in the history
  • Loading branch information
boitumeloruf committed Dec 6, 2024
1 parent 8ef02f2 commit 5ab8f23
Show file tree
Hide file tree
Showing 11 changed files with 55 additions and 54 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -92,8 +92,9 @@ class CameraAravisNodeBase : public rclcpp::Node
/**
* @brief List available camera devices.
*
* @return Number of available devices.
*/
[[nodiscard]] bool listAvailableCameraDevices() const;
uint listAvailableCameraDevices() const;

protected:
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ class CameraXmlExporter : public CameraAravisNodeBase
*
* @return True if successful. False, otherwise.
*/
[[nodiscard]] bool export_xml_data_to_file();
[[nodiscard]] bool exportXmlDataToFile();

protected:
/**
Expand Down
29 changes: 15 additions & 14 deletions camera_aravis2/src/camera_aravis_node_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,30 +70,31 @@ bool CameraAravisNodeBase::isInitialized() const
}

//==================================================================================================
[[nodiscard]] bool CameraAravisNodeBase::listAvailableCameraDevices() const
uint CameraAravisNodeBase::listAvailableCameraDevices() const
{
//--- Discover available interfaces and devices.

arv_update_device_list();
auto n_interfaces = arv_get_n_interfaces();
auto n_devices = arv_get_n_devices();

if (n_devices == 0)
if (n_devices > 0)
{
RCLCPP_FATAL(logger_, "No cameras detected.");
return false;
RCLCPP_INFO(logger_, "Attached cameras (Num. Interfaces: %d | Num. Devices: %d):",
n_interfaces, n_devices);
for (uint i = 0; i < n_devices; i++)
{
RCLCPP_INFO(logger_, " Device %d: %s (%s)", i,
arv_get_device_id(i),
arv_get_device_address(i));
}
}

RCLCPP_INFO(logger_, "Attached cameras (Num. Interfaces: %d | Num. Devices: %d):",
n_interfaces, n_devices);
for (uint i = 0; i < n_devices; i++)
else
{
RCLCPP_INFO(logger_, " Device %d: %s (%s)", i,
arv_get_device_id(i),
arv_get_device_address(i));
RCLCPP_FATAL(logger_, "No cameras detected.");
}

return true;
return n_devices;
}

//==================================================================================================
Expand All @@ -115,8 +116,8 @@ void CameraAravisNodeBase::setupParameters()

//--- Discover available interfaces and devices.

bool is_successful = listAvailableCameraDevices();
if (!is_successful)
uint n_devices = listAvailableCameraDevices();
if (n_devices == 0)
return false;

//--- connect to camera specified by guid parameter
Expand Down
6 changes: 3 additions & 3 deletions camera_aravis2/src/camera_driver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1064,7 +1064,7 @@ void CameraDriver::handleMessageSubscriptionChange(rclcpp::MatchedInfo& iEventIn
//--- no subscriber so far, start acquisition
if (iEventInfo.current_count > 0 && current_num_subscribers_ == 0)
{
RCLCPP_INFO(logger_, "-> Acquisition start.");
RCLCPP_INFO(logger_, "|-> Acquisition start.");

arv_device_execute_command(p_device_, "AcquisitionStart", err.ref());
CHECK_GERROR_MSG(err, logger_, "In executing 'AcquisitionStart'.");
Expand All @@ -1073,7 +1073,7 @@ void CameraDriver::handleMessageSubscriptionChange(rclcpp::MatchedInfo& iEventIn
//--- subscribers until now, stop acquisition
else if (iEventInfo.current_count == 0 && current_num_subscribers_ > 0)
{
RCLCPP_INFO(logger_, "-> Acquisition stop.");
RCLCPP_INFO(logger_, "->| Acquisition stop.");

arv_device_execute_command(p_device_, "AcquisitionStop", err.ref());
CHECK_GERROR_MSG(err, logger_, "In executing 'AcquisitionStop'.");
Expand Down Expand Up @@ -1707,7 +1707,7 @@ void CameraDriver::spawnCameraStreams()
//--- When there are already subscribers to the image topic, start acquisition.
if (current_num_subscribers_ > 0)
{
RCLCPP_INFO(logger_, "-> Acquisition start at initialization.");
RCLCPP_INFO(logger_, "|-> Acquisition start at initialization.");

arv_device_execute_command(p_device_, "AcquisitionStart", err.ref());
CHECK_GERROR_MSG(err, logger_, "In executing 'AcquisitionStart'.");
Expand Down
2 changes: 0 additions & 2 deletions camera_aravis2/src/camera_driver_gv.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -118,8 +118,6 @@ CameraDriverGv::~CameraDriverGv()
//--- stop acquisition
if (p_device_)
{
RCLCPP_INFO(logger_, "-> Acquisition stop.");

arv_device_execute_command(p_device_, "AcquisitionStop", err.ref());
CHECK_GERROR_MSG(err, logger_, "In executing 'AcquisitionStop'.");
}
Expand Down
20 changes: 11 additions & 9 deletions camera_aravis2/src/camera_driver_gv_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,18 +37,20 @@ int main(int argc, char** argv)
rclcpp::init(argc, argv);

auto p_node = std::make_shared<camera_aravis2::CameraDriverGv>();
if (p_node->isSpawning() || p_node->isInitialized())

bool is_successful = false;
if (p_node)
{
rclcpp::spin(p_node->get_node_base_interface());
}
if (p_node->isSpawning() || p_node->isInitialized())
{
rclcpp::spin(p_node->get_node_base_interface());
}

//--- check if driver was initialized, i.e. if it has successfully spawned a camera stream
bool is_successful = p_node->isInitialized();
//--- check if driver was initialized, i.e. if it has successfully spawned a camera stream
is_successful = p_node->isInitialized();
}

rclcpp::shutdown();

if (is_successful)
return EXIT_SUCCESS;
else
return EXIT_FAILURE;
return is_successful ? EXIT_SUCCESS : EXIT_FAILURE;
}
2 changes: 0 additions & 2 deletions camera_aravis2/src/camera_driver_uv.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -110,8 +110,6 @@ CameraDriverUv::~CameraDriverUv()
//--- stop acquisition
if (p_device_)
{
RCLCPP_INFO(logger_, "-> Acquisition stop.");

arv_device_execute_command(p_device_, "AcquisitionStop", err.ref());
CHECK_GERROR_MSG(err, logger_, "In executing 'AcquisitionStop'.");
}
Expand Down
20 changes: 11 additions & 9 deletions camera_aravis2/src/camera_driver_uv_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,18 +37,20 @@ int main(int argc, char** argv)
rclcpp::init(argc, argv);

auto p_node = std::make_shared<camera_aravis2::CameraDriverUv>();
if (p_node->isSpawning() || p_node->isInitialized())

bool is_successful = false;
if (p_node)
{
rclcpp::spin(p_node->get_node_base_interface());
}
if (p_node->isSpawning() || p_node->isInitialized())
{
rclcpp::spin(p_node->get_node_base_interface());
}

//--- check if driver was initialized, i.e. if it has successfully spawned a camera stream
bool is_successful = p_node->isInitialized();
//--- check if driver was initialized, i.e. if it has successfully spawned a camera stream
is_successful = p_node->isInitialized();
}

rclcpp::shutdown();

if (is_successful)
return EXIT_SUCCESS;
else
return EXIT_FAILURE;
return is_successful ? EXIT_SUCCESS : EXIT_FAILURE;
}
11 changes: 5 additions & 6 deletions camera_aravis2/src/camera_finder_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,13 +36,12 @@ int main(int argc, char** argv)
{
rclcpp::init(argc, argv);

auto p_node = std::make_shared<camera_aravis2::CameraAravisNodeBase>("camera_finder");
bool is_successful = p_node->listAvailableCameraDevices();
auto p_node = std::make_shared<camera_aravis2::CameraAravisNodeBase>("camera_finder");

if (p_node)
p_node->listAvailableCameraDevices();

rclcpp::shutdown();

if (is_successful)
return EXIT_SUCCESS;
else
return EXIT_FAILURE;
return p_node ? EXIT_SUCCESS : EXIT_FAILURE;
}
2 changes: 1 addition & 1 deletion camera_aravis2/src/camera_xml_exporter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ CameraXmlExporter::~CameraXmlExporter()
}

//==================================================================================================
[[nodiscard]] bool CameraXmlExporter::export_xml_data_to_file()
[[nodiscard]] bool CameraXmlExporter::exportXmlDataToFile()
{
if (!is_initialized_)
{
Expand Down
12 changes: 6 additions & 6 deletions camera_aravis2/src/camera_xml_exporter_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,13 +36,13 @@ int main(int argc, char** argv)
{
rclcpp::init(argc, argv);

auto p_node = std::make_shared<camera_aravis2::CameraXmlExporter>();
bool is_successful = p_node->export_xml_data_to_file();
auto p_node = std::make_shared<camera_aravis2::CameraXmlExporter>();

bool is_successful = false;
if (p_node && p_node->isInitialized())
is_successful = p_node->exportXmlDataToFile();

rclcpp::shutdown();

if (is_successful)
return EXIT_SUCCESS;
else
return EXIT_FAILURE;
return is_successful ? EXIT_SUCCESS : EXIT_FAILURE;
}

0 comments on commit 5ab8f23

Please sign in to comment.