Working with C++ Objects embedding ROS ServiceServer
Extra caution should be taken when working with ROS Objects and std:vector. Here is my own experience with ros::ServiceServer. The main concern is that there can be only one service server exist for a certain service name (and there for allows one-to-one connections to the server, more about this from ROS wiki).
When we are pushing objects to a std:vector, the content is copied by default. Here is how the reference [1] [2] describe std::vector::push_back:
void push_back (const value_type& val);
void push_back (value_type&& val);
---
Add element at the end
Adds a new element at the end of the vector, after its current last element. The content of val is copied (or moved) to the new element.
This effectively increases the container size by one, which causes an automatic reallocation of the allocated storage space if -and only if- the new vector size surpasses the current vector capacity.
When the object we copy includes a ros::ServiceServer, things start becoming more interesting. Here is a code snippet wrapping the origin object with ROS Service.
class PIDCtlrROS : public PIDFController {
public:
PIDCtlrROS(ros::NodeHandle& nh, ros::NodeHandle& nh_priv, std::string prefix
, double kP=1, double kI=0, double kD=0, double kFF=0
, double iLimit=10, double outLimit=10, double deadzone=0.01)
:PIDFController(kP,kI,kD,kFF,iLimit,outLimit,deadzone) {
pidset_srv = nh.advertiseService(prefix+std::string("/pid_set"),&PIDCtlrROS::pidset_srv_cb,this);
nh_priv.param(prefix+std::string("/kP"),this->kP,0.0);
nh_priv.param(prefix+std::string("/kI"),this->kI,0.0);
nh_priv.param(prefix+std::string("/kD"),this->kD,0.0);
nh_priv.param(prefix+std::string("/kFF"),this->kFF,1.0);
nh_priv.param(prefix+std::string("/dz"),this->deadzone,0.01);
nh_priv.param(prefix+std::string("/iLimit"),this->integral_limit, 100.0);
nh_priv.param(prefix+std::string("/outLimit"),this->output_limit, 10.0);
}
private:
ros::ServiceServer pidset_srv;
bool pidset_srv_cb(m2_ctlrs::PIDFSet::Request& req, m2_ctlrs::PIDFSet::Response& resp) {
ROS_INFO("called pidset");
this->kP = req.param.kP;
this->kI = req.param.kI;
this->kD = req.param.kD;
this->kFF = req.param.kFF;
resp.stamp = ros::Time::now();
ROS_INFO("done pidset");
return true;
}
};
This works fine alone, but using multiple of them in an std:vector will result in a segmentation fault. Here is the code snippet for an object wrapping 3 instances of PIDCtlrROS at a time.
class TwistCtlr2D {
public:
geometry_msgs::Twist input;
geometry_msgs::Twist feedback;
geometry_msgs::Twist output;
TwistCtlr2D(ros::NodeHandle& nh, ros::NodeHandle& nh_priv) : input_is_updated(false), fb_is_updated(false) {
controllers.push_back(PIDCtlrROS(nh, nh_priv, "x"));
controllers.push_back(PIDCtlrROS(nh, nh_priv, "y"));
controllers.push_back(PIDCtlrROS(nh, nh_priv, "z"));
...
}
...
}
The problem arise as the std::vector::push_back is copying the PIDCtlrROS object, which duplicates the ros::ServiceServer at the same time. Multiple server with the same config is not allowed in ROS (as the ROS Service model has a one-to-many relation).
Solution / Fixes
Notice the std::vector::push_back reference at the beginning? It said The content of val is copied (or moved) to the new element.. To move the content of val, we need to use std::move [2] [3] and implement a proper move constructor.
The updated code now look like this:
PIDFCtlrROS(PIDFCtlrROS&& ctlr) :
PIDFController(ctlr.kP,ctlr.kI,ctlr.kD,ctlr.kFF,ctlr.integral_limit,ctlr.output_limit,ctlr.deadzone) {
nh_ = ctlr.nh_;
prefix_ = ctlr.prefix_;
this->clearError();
// Reinit services to make ROS happy when adding this class instance to a std::vector.
ctlr.pidset_srv.shutdown();
pidset_srv = nh_.advertiseService(prefix_+std::string("/pid_set"),&PIDFCtlrROS::pidset_srv_cb,this);
}
TwistCtlr2D(ros::NodeHandle& nh, ros::NodeHandle& nh_priv) : force_ff(false),pause_output(false), input_is_updated(false), fb_is_updated(false) {
// std::vector uses copy constructor so please use std::move and define move constructor if you want to preserve callbacks
controllers.push_back(std::move(PIDFCtlrROS(nh, nh_priv, "x")));
controllers.push_back(std::move(PIDFCtlrROS(nh, nh_priv, "y")));
controllers.push_back(std::move(PIDFCtlrROS(nh, nh_priv, "z")));
...
}