Gazebo | Ignition | Community
Ask Your Question
0

Subscribing to a contact topic from a model plugin

asked 2020-09-19 01:29:28 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

Hello,

I am aware that there is lot of material on the above question but for some reason I am unable to get it to work. I have a model plugin of a ping pong ball which bounces on the table every 2 seconds ( serves like in table tennis ) and I have a contact sensor which publishes to "/gazebo/default/table_tennis_ball/ball/my_contact" messages of type gazebo.msgs.Contacts. I would like to add the condition to re serve on certain collision conditions ( collision with the net ).

#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Pose3.hh>
#include "ros/ros.h"
#include "std_msgs/String.h"

using namespace gazebo;
class ModelPush : public ModelPlugin{

    private: transport::SubscriberPtr mySubscriber;


    public: void CollisionUpdates(ConstContactsPtr &_msg){

    }

    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/){


        // if (!ros::isInitialized()){
        int argc = 0;
        char **argv = NULL;
        client::setup(argc, argv);
        ros::init(argc, argv, "force_measure");
        // }
        transport::NodePtr node(new transport::Node());
        node->Init();

        this->mySubscriber = node->Subscribe("/gazebo/default/table_tennis_ball/ball/my_contact",CollisionUpdates);
    }   

};
GZ_REGISTER_MODEL_PLUGIN(ModelPush);

I have attached the code which is throwing the error. My guess is that I'm not subscribing in the correct format but I cannot for the life of me figure out what the right way is. The error being thrown is :

In member function ‘virtual void ModelPush::Load(gazebo::physics::ModelPtr, sdf::ElementPtr)’:
/home/shreyas/catkin_ws/src/gazebo_plugin_tutorial/model_push.cc:64:108: error: invalid use of non-static member function ‘void ModelPush::CollisionUpdates(ConstContactsPtr&)’
   this->mySubscriber = node->Subscribe("/gazebo/default/table_tennis_ball/ball/my_contact",CollisionUpdates);

CMakeLists.txt :

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(gazebo REQUIRED)
find_package(catkin REQUIRED COMPONENTS roscpp)
find_package(std_msgs REQUIRED)

include_directories(${GAZEBO_INCLUDE_DIRS})
include_directories(${catkin_INCLUDE_DIRS})
include_directories(${roscpp_INCLUDE_DIRS})
include_directories(${std_msgs_INCLUDE_DIRS})

link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")

add_library(hello_world SHARED hello_world.cc)
target_link_libraries(hello_world ${GAZEBO_LIBRARIES})

add_library(model_push SHARED model_push.cc)
target_link_libraries(model_push ${GAZEBO_LIBRARIES})
edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
0

answered 2020-12-07 21:21:07 -0500

Since you already include ROS, one way is to create a ROS node within the plugin instead of the transport::NodePtr, and create a ROS subscriber. Put both of the following into private:

ros::NodeHandle node;
private: ros::Subscriber sub1;

Then define the subscriber within "public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /_sdf/){...}"

sub1 = node.subscribe("/Topic_of_interest", 1, &ModelPush::callback_function,this);

and finally create the callback function to handle the incoming messages

public:
      void callback_function(<whatever your message type is>) {<your code here>}
edit flag offensive delete link more
0

answered 2020-10-05 03:19:30 -0500

ahcorde gravatar image

Hello,

This line looks wrong:

this->mySubscriber = node->Subscribe("/gazebo/default/table_tennis_ball/ball/my_contact",CollisionUpdates);

Try with this:

this->mySubscriber = node->Subscribe("/gazebo/default/table_tennis_ball/ball/my_contact", &ModelPush::CollisionUpdates, this);

Regards

edit flag offensive delete link more

Question Tools

Stats

Asked: 2020-09-19 01:29:28 -0500

Seen: 450 times

Last updated: Dec 07 '20