take from LCM and ROS, send to LCM
Hi all, I need to interface ROS and LCM.
I thought to do in the following way:
I have a class. Let's call it MyClass
I have an LCM callback that I use to fill a class variable. Lets call it MyClass::lcm_cb()
;
I have a ROS callback that I use to do my processing. Let's call it MyClass::ros_cb()
;
Actually the ROS callback is a synchronized callback that takes from two topics via message_filter.
I need process the data from the ROS message. As soon as I receive the ROS message, I want to get the last available message from LCM, invoking MyClass::lcm_cb()
, and use it together with the ROS message. Then I want to publish on LCM again something.
I want that what dictates the frequency is ROS and not LCM. My pseudocode is something like this:
MyClass::MyClass(ros::NodeHandle& nh) : nh_(nh) {
sync = new message_filters::Synchronizer<MySyncPolicy> (MySyncPolicy(10), first_sub, second_sub);
sync->registerCallback(boost::bind(&MyClass::ros_cb, this, _1, _2));
lcm_sub = mylcm.subscribe("LCM_TOPIC", MyClas::lcm_cb, this);
}
MyClass::ros_cb(const sensor_msgs::ImageConstPtr& depth_input,
const sensor_msgs::PointCloud2ConstPtr& cloud_input) {
// INVOKE LCM CALLBACK
mylcm.handle();
// DO STUFF
mylcm.publish("LCM_OUTPUT", &out_msg_);
}
void MyClass::lcm_cb(const lcm::ReceiveBuffer *rbuf,
const string &chan,
const mav::pose_t *msg) {
// DO OTHER STUFF
}
MyClass::run() {
ros::spin();
}
int main(int argc, char **argv) {
ros::init(argc, argv, "my_node");
ros::NodeHandle nh;
MyClass mc(nh);
mc.run();
}
If I run it, apparently the ROS callback is not called or it gets stuck forever after the first call.
What is wrong?
Asked by mark_vision on 2015-09-23 06:26:21 UTC
Answers
I'm not sure what is wrong with your code, but you might want to take a look at the Pronto LCM<->ROS translators. I have used those and know they work, so you should be able to find out the issue by comparing them with your code.
/edit: Update as per your comment below. I'm not sure why things don't work as is, but what you could try is alternating ros::spin and lcm's handle in your run loop so their callbacks are always triggered:
void MyClass::lcm_cb(const lcm::ReceiveBuffer *rbuf,
const string &chan,
const mav::pose_t *msg) {
// SAVE LCM DATA TO CLASS MEMBER SO IT'S ACCESSIBLE IN ROS CALLBACK
}
MyClass::run() {
LOOP_WITH_RATE
{
mylcm.handle();
ros::spinOnce();
}
}
Asked by Stefan Kohlbrecher on 2015-09-23 06:35:04 UTC
Comments
I already know LCM ROS translators. They just listen to ROS and publish to LCM or the other way round. Here I want to listen both from LCM and ROS and to publish on LCM. It's a different issue. Thanks for the suggestion though.
Asked by mark_vision on 2015-09-23 07:39:18 UTC
Oh ok, I see. Updated with a suggestion of what you could try.
Asked by Stefan Kohlbrecher on 2015-09-23 16:31:11 UTC
Comments