Publisher gives segmentation fault unless message is defined in a "parent scope"
Hi,
A peculiar bug occurred while making a simple listener/publisher for a visualization application.
I am using boost::function to pass a lambda-function; which might be a bit odd, but I do not see how this could give me a segmentation error or whether it is something different..
So the following gives a segmentation error when it tries to publish the message:
#include ....
int main(int argc, char** argv) {
ros::init(argc, argv, "inertial_odom_demo_node");
ros::NodeHandle n;
auto pose_pub = n.advertise<geometry_msgs::PoseStamped>("/pose", 10);
odometry inertial_odom;
boost::function<void (const sensor_msgs::Imu&)> callback =
[&] (const sensor_msgs::Imu& msg) {
Eigen::Vector3d a, g;
a << msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z;
g << msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z;
uint64_t t = msg.header.stamp.toNSec();
if (inertial_odom.imu(a,g,t))
{
auto state_vector = inertial_odom.pose();
geometry_msgs::PoseStamped poseStamped;
poseStamped.header.frame_id = "map";
poseStamped.header.stamp = msg.header.stamp;
poseStamped.pose.position.x = state_vector(0);
poseStamped.pose.position.y = state_vector(1);
poseStamped.pose.position.z = state_vector(2);
poseStamped.pose.orientation.w = state_vector(3);
poseStamped.pose.orientation.x = state_vector(4);
poseStamped.pose.orientation.y = state_vector(5);
poseStamped.pose.orientation.z = state_vector(6);
pose_pub.publish(poseStamped);
}
};
auto imu_sub = n.subscribe<sensor_msgs::Imu>("/imu", 1000, callback);
ros::spin();
}
while the following does not:
#include ....
int main(int argc, char** argv) {
ros::init(argc, argv, "inertial_odom_demo_node");
ros::NodeHandle n;
auto pose_pub = n.advertise<geometry_msgs::PoseStamped>("/pose", 10);
geometry_msgs::PoseStamped poseStamped;
poseStamped.header.frame_id = "map";
odometry inertial_odom;
boost::function<void (const sensor_msgs::Imu&)> callback =
[&] (const sensor_msgs::Imu& msg) {
Eigen::Vector3d a, g;
a << msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z;
g << msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z;
uint64_t t = msg.header.stamp.toNSec();
if (inertial_odom.imu(a,g,t))
{
auto state_vector = inertial_odom.pose();
poseStamped.header.stamp = msg.header.stamp;
poseStamped.pose.position.x = state_vector(0);
poseStamped.pose.position.y = state_vector(1);
poseStamped.pose.position.z = state_vector(2);
poseStamped.pose.orientation.w = state_vector(3);
poseStamped.pose.orientation.x = state_vector(4);
poseStamped.pose.orientation.y = state_vector(5);
poseStamped.pose.orientation.z = state_vector(6);
pose_pub.publish(poseStamped);
}
};
auto imu_sub = n.subscribe<sensor_msgs::Imu>("/imu", 1000, callback);
ros::spin();
}
I toyed around with different message-type, but it does not change the result... Any ideas?
Asked by Thomash on 2018-08-10 04:59:24 UTC
Comments
Two things:
boost::function
to pass a lambda toNodeHandle::subscribe(..)
, do you have a particular reason for wanting to use it?auto
worked fine for me.This might have something to do with
poseStamped
having gone ..Asked by gvdhoorn on 2018-08-10 05:51:52 UTC
.. out of scope before it's actually published, and ROS internals trying to use a dangling reference (as much as those exist).
Some interesting links: passing functions to functions and ..
Asked by gvdhoorn on 2018-08-10 06:02:28 UTC
.. Segmentation fault with std::function and lambda parameters.
Asked by gvdhoorn on 2018-08-10 06:06:37 UTC
hi,
Thanks for those two links; they look highly relevant.. Using auto seemed to work, then I did something and now I keep getting an error.. I will retrace my steps and comment once I have something working :-)
Asked by Thomash on 2018-08-10 06:41:47 UTC
ps; this is the main complaint when using auto for now: error: no match for call to .... BOOST_FUNCTION_RETURN((*f)(BOOST_FUNCTION_ARGS));
Asked by Thomash on 2018-08-10 06:47:02 UTC
As for using lambda's; no success. (also here https://stackoverflow.com/questions/45340189/ros-use-lambda-as-callback-in-nodehandle-subscribe). As for the return value of the lambda and seg fault.. also didn't work yet.. I am going to look at it some more later.
Asked by Thomash on 2018-08-10 07:59:25 UTC
Little update on the auto-lambda-callback; it stumbled upon me that ros uses xx::yy::ConstPtr when passing messages; while in the lambda I defined the function argument as reference and use it as such.. now this is ok; but it does not work with auto, if I treat it as ConstPtr (e.g. *msg), auto works
Asked by Thomash on 2018-08-30 15:44:56 UTC