Subscribing to a contact topic from a model plugin
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})