nodelet.cpp
Go to the documentation of this file.
1 // tf
2 // nodelets
3 
4 #include <nodelet/nodelet.h>
6 
9 
10 #include <thread>
11 #include <atomic>
12 #include <vector>
13 #include <set>
14 #include <memory>
15 #include <functional>
16 
17 namespace astra_ros
18 {
19  class Nodelet : public nodelet::Nodelet
20  {
21  public:
22  void onInit()
23  {
26 
27  // Get all parameter keys from the parameter server (I didn't see a way to only get a namespace)
28  std::vector<std::string> param_names;
29  if (!pnh.getParamNames(param_names))
30  {
31  NODELET_ERROR("NodeHandle::getParamNames() failed. Aborting.");
32  return;
33  }
34 
35  // Extract the device names from the parameter key list
36  std::set<std::string> devices;
37 
38  const std::string prefix = pnh.getNamespace() + "/" + DEVICE_NAMESPACE;
39  for (const auto &param : param_names)
40  {
41  if (param.find(prefix) != 0) continue;
42 
43  std::string::size_type next_slash = param.find("/", prefix.size() + 1);
44  if (next_slash == std::string::npos) next_slash = param.size();
45  const std::string device = param.substr(prefix.size() + 1, next_slash - prefix.size() - 1);
46  devices.insert(device);
47  }
48 
49  // Construct the ROS representations of the devices
50  for (const auto &device : devices)
51  {
52  ros_devices_.emplace_back(device, nh, pnh);
53  }
54 
55  running_ = true;
56  thread_ = std::make_unique<std::thread>(std::bind(&Nodelet::run, this));
57  }
58 
60  {
61  running_ = false;
62  thread_->join();
63  }
64 
65  private:
66  void run()
67  {
68  // Finally, start streaming
69  ros::Rate rate(35.0);
70  while (running_)
71  {
72  for (auto &ros_device : ros_devices_)
73  {
74  try
75  {
76  ros_device.update();
77  }
78  catch (const Exception &exception)
79  {
80  NODELET_ERROR_STREAM("Failed to update device \"" << ros_device.getName() << "\": " << exception.what());
81  continue;
82  }
83  catch(const std::exception &exception)
84  {
85  NODELET_FATAL_STREAM("Unknown exception occurred while updating device \"" << ros_device.getName() << "\". Aborting: " << exception.what());
86  return;
87  }
88  }
89 
90  ros::spinOnce();
91  rate.sleep();
92  }
93  }
94 
95  std::atomic<bool> running_;
96  std::unique_ptr<std::thread> thread_;
97  std::vector<RosDevice> ros_devices_;
98  };
99 }
100 
astra_ros::Exception
Wraps a astra_status_t when an Astra SDK call fails.
Definition: Exception.hpp:14
NODELET_ERROR
#define NODELET_ERROR(...)
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
ros::spinOnce
ROSCPP_DECL void spinOnce()
astra_ros::Nodelet::thread_
std::unique_ptr< std::thread > thread_
Definition: nodelet.cpp:96
astra_ros::Nodelet::run
void run()
Definition: nodelet.cpp:66
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
class_list_macros.h
astra_ros::Exception::what
virtual const char * what() const noexcept
Definition: Exception.cpp:14
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(astra_ros::Nodelet, nodelet::Nodelet)
astra_ros::Nodelet::onInit
void onInit()
Definition: nodelet.cpp:22
NODELET_ERROR_STREAM
#define NODELET_ERROR_STREAM(...)
RosDevice.hpp
astra_ros::Nodelet::~Nodelet
~Nodelet()
Definition: nodelet.cpp:59
astra_ros::DEVICE_NAMESPACE
const std::string DEVICE_NAMESPACE
astra_ros::Nodelet
Definition: nodelet.cpp:19
ros::Rate::sleep
bool sleep()
astra_ros::Nodelet::running_
std::atomic< bool > running_
Definition: nodelet.cpp:95
nodelet::Nodelet
ros::NodeHandle::getParamNames
bool getParamNames(std::vector< std::string > &keys) const
Exception.hpp
nodelet.h
NODELET_FATAL_STREAM
#define NODELET_FATAL_STREAM(...)
param
T param(const std::string &param_name, const T &default_val)
ros::Rate
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
astra_ros::Nodelet::ros_devices_
std::vector< RosDevice > ros_devices_
Definition: nodelet.cpp:97
ros::NodeHandle
astra_ros
Definition: Device.hpp:14


astra_ros
Author(s): Braden McDorman
autogenerated on Wed Mar 2 2022 00:53:06