Ask Your Question
0

Synchronize imu joint_states foot_sensor errors

asked 2019-05-11 07:06:01 -0600

crank gravatar image

updated 2019-05-11 07:13:06 -0600

I use imu, robot joint_state and foot sensors, such as

<sensor_msgs::Imu>
<sensor_msgs::JointState>
<gazebo_msgs::ContactsState>
<gazebo_msgs::ContactsState>
<gazebo_msgs::ContactsState>
<gazebo_msgs::ContactsState>

But when I use message_filters for imu + joint_states , it can enter callback, when I use message_filters for 4 foot sensors, iit can also enter callbackwhen I use imu + joint_states + 4 foot sensors t can't enter callback,. I don't know why , can anybady tell me how to Synchronize these msg? Thank you !

Here is my function:

void sensorcallback(const sensor_msgs::ImuConstPtr &imu_msg, 
                const sensor_msgs::JointStateConstPtr& joint_msg,
               const gazebo_msgs::ContactsStateConstPtr &feet1_msg,
                const gazebo_msgs::ContactsStateConstPtr &feet2_msg,
                const gazebo_ms
                const gazebo_msgs::ContactsStateConstPtr &feet4_msg)
{
    ROS_ERROR("Enter ");
}

int main(int argc, char **argv)
{
   ros::init(argc, argv, "test");
   ros::NodeHandle nh;
   ROS_INFO("i'm here");
   message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh,"/imu/data", 10);
   message_filters::Subscriber<sensor_msgs::JointState> joints_sub(nh,"/joint_states",10);
   message_filters::Subscriber<gazebo_msgs::ContactsState> feet1_sub(nh,"/leg1_4_bumper",10);
   message_filters::Subscriber<gazebo_msgs::ContactsState> feet2_sub(nh,"/leg2_4_bumper",10);
   message_filters::Subscriber<gazebo_msgs::ContactsState> feet3_sub(nh,"/leg3_4_bumper",10);
   message_filters::Subscriber<gazebo_msgs::ContactsState> feet4_sub(nh,"/leg4_4_bumper",10);
   message_filters::TimeSynchronizer<sensor_msgs::Imu,sensor_msgs::JointState,gazebo_msgs::ContactsState, 
   gazebo_msgs::ContactsState, gazebo_msgs::ContactsState, gazebo_msgs::ContactsState> sync(imu_sub,joints_sub,feet1_sub,feet2_sub,feet3_sub,feet4_sub,10);
   sync.registerCallback(boost::bind(&sensorcallback,_1,_2,_3,_4,_5,_6));

   ros::spin();
   return 0;
}
edit retag flag offensive close merge delete

Comments

You don't seem to be using an ApproximateTimeSynchronizer. That requires all messages to have the exact same timestamp before your callback will be called. It's likely that imu + jointstates have the same timestamp, while the ContactState msgs have a slightly different timestamp.

gvdhoorn gravatar imagegvdhoorn ( 2019-05-11 07:58:23 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-05-11 19:58:09 -0600

crank gravatar image

updated 2019-05-11 20:11:14 -0600

Thank you for your help! But I still try another way to test, just as follow:

message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh,"/imu/data", 1);
message_filters::Subscriber<sensor_msgs::JointState> joints_sub(nh,"/joint_states",1);
message_filters::Subscriber<gazebo_msgs::ContactsState> feet1_sub(nh,"/leg1_4_bumper",1);
message_filters::Subscriber<gazebo_msgs::ContactsState> feet2_sub(nh,"/leg2_4_bumper",1);
message_filters::Subscriber<gazebo_msgs::ContactsState> feet3_sub(nh,"/leg3_4_bumper",1);
message_filters::Subscriber<gazebo_msgs::ContactsState> feet4_sub(nh,"/leg4_4_bumper",1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu,sensor_msgs::JointState,gazebo_msgs::ContactsState,gazebo_msgs::ContactsState,gazebo_msgs::ContactsState,gazebo_msgs::ContactsState> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), imu_sub,joints_sub,feet1_sub,feet2_sub,feet3_sub,feet4_sub);
sync.registerCallback(boost::bind(&sensorcallback,_1,_2,_3,_4,_5,_6));

but IMU+joint_states+foot_sensors cann't inter callback, is there stlii have something wrong ?

I forget to tell you, imu is 100hz, joints have 50hz, 4 foot_sensor have 50hz , when I use message fittler for imu + joints, the callback only have 40 hz , and for 4 foot_sensors, these only have 30hz, at now I don't know how to solve it, Thanks for your help!

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-05-11 07:06:01 -0600

Seen: 28 times

Last updated: May 11