Ask Your Question
1

Subscriber callback isn't invoked

asked 2012-11-19 04:33:52 -0600

FTrommsdorff gravatar image

updated 2012-11-20 00:06:53 -0600

I'm currently rewriting this gazebo controller plugin to apply additional forces to a link of my model. My goal is to simulate the flight behaviour of a indoor blimp at low velocity. I tried to wrap the computations of the additional mechanical forces and moments and the subscription of the control input (which is at the moment some input from the keyboard, just for testing purposes) in one class. I tried to keep the example as tight as possible:

namespace gazebo
{

class Blimp_FlightMechanics : public ModelPlugin
{
  public: Blimp_FlightMechanics()
  {
        this->wrench_msg_.force.x = 0;
        this->wrench_msg_.force.y = 0;
        this->wrench_msg_.force.z = 0;
        this->wrench_msg_.torque.x = 0;
        this->wrench_msg_.torque.y = 0;
        this->wrench_msg_.torque.z = 0;
  };
  public: virtual ~Blimp_FlightMechanics()
  {
    event::Events::DisconnectWorldUpdateStart(this->update_connection_);    
        this->queue_.clear();
        this->queue_.disable();
        this->rosnode_->shutdown();
        delete this->rosnode_;      
  };
protected: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
// Load data from xml file
  if (!ros::isInitialized())
    return;
  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);

  ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Wrench>(
    this->topic_name_,1,
    boost::bind( &Blimp_FlightMechanics::UpdateObjectForce,this,_1),
    ros::VoidPtr(), &this->queue_);
  this->sub_ = this->rosnode_->subscribe(so);

  this->update_connection_ = event::Events::ConnectWorldUpdateStart(
      boost::bind(&Blimp_FlightMechanics::UpdateChild, this));
}

  protected: virtual void UpdateChild()
  {
this->lock_.lock();
    math::Vector3 force_sum = math::Vector3(0,0,0);
    math::Vector3 torque_sum = math::Vector3(0,0,0);

[Do computations of forces and torque here]

    this->link_->SetForce(force_sum);
    this->link_->SetTorque(torque_sum);
        this->lock_.unlock();
  };
   // this is the crucial part: although the callback is set above, it will not be called within the simulation; for debugging, I juse "ROS_INFO()", but the call here is never executed
    private: void UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _msg)
    {
        this->wrench_msg_.force.x = _msg->force.x;
        this->wrench_msg_.force.y = _msg->force.y;
        this->wrench_msg_.force.z = _msg->force.z;
        this->wrench_msg_.torque.x = _msg->torque.x;
        this->wrench_msg_.torque.y = _msg->torque.y;
        this->wrench_msg_.torque.z = _msg->torque.z;
        ROS_INFO("F-Ctrl: [%g %g %g], T-Ctrl: [%g %g %g]",this->wrench_msg_.force.x,this->wrench_msg_.force.y,this->wrench_msg_.force.z,this->wrench_msg_.torque.x,this->wrench_msg_.torque.y,this->wrench_msg_.torque.z);
    }
}
GZ_REGISTER_MODEL_PLUGIN(Blimp_FlightMechanics);

I also tried to keep the basic tutorials for subscribers at the wiki page in mind. At runtime, I execute my tele-operating (toy) keybord input programm and made a check using "rostopic info force1", with the following output:

Type: geometry_msgs/Wrench

Publishers: 
 * /talker (<adress>)

Subscribers: 
 * /gazebo (<adress>)

which seems feasible. I also receive messages, using "rostopic echo force1". Anybody has a clue or do I forget something?

EDIT: Problem was solved. For the subscriber, I used:

this->sub_ = this->rosnode_->subscribe<geometry_msgs::Wrench>(this->topic_name_, 1,boost::bind(&Blimp_FlightMechanics::UpdateObjectForce,this,_1));

and the callbacks are executed as expected, when a new message is published. Thanks to Lorenz.

edit retag flag offensive close merge delete

Comments

@Lorenz: Thank you for the reference about the track_object. But I wonder, why many examples of controllers are using the SubscriberOptions the same way? I try to figure out the difference between them and my code, but until now, I didn't find the crucial point.

FTrommsdorff gravatar imageFTrommsdorff ( 2012-11-19 21:48:47 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-11-19 05:27:13 -0600

Lorenz gravatar image

I would guess the problem is the temporary object you use for the tracked_object parameter in the SubscriberOptions. It will go out of scope immediately which will cause your subscriber to be destroyed right after creation.

I would not suggest to use the version of subscribe that takes an instance of SubscriberOptions. Instead, better use one of the higher-level functions. If you need to use a different callback queue, set it for the complete NodeHandle if possible. See here for more info.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2012-11-19 04:33:52 -0600

Seen: 547 times

Last updated: Nov 20 '12