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.
Originally posted by FTrommsdorff on ROS Answers with karma: 18 on 2012-11-19
Post score: 1
Original comments
Comment by FTrommsdorff on 2012-11-19:
@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.