Situation
I'm part of a larger team working with robots and ROS. My current project is to create easy-to-use libraries for navigation and other things from existing code in our repositories. Over time, many people in our lab have implemented the same function a bit differently and I want to unify and streamline these implementations in one single library.
Now, the library has to publish on and subscribe to multiple topics and therefore it requires a ros::NodeHandle to work with. So far my library is purely static (i.e. no classes are instantiated, static members are called directly). This has the advantage, that my library has global awareness and I can prevent many user/robot/real-time errors. Furthermore, subscribing to topics takes a bit of time and messages can be missed. This is another argument for static members, since I can have a static subscriber and initialize it when the node is created.
Problem
When I have a ros::NodeHandle as a static class member, it is initialized right on program-launch, before ros::init() can be called. This returns an error as soon as the rosnode using my library is launched - makes sense.
My Solution
I thought I could perform lazy-initialization of my static members, including subscribers, publishers and the nodehandle. In order to achieve this, all ros objects are static pointers, that I initialize right before first use. My classes all look something like this.
// Class used to actually send various commands to robot
class RobotHelper {
public:
static int sendCommand(...);
private:
static void initialize();
static void callback(const my_msgs::Telemetry &msg);
static bool initialized;
static std::unique_ptr<ros::NodeHandle> nh_ptr_;
static std::unique_ptr<ros::Publisher> cmd_pub_ptr_;
static std::unique_ptr<ros::Subscriber> telemetry_sub_ptr_;
};
int RobotHelper::sendCommand(...) {
// Lazy-initialize the RobotHelper
initialize();
// Uses telemetry and other things to make sure nothing dangeorus happens when
// command is sent
// Also uses telemetry to make sure the robot accepted the command, so
// we don't have to worry about that later
// ...
// ...
return 0;
}
void RobotHelper::initialize() {
if (initialized) {
// nothing to do, already initialized
return;
}
// Initialize all the static ros members
nh_ptr_ = std::unique_ptr<ros::NodeHandle>(new ros::NodeHandle);
// create subscriber
telemetry_sub_ptr_ = std::unique_ptr<ros::Subscriber>(new ros::Subscriber);
*telemetry_sub_ptr_ =
nh_ptr_->subscribe("telemetry", 10, &RobotHelper::callback);
while (ros::ok()) {
// Wait for crucial telemetry subscriber to receive messages
// NOTE: Here, time is wasted waiting. I don't want to do that too often
// ...
}
// Create publisher
some_pub_ptr_ = std::unique_ptr<ros::Publisher>(new ros::Publisher);
*some_pub_ptr_ = nh_ptr_->advertise<std_msgs::Bool>("commands", 10);
}
class PathNavigation : public NavigationBase {
public:
// For every navigation action I create its own object
PathNavigation(double x, double y, double z);
// Actually perform the navigation
// This has to be called periodically until navigation is complete.
// In between I could handle obstacle avoidance or perform other actions.
// Hence this should be a non-blocking function and called periodically
int execute(bool &navigation_completed);
private:
static std::unique_ptr<ros::NodeHandle> nh_ptr_;
static std::unique_ptr<ros::Publisher> some_pub_ptr_;
};
PathNavigation::PathNavigation(double x, double y, double z) {
// In the constructor I compute the trajectory for example, as this remains
// valid until the goal is reached.
}
int PathNavigation::execute() {
// Every class function needs to check the initialization
// since there is no knowing which function is going to be called first.
initialize();
// Publish commands like this
RobotHelper::sendCommand(...); // First call will initialize RobotHelper
return 0; // no errors
}
int main(int argc, char **argv) {
ros::init(argc, argv, "example");
// Create navigation
// The end-user does not need to know about RobotHelper. The navigation-
// objects take care of everything automatically.
std::queue<PathNavigation> navigation_queue;
PathNavigation waypoint1(1.0, 2.0, 0.0);
navigation_queue.push(waypoint1);
PathNavigation waypoint2(3.0, 3.0, 0.0);
navigation_queue.push(waypoint2);
// Execute navigation
bool nav_complete = false;
while (ros::ok() && !navigation_queue.empty()) {
navigation_queue.front().execute(nav_complete);
// Check if waypoint reached
if (nav_complete) {
// Proceed with next waypoint
navigation_queue.pop();
}
}
return 0;
}
This approach works fine, but I have the feeling I am wasting resources with the constant if(initialized) checks inside every single one of the library's functions. Is there a common approach to this problem of requiring a static node handle and other things?
EDIT: I expanded the example to make more sense.
Originally posted by potaahto on ROS Answers with karma: 3 on 2017-01-14
Post score: 0
Original comments
Comment by potaahto on 2017-01-15:
@gvdhoom :There are two reasons for the statics. First one is as you suspected, that I don't want to miss messages. So I initialize the subscribers once and wait to receive messages. If I do that for every new navigation action, I'm wasting time waiting for the new subscriber every time.
Comment by potaahto on 2017-01-15:
Global awareness might be the wrong term. Essentially I wanted to always know whether my subscribers are receiving messages after they have been initialized once, statically. In other words, I'm only using static to eliminate the time it would take to re-initialize my publishers and subscribers.
Comment by potaahto on 2017-01-15:
What did you mean by "exposing as action"? This sounds exactly like what I need. Would that be having a separate Node running for these actions and triggering them via service calls for example?
