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

edit

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)