Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <boost/bind.hpp>
- #include <gazebo.hh>
- #include <physics/physics.hh>
- #include <common/common.hh>
- #include <stdio.h>
- #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<geometry_msgs::Twist>("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<geometry_msgs::Twist const>& 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)
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement