Synchronize imu joint_states foot_sensor errors
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;
}
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 theContactState
msgs have a slightly different timestamp.