ROS Approximate Time Syncronizer, not entering callback
I am trying to snychronize two messages (Imu and AccelStamped) using the ApproximateTimeSynchronizer. The callback does not seem to be called ever. The code compiles and using rostopic echo I have confirmed that the messages are coming in.
If anyone can see what is wrong with my code, it would be much appreciated.
#include "ros/ros.h"
#include "sensor_msgs/Joy.h"
#include "geometry_msgs/Accel.h"
#include "geometry_msgs/AccelStamped.h"
#include <std_msgs/Float64.h>
#include <sensor_msgs/Imu.h>
#include <control_toolbox/pid.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <dynamic_reconfigure/server.h>
#include <pid_control/pidConfig.h>
#include <boost/asio.hpp>
void dyn_callback(pid_control::pidConfig &config, uint32_t level) {
ROS_INFO("Reconfigure Request: %d %f %s %s %d",
config.int_param, config.double_param,
config.str_param.c_str(),
config.bool_param?"True":"False",
config.size);
}
void callback(const sensor_msgs::Imu::ConstPtr& current_accel, const geometry_msgs::AccelStamped::ConstPtr& accel_set)
{
geometry_msgs::Vector3 accel_des;
ros::Time time;
ros::Duration time_diff;
ros::Time last_time;
double currentx=0;
double currenty=0;
double currentz=0;
ROS_INFO("Entered Callback");
accel_des.x = accel_set->accel.linear.x;
accel_des.y = accel_set->accel.linear.y;
accel_des.z = accel_set->accel.linear.z;
currentx = current_accel->linear_acceleration.x;
currenty = current_accel->linear_acceleration.y;
currentz = current_accel->linear_acceleration.z;
control_toolbox::Pid pid;
dynamic_reconfigure::Server<pid_control::pidConfig> server;
dynamic_reconfigure::Server<pid_control::pidConfig>::CallbackType f;
f = boost::bind(&dyn_callback, _1, _2);
server.setCallback(f);
pid.initPid(3, .5, 2, 0.3, -0.3);
time = ros::Time::now();
time_diff = time-last_time;
pid.control_toolbox::Pid::computeCommand((accel_des.x-currentx),time_diff);
pid.control_toolbox::Pid::getCurrentCmd();
last_time = time;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "pid_control");
ros::NodeHandle nh;
ros::Publisher a_e;
geometry_msgs::Accel accels_err;
message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh, "imu/imu",1);
message_filters::Subscriber<geometry_msgs::AccelStamped> accel_sub(nh, "accel_set_pt",1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu, geometry_msgs::AccelStamped> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(100), imu_sub, accel_sub);
sync.registerCallback(boost::bind(&callback,_1,_2));
a_e = nh.advertise<geometry_msgs::Accel>("accel_error", 1);
ros::spin();
}
Asked by smahaff9876 on 2016-01-24 12:08:50 UTC
Comments
Your code looks reasonable. You may want to use rostopic info or rqt_graph to confirm that both of your subscribers are connected.
Asked by ahendrix on 2016-01-24 15:29:50 UTC
What exactly do you mean by connected? When I use rostopic info both of the topics that I am subscribing to list the node as a subscriber.
Asked by smahaff9876 on 2016-01-24 16:03:34 UTC
When you do
rostopic info
, you should see your node listed as a subscriber, and at least one other node listed as a publisher.Asked by ahendrix on 2016-01-24 17:16:20 UTC
Yes, that is what i saw.
Asked by smahaff9876 on 2016-01-28 18:27:47 UTC