Hello, I am running this plugin: https://pastebin.com/HUUJFMnU
but my joints are not moving. I tried to put a ROS_INFO message into each function to see if they are printed. Load function message is printed once, Onupdate function message is printed endlessly and RosCallback function message never gets printed. Why is this happening? What should I change?
#include
#include
#include
#include
#include
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "geometry_msgs/Twist.h"
namespace gazebo
{
class ROSModelPlugin : public ModelPlugin
{
public: ROSModelPlugin()
{
// Start up ROS
std::string name = "gazebo_p3at";
int argc = 0;
ros::init(argc, NULL, name);
}
public: ~ROSModelPlugin()
{
delete this->node;
}
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
// Store the pointer to the model
this->model = _parent;
// ROS Nodehandle
this->node = new ros::NodeHandle("~");
// ROS Subscriber
this->sub = this->node->subscribe("cmd_vel", 10, &ROSModelPlugin::ROSCallback, this );
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateStart(
boost::bind(&ROSModelPlugin::OnUpdate, this));
}
// Called by the world update start event
public: void OnUpdate()
{
ros::spinOnce();
}
void ROSCallback(const boost::shared_ptr& cmd_vel)
{
physics::Joint_V joints = this->model->GetJoints();
float maxForce = 3.0;
joints[0]->SetMaxForce(0, maxForce);
joints[1]->SetMaxForce(0, maxForce);
joints[2]->SetMaxForce(0, maxForce);
joints[3]->SetMaxForce(0, maxForce);
float wheelRadius = 0.110; // (Meters)
float wheelBase = 0.250; // Distance between opposite wheels (Meters)
float RotVel_lin = cmd_vel->linear.x / wheelRadius;
float RotVel_rot = cmd_vel->angular.z * (wheelBase / wheelRadius);
joints[0]->SetVelocity(0, RotVel_lin - RotVel_rot);
joints[1]->SetVelocity(0, RotVel_lin - RotVel_rot);
joints[2]->SetVelocity(0, RotVel_lin + RotVel_rot);
joints[3]->SetVelocity(0, RotVel_lin + RotVel_rot);
ROS_INFO("cmd_vel: [%f, %f]"
, cmd_vel->linear.x
, cmd_vel->angular.z );
}
// Pointer to the model
private: physics::ModelPtr model;
// Pointer to the update event connection
private: event::ConnectionPtr updateConnection;
// ROS Nodehandle
private: ros::NodeHandle* node;
// ROS Subscriber
ros::Subscriber sub;
};
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(ROSModelPlugin)
}
↧