Subscribing to ROS and Gazebo topic in one node (ROS callback failing)
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)
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 thatGood questions, thanks! I did check rqt_graph indeed and force_measure (this node) is connected (and receiving) topic
/ground_state/state
from nodeground_truth
. I also did try the "/", in fact I had it that way before but that also didn't help.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 You'll probably have better luck getting an answer by asking a new question.
@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)?
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
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?
@AyaO
See below: https://pastebin.com/f5n6B3za