Question

I am trying to work with gazebomotormodel.cpp. I need to visual some data that is calculated inside the plugin such a force, drag_torque etc. Is there a way i can publish this data to a topic or node so that i can visualize it using rqt.

P.S. - I am working on ROS kinetic with gazebo 7

Adding the .cpp file for reference. C:\fakepath\gazebomotormodel.cpp

Asked by zaccer on 2019-10-07 04:46:14 UTC

Answer 1

Sure, just initialize the ROS node inside the load function

if (!ros::isInitialized())
    {
        int argc = 0;
        char **argv = NULL;

        ros::init(argc, argv, node_name,
            ros::init_options::NoSigintHandler);
    }

    // Create ROS node.
    this->rosNode.reset(new ros::NodeHandle( node_name ));

and create a publisher

this->rosPub = this->rosNode->advertise<std_msgs::Float64>(topic_name,1);

Then you can access the class variable this->rosPub from within the plugin and send the messages.

EDIT

Include ROS at the top of the header file gazebo_motor_model.h:

#include "ros/ros.h"

Declare the class variables inside the header file

namespace gazebo {
    ...
    class GazeboMotorModel : public MotorModel, public ModelPlugin {
         ...
         private: std::unique_ptr<ros::NodeHandle> rosNode;
         private: ros::Publisher rosPub;

initialize the ROS node inside the Load function of the plugin inside the gazebo_motor_model.cpp file

if (!ros::isInitialized())
    {
        int argc = 0;
        char **argv = NULL;

        ros::init(argc, argv, "my_ros_node",
            ros::init_options::NoSigintHandler);
    }

    // Create ROS node.
    this->rosNode.reset(new ros::NodeHandle( "my_ros_node" ));

initialize the ros publisher right under the ROS node initialization

this->rosPub = this->rosNode->advertise<std_msgs::Float64>("my_ros_topic",1);

note that the message type (here std_msgs::Float64) must be edited to match your topics message type.

Now you can send the message anywhere in your update function

  std_msgs::Float64 msg;
  msg.data = 15.3;
  this->rosPub.publish(msg);

Asked by kumpakri on 2019-10-07 05:24:46 UTC

Comments

does the node need to be initialized in a separate script or in the same plugin file? Is there some example I can take reference of?

Asked by zaccer on 2019-10-07 05:52:03 UTC

You do it inside the plugin from which you want to get the information out to ROS.

Asked by kumpakri on 2019-10-07 06:32:04 UTC

After implementing the code, i tried to compile it but got the error ‘ros’ has not been declared if (!ros::isInitialized()) ^. Any suggestions?

Asked by zaccer on 2019-10-07 06:42:51 UTC

The gazebo plugins typically have a source code .cpp inside the /src folder and the header file .h or .hh of the same name inside the /include folder. You need to tell the interpreter to look into the ROS library. That is typically done inside the header file. The class variables (rosNode and rosPub in the example) are also typically declared in the header file. See the new edit in my answer.

Asked by kumpakri on 2019-10-07 07:09:30 UTC

private: std::unique_ptrros::NodeHandle rosNode; private: ros::Publisher rosPub; Do I need to modify the ros.h file to include the above class.

And how would i use the this->rospub class to publish lets say force inside UpdateForcesAndMoments function.

I should have mentioned, I am relatively new to ROS hence the struggle with such basic stuff.

Asked by zaccer on 2019-10-07 07:18:55 UTC

See if the current edit has all the answers you need. I think I addressed them all. Point to me what stays unclear still.

Asked by kumpakri on 2019-10-07 07:23:17 UTC

Your source code is named gazebo_motor_model.cpp so your header file inside the /include folder is named gazebo_motor_model.h. That is the header file you need to modify with the three lines I showed you above.

Asked by kumpakri on 2019-10-07 07:37:03 UTC

When you look inside the Gazebo 7 API documentation (Physics --> Joint --> GetVelocity()) You can see it returns a scalar number in double format, so, actually, the example with Float64 message type could be used if you are creating your own topic to publish to.

In that case also include the message type somewhere in your code (you can do it in source code or header file alike)

#include <std_msgs/Float64.h>

Asked by kumpakri on 2019-10-07 07:46:28 UTC

Thanks a lot for the detailed explanation.

Asked by zaccer on 2019-10-07 23:33:40 UTC

I implemented the code in the gazebo_motor_model.cpp, i was able to compile it without any error. but neither the node name nor the topic comes up during rosnode list and rostopic list.I am editing my question to include my modified gazebo_motor_model.cpp file. Please tell me where i might be making mistake.

Asked by zaccer on 2019-10-08 00:22:12 UTC

Are you sure the correct plugin is loaded? Can you add a debug message to the Load function that would be printed in the terminal upon the simulation start, for example?

Asked by kumpakri on 2019-10-08 06:53:57 UTC

How to modify a model plugin to publish data to a rostopic or ros node?

Comments

Answers

Comments