Gazebo | Ignition | Community
Ask Your Question
0

cmd_vel not working with custom plugin

asked 2017-01-05 01:07:40 -0600

wilmer gravatar image

updated 2017-01-05 15:09:25 -0600

Hi, i wrote a plugin(attached down) to move a robot in the space , i compile the plugin and is compiling ok.

I add this line to my SDF at the end of model

  <plugin name="plugins" filename="libplugins.so"/>

i am running the command

rostopic pub /src/wilmer_gazebo/cmd_vel geometry_msgs/Twist "linear:
 x: 0.1
 y: 0.0
 z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.3" 
publishing and latching message. Press ctrl-C to terminate

when i run echo i dont get any return

WARNING: no messages received and simulated time is active

i run nodegraph and i get this image description

there is no connection between nodes...how can i solve this?

EDIT I change my cmd_vel to rostopic pub cmd_vel geometry_msgs/Twist "linear: x: 0.1 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 0.3" the run rostopic list and get:

 /clock
 /gazebo/link_states
 /gazebo/model_states
 /gazebo/parameter_descriptions
 /gazebo/parameter_updates
 /gazebo/set_link_state
 /gazebo/set_model_state
 /rosout
 /rosout_agg
 /wcmd_vel

after doing echo i get this graph image description

I see my error is that my plugin is not working over the same rostopic but how can i correct this?

my plugin is:

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include "geometry_msgs/Twist.h"
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
static float velx=0.0,vely=0.0,velz=0.0,Vangx=0.0,Vangy=0.0, Vangz=0.0; // set as global
namespace gazebo
{
  class plugins : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      // Store the pointer to the model
      this->model = _parent;

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&plugins::OnUpdate, this, _1));
    }
    // callback functions run every time data is published to the topic
    void ROSCallback_Vel(const geometry_msgs::Twist::ConstPtr& msg)
    {
    velx=msg->linear.x;
    vely=msg->linear.y;
    velz=msg->linear.z;
    Vangx=msg->angular.x;
    Vangy=msg->angular.y;
    Vangz=msg->angular.z;
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      // Apply a small linear velocity to the model.
      this->model->SetLinearVel(math::Vector3(velx, vely, velz));
       this->model->SetAngularVel(math::Vector3(Vangx, Vangy, Vangz));
    }



    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(plugins)
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-01-25 16:28:53 -0600

Francis123 gravatar image

I believe the problem is that you are not creating a ROS node& ROS subscriber within your gazebo control plugin.

This tutorial should explain how to connect to ROS within a gazebo node. AKA create a ROS node and ROS subscriber object to connect to the ROS callback function you have defined ("ROSCallback_Vel")

http://gazebosim.org/tutorials?cat=gu...

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2017-01-05 01:07:40 -0600

Seen: 3,281 times

Last updated: Jan 25 '17