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)
Originally posted by niall on ROS Answers with karma: 47 on 2015-07-29
Post score: 0
Original comments
Comment by cyborg-x1 on 2015-07-29:
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
Comment by niall on 2015-07-29:
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.
Comment by polde on 2017-10-24:
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
Comment by jayess on 2017-10-24:
@polde You'll probably have better luck getting an answer by asking a new question.
Comment by niall on 2017-10-25:
@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)?
Comment by AyaO on 2018-11-21:
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
Comment by niall on 2018-11-21:
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?
Comment by niall on 2018-12-01:
@AyaO
See below: https://pastebin.com/f5n6B3za
Comment by jayess on 2018-12-01:
@niall I updated your question to include the code from the pastebin in order to keep the question self-contained in case that site goes down or the pastebin disappears.