Ask Your Question
0

sensor fusion of 2 Points using message_filters

asked 2018-06-08 01:52:14 -0500

Markus gravatar image

updated 2018-06-08 02:40:27 -0500

Hey there,

I would like to write a nod that does some sensor fusion. The node should subscribe 2 Topics of type geometry_msgs::Point

if the two messages have the approximate same time_stamp simple the median should be calculated.

Now I would like to use ros_message_filters_indigo and in particular the approximate_message filter algorithm.

As in ros_message_filters_indigo, this code compiles for me:

  message_filters::Subscriber<sensor_msgs::Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<sensor_msgs::Image> image2_sub(nh, "image2", 1);

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);

Now I would like to do that with Point messages:

  message_filters::Subscriber<geometry_msgs::Point> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<geometry_msgs::Point> image2_sub(nh, "image2", 1);

  typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::Point, geometry_msgs::Point> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);

This code however does not build and gives a massive error message:

In file included from /hri/localdisk/markus/ros-alex/src/point_fusion/include/point_fusion/point_fusion_node.h:20:0,
                 from /hri/localdisk/markus/ros-alex/src/point_fusion/src/point_fusion_node.cpp:1:
/opt/ros/indigo/include/message_filters/sync_policies/approximate_time.h: In instantiation of ‘void message_filters::sync_policies::ApproximateTime<M0, M1, M2, M3, M4, M5, M6, M7, M8>::checkInterMessageBound() [with int i = 0; M0 = geometry_msgs::Point_<std::allocator<void> >; M1 = geometry_msgs::Point_<std::allocator<void> >; M2 = message_filters::NullType; M3 = message_filters::NullType; M4 = message_filters::NullType; M5 = message_filters::NullType; M6 = message_filters::NullType; M7 = message_filters::NullType; M8 = message_filters::NullType]’:
/opt/ros/indigo/include/message_filters/sync_policies/approximate_time.h:218:29:   required from ‘void message_filters::sync_policies::ApproximateTime<M0, M1, M2, M3, M4, M5, M6, M7, M8>::add(const typename boost::mpl::at_c<typename message_filters::PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>::Events, i>::type&) [with int i = 0; M0 = geometry_msgs::Point_<std::allocator<void> >; M1 = geometry_msgs::Point_<std::allocator<void> >; M2 = message_filters::NullType; M3 = message_filters::NullType; M4 = message_filters::NullType; M5 = message_filters::NullType; M6 = message_filters::NullType; M7 = message_filters::NullType; M8 = message_filters::NullType; typename boost::mpl::at_c<typename message_filters::PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>::Events, i>::type = ros::MessageEvent<const geometry_msgs::Point_<std::allocator<void> > >]’
/opt/ros/indigo/include/message_filters/synchronizer.h:358:5:   required from ‘void message_filters::Synchronizer<Policy>::cb(const typename boost::mpl::at_c<typename Policy::Events, i>::type&) [with int i = 0; Policy = message_filters::sync_policies::ApproximateTime<geometry_msgs::Point_<std::allocator<void> >, geometry_msgs::Point_<std::allocator<void> > >; typename boost::mpl::at_c<typename Policy::Events, i>::type = ros::MessageEvent<const geometry_msgs::Point_<std::allocator<void> > >]’
/opt/ros/indigo/include/message_filters/synchronizer.h:290:138:   required from ‘void message_filters::Synchronizer<Policy>::connectInput(F0&, F1&, F2&, F3&, F4&, F5&, F6&, F7&, F8&) [with F0 = message_filters::Subscriber<geometry_msgs::Point_<std::allocator<void> > >; F1 = message_filters::Subscriber<geometry_msgs::Point_<std::allocator<void> > >; F2 = message_filters::NullFilter<message_filters::NullType>; F3 = message_filters::NullFilter<message_filters::NullType>; F4 = message_filters::NullFilter<message_filters::NullType>; F5 = message_filters::NullFilter<message_filters::NullType>; F6 = message_filters::NullFilter<message_filters::NullType ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2018-06-08 02:41:58 -0500

Markus gravatar image

--> Solved my own problem

Point is not working PointStamped is required!

  point_1_topic_ = nh.param("point_fusion/point_1_topic", std::string("/turtlebot3/ball_tracking/first_ball_state/position"));
  point_2_topic_ = nh.param("point_fusion/point_2_topic", std::string("/ball_tracking_depth/first_ball_state"));


  message_filters::Subscriber<geometry_msgs::PointStamped> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<geometry_msgs::PointStamped> image2_sub(nh, "image2", 1);

  typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PointStamped, geometry_msgs::PointStamped> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
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: 2018-06-08 01:52:14 -0500

Seen: 46 times

Last updated: Jun 08 '18