Ask Your Question
0

Subscribing to ROS and Gazebo topic in one node (ROS callback failing)

asked 2015-07-29 06:43:20 -0500

niall gravatar image

updated 2018-12-01 13:29:17 -0500

jayess gravatar image

Hi,

I have a node which successfully subscribes to a contacts topic in Gazebo. Now I want the same node to subscribe to a ROS topic which of course runs through a separate callback. Unfortunately the ROS callback is not being run even though the topic is available and publishing. Only the Gazebo callback is running. Is there something I'm missing here? Are the publishers/subscribers from ROS and Gazebo conflicting?

Of course I asked this question on the Gazebo answers page as well but seeing as it is the ROS subscriber that is not working, I was hoping someone could tip me why the callback could not be running.

Here is my code:

#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo.hh>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <geometry_msgs/Vector3.h>

#include <iostream>

ros::Publisher pub;
bool airborne;

// Forces callback function
void forcesCb(ConstContactsPtr &_msg){
    geometry_msgs::Vector3 msgForce;
    // What to do when callback
    if ( _msg->contact_size()!= 0){
        // Now publish
        msgForce.x = _msg->contact(0).wrench().Get(0).body_1_wrench().force().x();
        msgForce.y = _msg->contact(0).wrench().Get(0).body_1_wrench().force().y();
        msgForce.z = _msg->contact(0).wrench().Get(0).body_1_wrench().force().z();
        pub.publish(msgForce);
    } else if (airborne = false) {
        msgForce.x = 0;
        msgForce.y = 0;
        msgForce.z = 0;
    } else {
        msgForce.x = 0;
        msgForce.y = 0;
        msgForce.z = 0;
    }
    pub.publish(msgForce);
}

// Position callback function
void positionCb(const nav_msgs::Odometry::ConstPtr& msg2){
    if (msg2->pose.pose.position.z > 0.3) {
        airborne = true;
    } else {
        airborne = false;
    }
}

int main(int _argc, char **_argv){
  // Set variables
  airborne = false;

  // Load Gazebo & ROS
  gazebo::setupClient(_argc, _argv);
  ros::init(_argc, _argv, "force_measure");

  // Create Gazebo node and init
  gazebo::transport::NodePtr node(new gazebo::transport::Node());
  node->Init();

  // Create ROS node and init
  ros::NodeHandle n;
  pub = n.advertise<geometry_msgs::Vector3>("forces", 1000);

  // Listen to Gazebo contacts topic
  gazebo::transport::SubscriberPtr sub = node->Subscribe("~/quadrotor/base_link/quadrotor_bumper/contacts", forcesCb);

  // Listen to ROS for position
  ros::Subscriber sub2 = n.subscribe("ground_truth/state", 1000, positionCb);

  // Busy wait loop...replace with your own code as needed.
  while (true)
    gazebo::common::Time::MSleep(20);

  // Spin ROS (needed for publisher)
  ros::spinOnce();

  // Mayke sure to shut everything down.
  gazebo::shutdown();
}

Update:

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(force_measure)

include (FindPkgConfig)
if (PKG_CONFIG_FOUND)
  pkg_check_modules(GAZEBO gazebo)
endif()

include(FindBoost)
find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)

find_package(Protobuf REQUIRED)
find_package(catkin REQUIRED COMPONENTS roscpp gazebo_msgs geometry_msgs nav_msgs)

catkin_package()

include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})

add_executable(listener listener.cpp)
add_executable(listener2 listener2.cpp)

target_link_libraries(listener ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
target_link_libraries(listener2 ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
edit retag flag offensive close merge delete

Comments

Did you check rqt_graph? So that your node is connected to the right topic? You do not have a "/" in the beginning so there could be: <possible namespace>/ground_truth/state or something like that

cyborg-x1 gravatar imagecyborg-x1 ( 2015-07-29 06:50:40 -0500 )edit

Good questions, thanks! I did check rqt_graph indeed and force_measure (this node) is connected (and receiving) topic /ground_state/state from node ground_truth. I also did try the "/", in fact I had it that way before but that also didn't help.

niall gravatar imageniall ( 2015-07-29 07:16:18 -0500 )edit

Hi. I know this may be an old question. How to make "#include <gazebo transport="" transport.hh="">" generate no error? What do I have to modify in order to let this library properly work? Thanks

polde gravatar imagepolde ( 2017-10-24 10:16:28 -0500 )edit

@polde You'll probably have better luck getting an answer by asking a new question.

jayess gravatar imagejayess ( 2017-10-24 11:13:31 -0500 )edit

@polde What are you trying to do? This include was never the issue for me (as explained below). I presume you're not including the library in exactly that way (with all the extra quotes)?

niall gravatar imageniall ( 2017-10-25 13:40:25 -0500 )edit

Hi, Can you share the CMakeLists.txt of your program ? because I want to implement a similar program but I have a problem when executing catkin_make caused by the CmakeLists.txt

AyaO gravatar imageAyaO ( 2018-11-21 08:55:01 -0500 )edit

Sure. But I'll have to dig it up out of a zip far far away. Give me a few days.

In the meantime, what "problem" is catkin_make giving you?

niall gravatar imageniall ( 2018-11-21 09:35:32 -0500 )edit
niall gravatar imageniall ( 2018-12-01 04:01:04 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-07-29 08:06:52 -0500

updated 2015-07-29 08:11:19 -0500

Your problem is here:

  // Busy wait loop...replace with your own code as needed.
  while (true)
    gazebo::common::Time::MSleep(20); //<<< That is your only while loop ;-)

  // Spin ROS (needed for publisher) 
  ros::spinOnce();//<< executed at the end only once...

try:

  // Busy wait loop...replace with your own code as needed.
  while (true)
  {
     gazebo::common::Time::MSleep(20);

  // Spin ROS (needed for publisher) // (nope its actually for subscribers-calling callbacks ;-) )
  ros::spinOnce();

     ...
  }
edit flag offensive delete link more

Comments

Simple and effective, stupid mistake.. Thanks a bunch!

niall gravatar imageniall ( 2015-07-29 08:42:19 -0500 )edit

Welcome ;-)

cyborg-x1 gravatar imagecyborg-x1 ( 2015-07-29 09:09:08 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2015-07-29 06:43:20 -0500

Seen: 1,660 times

Last updated: Dec 01 '18