Error "Call to publish() on an invalid Publisher" using ApproximateTime Policy
Hi everyone, I try to use the ApproximateTime Policy-Based Synchronizer to get the force and position data from a force sensor and NDI, the code is show here which imitated from #q297174, in this code, I try to use geometry_msgs::TransformStamped
to get force and position data in one topic and publish it in callback
, but it reported a mistake when the code run. then I try to publish the topic of force and position separately. it still fail under same mistake:
[ INFO] [1555644883.009164946]: Synchronization successful [10.000000]
[FATAL] [1555644883.009201476]: ASSERTION FAILED
file = /opt/ros/indigo/include/ros/publisher.h
line = 102
cond = false
message =
[FATAL] [1555644883.009213278]: Call to publish() on an invalid Publisher
[FATAL] [1555644883.009220410]:
Trace/breakpoint trap (core dumped)
[ INFO] [1555646329.618371967]: Synchronization successful [10.000000]
[FATAL] [1555646329.618447480]: ASSERTION FAILED
file = /opt/ros/indigo/include/ros/publisher.h
line = 69
cond = false
message =
[FATAL] [1555646329.618476584]: Call to publish() on an invalid Publisher
[FATAL] [1555646329.618495410]:
Trace/breakpoint trap (core dumped)
and the code is show here:
#include "ros/ros.h"
#include <sstream>
#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/PoseArray.h>
#include "std_msgs/Float64.h"
using namespace message_filters;
//tf2_ros::TransformBroadcaster tf2_broadcaster;
geometry_msgs::TransformStamped transformStamped;
//ros::Publisher sensor_pub;
ros::Publisher sensor_pub1;
ros::Publisher sensor_pub2;
void callback(const geometry_msgs::WrenchStampedConstPtr& netft_data_msg, const geometry_msgs::PoseArrayConstPtr& targets);
int main(int argc, char** argv)
{
ros::init(argc, argv, "get_all_sensor_in_one");
ros::NodeHandle nh;
//ros::Publisher sensor_pub = nh.advertise<geometry_msgs::TransformStamped>("/sensor_together", 1000);
ros::Publisher sensor_pub1 = nh.advertise<geometry_msgs::WrenchStamped>("/sensor_ati", 1000);
ros::Publisher sensor_pub2 = nh.advertise<geometry_msgs::PoseArray>("/sensor_ndi", 1000);
message_filters::Subscriber<geometry_msgs::WrenchStamped> force_sub(nh, "/netft_data", 1);
message_filters::Subscriber<geometry_msgs::PoseArray> pose_sub(nh, "/polaris_sensor/targets", 1);
//message_filters::TimeSynchronizer<geometry_msgs::WrenchStamped, geometry_msgs::PoseArray> sync(force_sub, pose_sub, 2);
typedef sync_policies::ApproximateTime<geometry_msgs::WrenchStamped, geometry_msgs::PoseArray> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), force_sub, pose_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));
// sensor_pub.publish(sync);
ros::spin();
return 0;
}
void callback(const geometry_msgs::WrenchStampedConstPtr& netft_data_msg, const geometry_msgs::PoseArrayConstPtr& targets)
{
// static tf2_ros::TransformBroadcaster tf2_broadcaster;
// geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "ts_frame";
transformStamped.child_frame_id = "prism_frame";
transformStamped.transform.translation.x = targets->poses[0].position.x;
transformStamped.transform.translation.y = targets->poses[0].position.y;
transformStamped.transform.translation.z = targets->poses[0].position.z;
tf2::Quaternion q;
transformStamped.transform.rotation.x = netft_data_msg->wrench.force.x;
transformStamped.transform.rotation.y = netft_data_msg->wrench.force.y;
transformStamped.transform.rotation.z = netft_data_msg->wrench.force.z;
transformStamped.transform.rotation.w = targets->poses[0].orientation.w;
//tf2_broadcaster.sendTransform(transformStamped);
ROS_INFO("Synchronization successful [%f]", 10.00);//transformStamped.transform.rotation.z
//sensor_pub.publish(transformStamped);
sensor_pub1.publish(netft_data_msg);
sensor_pub2.publish(targets);
}
I edit the code and read the data without using the publisher:
#include "ros/ros.h"
#include <sstream>
#include <iostream>
#include ...