Ask Your Question

Revision history [back]

I tried something pointed out by lucasw and it works! I found that a dynamic_reconfigure server is linked to a NodeHandle and there can only be one instance otherwise you get the error: "tried to advertise a service that is already advertised in this node". So I created a vector of NodeHandle that I link to each server. Also I had to make sure to call ros::init(...) before creating any instance of my class that has the CallbackType as a member otherwise I got the error: Couldn't find an AF_INET address...

My class is for filtering motor commands, here are a few lines of code that focus on solving this problem: In my header file :

class CommandFilter
{
    /* some functions */

    void motorDynConfigCallback(osa_control::MotorDynConfig &config, uint32_t level, const int idx);

private:
    std::vector<ros::NodeHandle*> nh_list_;
    std::vector<dynamic_reconfigure::Server<osa_control::MotorDynConfig>*> motor_dyn_config_server_list_;
    std::vector<dynamic_reconfigure::Server<osa_control::MotorDynConfig>::CallbackType> motor_dyn_config_callback_f_list_;
    osa_control::MotorDynConfig motor_param_;
    /* other class members */
};

In my implementation file:

bool CommandFilter::init()
{
/* some init code */

for(int i=0; i<ptr_robot_description_->getRobotDof(); i++)
{
        const std::string dof_name = ptr_robot_description_->getControllerList().at(i)->getName();

        ros::NodeHandle *node_handle = new ros::NodeHandle(dof_name.c_str());
        nh_list_.push_back(node_handle);

        dynamic_reconfigure::Server<osa_control::MotorDynConfig> *s = new dynamic_reconfigure::Server<osa_control::MotorDynConfig>(*node_handle);
        dynamic_reconfigure::Server<osa_control::MotorDynConfig>::CallbackType f;

        f = boost::bind(&CommandFilter::motorDynConfigCallback, this, _1, _2, i);
        motor_dyn_config_callback_f_list_.push_back(f);

        s->setCallback(f);
        motor_dyn_config_server_list_.push_back(s);
}

/* some init code */
}

void CommandFilter::motorDynConfigCallback(osa_control::MotorDynConfig &config, uint32_t level, const int idx)
{
    ROS_INFO("Dynamic Reconfigure Request for dof%d [%s]: %s %d %d %d", idx+1, ptr_robot_description_->getControllerList().at(idx)->getName().c_str(), config.enable?"True":"False", config.min_pos, config.max_pos, config.offset_pos);

    motor_param_ = config;
}

Note that it's a first cut solution and the code can be improved, but it works and give me exactly what I needed.

I tried something pointed out by lucasw @lucasw and it works! I found that a dynamic_reconfigure server is linked to a NodeHandle and there can only be one instance otherwise you get the error: "tried to advertise a service that is already advertised in this node". So I created a vector of NodeHandle that I link to each server. Also I had to make sure to call ros::init(...) before creating any instance of my class that has the CallbackType as a member otherwise I got the error: Couldn't find an AF_INET address...

My class is for filtering motor commands, here are a few lines of code that focus on solving this problem: In my header file :

class CommandFilter
{
    /* some functions */

    void motorDynConfigCallback(osa_control::MotorDynConfig &config, uint32_t level, const int idx);

private:
    std::vector<ros::NodeHandle*> nh_list_;
    std::vector<dynamic_reconfigure::Server<osa_control::MotorDynConfig>*> motor_dyn_config_server_list_;
    std::vector<dynamic_reconfigure::Server<osa_control::MotorDynConfig>::CallbackType> motor_dyn_config_callback_f_list_;
    osa_control::MotorDynConfig motor_param_;
    /* other class members */
};

In my implementation file:

bool CommandFilter::init()
{
/* some init code */

for(int i=0; i<ptr_robot_description_->getRobotDof(); i++)
{
        const std::string dof_name = ptr_robot_description_->getControllerList().at(i)->getName();

        ros::NodeHandle *node_handle = new ros::NodeHandle(dof_name.c_str());
        nh_list_.push_back(node_handle);

        dynamic_reconfigure::Server<osa_control::MotorDynConfig> *s = new dynamic_reconfigure::Server<osa_control::MotorDynConfig>(*node_handle);
        dynamic_reconfigure::Server<osa_control::MotorDynConfig>::CallbackType f;

        f = boost::bind(&CommandFilter::motorDynConfigCallback, this, _1, _2, i);
        motor_dyn_config_callback_f_list_.push_back(f);

        s->setCallback(f);
        motor_dyn_config_server_list_.push_back(s);
}

/* some init code */
}

void CommandFilter::motorDynConfigCallback(osa_control::MotorDynConfig &config, uint32_t level, const int idx)
{
    ROS_INFO("Dynamic Reconfigure Request for dof%d [%s]: %s %d %d %d", idx+1, ptr_robot_description_->getControllerList().at(idx)->getName().c_str(), config.enable?"True":"False", config.min_pos, config.max_pos, config.offset_pos);

    motor_param_ = config;
}

Note that it's a first cut solution and the code can be improved, but it works and give me exactly what I needed.

I tried something pointed out by @lucasw and it works! I found that a dynamic_reconfigure server is linked to a NodeHandle and there can only be one instance otherwise you get the error: "tried to advertise a service that is already advertised in this node". So I created a vector of NodeHandle that I link to each server. Also I had to make sure to call ros::init(...) before creating any instance of my class that has the CallbackType as a member otherwise I got the error: Couldn't find an AF_INET address...

My class is for filtering motor commands, here are a few lines of code that focus on solving this problem: In my header file :

class CommandFilter
{
    /* some functions */

    void motorDynConfigCallback(osa_control::MotorDynConfig &config, uint32_t level, const int idx);

private:
    std::vector<ros::NodeHandle*> nh_list_;
    std::vector<dynamic_reconfigure::Server<osa_control::MotorDynConfig>*> motor_dyn_config_server_list_;
    std::vector<dynamic_reconfigure::Server<osa_control::MotorDynConfig>::CallbackType> motor_dyn_config_callback_f_list_;
    osa_control::MotorDynConfig motor_param_;
    /* other class members */
};

In my implementation file:

bool CommandFilter::init()
{
/* some init code */

for(int i=0; i<ptr_robot_description_->getRobotDof(); i++)
{
        const std::string dof_name = ptr_robot_description_->getControllerList().at(i)->getName();

        ros::NodeHandle *node_handle = new ros::NodeHandle(dof_name.c_str());
        nh_list_.push_back(node_handle);
node_handle(dof_name.c_str());

        dynamic_reconfigure::Server<osa_control::MotorDynConfig> *s = new dynamic_reconfigure::Server<osa_control::MotorDynConfig>(*node_handle);
dynamic_reconfigure::Server<osa_control::MotorDynConfig>(node_handle);
        dynamic_reconfigure::Server<osa_control::MotorDynConfig>::CallbackType f;
         f = boost::bind(&CommandFilter::motorDynConfigCallback, this, _1, _2, i);
        motor_dyn_config_callback_f_list_.push_back(f);

        s->setCallback(f);
        motor_dyn_config_server_list_.push_back(s);
}

/* some init code */
}

void CommandFilter::motorDynConfigCallback(osa_control::MotorDynConfig &config, uint32_t level, const int idx)
{
    ROS_INFO("Dynamic Reconfigure Request for dof%d [%s]: %s %d %d %d", idx+1, ptr_robot_description_->getControllerList().at(idx)->getName().c_str(), config.enable?"True":"False", config.min_pos, config.max_pos, config.offset_pos);

    motor_param_ = config;
}

Note that it's a first cut solution and the code can be improved, but it works and give me exactly what I needed.