ROS Approximate Time Syncronizer, not entering callback

asked 2016-01-24 11:11:01 -0500

smahaff9876 gravatar image

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();
}
edit retag flag offensive close merge delete

Comments

Your code looks reasonable. You may want to use rostopic info or rqt_graph to confirm that both of your subscribers are connected.

ahendrix gravatar image ahendrix  ( 2016-01-24 14:29:50 -0500 )edit

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.

smahaff9876 gravatar image smahaff9876  ( 2016-01-24 15:03:34 -0500 )edit

When you do rostopic info, you should see your node listed as a subscriber, and at least one other node listed as a publisher.

ahendrix gravatar image ahendrix  ( 2016-01-24 16:16:20 -0500 )edit

Yes, that is what i saw.

smahaff9876 gravatar image smahaff9876  ( 2016-01-28 17:27:47 -0500 )edit