0
$\begingroup$

Rosanswers logo

I have a sensor which measures a position of one frame and the other sensor measures the rotation. If I'm subscribing on those two sensor topics, how can i define a transform which gets both of the sensor data, position and rotation?

im also quite new to c++... Is it possible to write something like

void callbackFunc(const geometry_msgs::PointStampedConstPtr& msg, 
                  const sensor_msgs::ImuConstPrt& imuMsg){
//defining transformStamped and Broadcaster
tfStamped.translation.x = msg->point.x;
...// until translation.z
tf.Stamped.rotation.x  = imuMsg->orientation.x;
..// until rotation.w
}

int main(......){
ros::Subscriber sub = node.subscribe("....");
}

is it possible to realize it this way?

edit:

my current code is:

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Imu.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

void callback(const geometry_msgs::PointStampedConstPtr& msg,
                      const sensor_msgs::ImuConstPtr& imuMsg){

  static tf2_ros::TransformBroadcaster tf2_broadcaster;
  geometry_msgs::TransformStamped transformStamped;

  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "ts_frame";
  transformStamped.child_frame_id = "prism_frame";
  transformStamped.transform.translation.x = msg->point.x;
  transformStamped.transform.translation.y = msg->point.y;
  transformStamped.transform.translation.z = msg->point.z;
  tf2::Quaternion q;
  transformStamped.transform.rotation.x = imuMsg->orientation.x;
  transformStamped.transform.rotation.y = imuMsg->orientation.y;
  transformStamped.transform.rotation.z = imuMsg->orientation.z;
  transformStamped.transform.rotation.w = imuMsg->orientation.w;

  tf2_broadcaster.sendTransform(transformStamped);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "tf_ts2prism");
  ros::NodeHandle nh;

  message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh, "/imu/data_raw", 1);
  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub(nh, "/ts_points", 1);

  message_filters::TimeSynchronizer<sensor_msgs::Imu, geometry_msgs::PointStamped> sync(imu_sub, point_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();
  return 0;
}

the error message is:

In file included from /usr/include/boost/bind.hpp:22:0,
                 from /opt/ros/kinetic/include/ros/publisher.h:35,
                 from /opt/ros/kinetic/include/ros/node_handle.h:32,
                 from /opt/ros/kinetic/include/ros/ros.h:45,
                 from /home/mango/tut_ws/src/totalstation/src/tf_ts2prism.cpp:1:
/usr/include/boost/bind/bind.hpp: In instantiation of ‘void boost::_bi::list2<A1, A2>::operator()(boost::_bi::type<void>, F&, A&, int) [with F = void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&); A = boost::_bi::list9<const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&, const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&>; A1 = boost::arg<1>; A2 = boost::arg<2>]’:
/usr/include/boost/bind/bind.hpp:1102:50:   required from ‘boost::_bi::bind_t<R, F, L>::result_type boost::_bi::bind_t<R, F, L>::operator()(A1&&, A2&&, A3&&, A4&&, A5&&, A6&&, A7&&, A8&&, A9&&) [with A1 = const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&; A2 = const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&; A3 = const boost::shared_ptr<const message_filters::NullType>&; A4 = const boost::shared_ptr<const message_filters::NullType>&; A5 = const boost::shared_ptr<const message_filters::NullType>&; A6 = const boost::shared_ptr<const message_filters::NullType>&; A7 = const boost::shared_ptr<const message_filters::NullType>&; A8 = const boost::shared_ptr<const message_filters::NullType>&; A9 = const boost::shared_ptr<const message_filters::NullType>&; R = void; F = void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&); L = boost::_bi::list2<boost::arg<1>, boost::arg<2> >; boost::_bi::bind_t<R, F, L>::result_type = void]’
/usr/include/boost/bind/bind.hpp:827:35:   required from ‘void boost::_bi::list9<A1, A2, A3, A4, A5, A6, A7, A8, A9>::operator()(boost::_bi::type<void>, F&, A&, int) [with F = boost::_bi::bind_t<void, void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&), boost::_bi::list2<boost::arg<1>, boost::arg<2> > >; A = boost::_bi::list9<const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&, const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&>; A1 = boost::arg<1>; A2 = boost::arg<2>; A3 = boost::arg<3>; A4 = boost::arg<4>; A5 = boost::arg<5>; A6 = boost::arg<6>; A7 = boost::arg<7>; A8 = boost::arg<8>; A9 = boost::arg<9>]’
/usr/include/boost/bind/bind.hpp:1102:50:   required from ‘boost::_bi::bind_t<R, F, L>::result_type boost::_bi::bind_t<R, F, L>::operator()(A1&&, A2&&, A3&&, A4&&, A5&&, A6&&, A7&&, A8&&, A9&&) [with A1 = const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&; A2 = const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&; A3 = const boost::shared_ptr<const message_filters::NullType>&; A4 = const boost::shared_ptr<const message_filters::NullType>&; A5 = const boost::shared_ptr<const message_filters::NullType>&; A6 = const boost::shared_ptr<const message_filters::NullType>&; A7 = const boost::shared_ptr<const message_filters::NullType>&; A8 = const boost::shared_ptr<const message_filters::NullType>&; A9 = const boost::shared_ptr<const message_filters::NullType>&; R = boost::_bi::unspecified; F = boost::_bi::bind_t<void, void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&), boost::_bi::list2<boost::arg<1>, boost::arg<2> > >; L = boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> >; boost::_bi::bind_t<R, F, L>::result_type = void]’
/usr/include/boost/function/function_template.hpp:159:15:   required from ‘static void boost::detail::function::void_function_obj_invoker9<FunctionObj, R, T0, T1, T2, T3, T4, T5, T6, T7, T8>::invoke(boost::detail::function::function_buffer&, T0, T1, T2, T3, T4, T5, T6, T7, T8) [with FunctionObj = boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<void, void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&), boost::_bi::list2<boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >; R = void; T0 = const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&; T1 = const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&; T2 = const boost::shared_ptr<const message_filters::NullType>&; T3 = const boost::shared_ptr<const message_filters::NullType>&; T4 = const boost::shared_ptr<const message_filters::NullType>&; T5 = const boost::shared_ptr<const message_filters::NullType>&; T6 = const boost::shared_ptr<const message_filters::NullType>&; T7 = const boost::shared_ptr<const message_filters::NullType>&; T8 = const boost::shared_ptr<const message_filters::NullType>&]’
/usr/include/boost/function/function_template.hpp:940:38:   required from ‘void boost::function9<R, T1, T2, T3, T4, T5, T6, T7, T8, T9>::assign_to(Functor) [with Functor = boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<void, void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&), boost::_bi::list2<boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >; R = void; T0 = const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&; T1 = const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&; T2 = const boost::shared_ptr<const message_filters::NullType>&; T3 = const boost::shared_ptr<const message_filters::NullType>&; T4 = const boost::shared_ptr<const message_filters::NullType>&; T5 = const boost::shared_ptr<const message_filters::NullType>&; T6 = const boost::shared_ptr<const message_filters::NullType>&; T7 = const boost::shared_ptr<const message_filters::NullType>&; T8 = const boost::shared_ptr<const message_filters::NullType>&]’
/usr/include/boost/function/function_template.hpp:728:7:   required from ‘boost::function9<R, T1, T2, T3, T4, T5, T6, T7, T8, T9>::function9(Functor, typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type) [with Functor = boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<void, void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&), boost::_bi::list2<boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >; R = void; T0 = const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&; T1 = const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&; T2 = const boost::shared_ptr<const message_filters::NullType>&; T3 = const boost::shared_ptr<const message_filters::NullType>&; T4 = const boost::shared_ptr<const message_filters::NullType>&; T5 = const boost::shared_ptr<const message_filters::NullType>&; T6 = const boost::shared_ptr<const message_filters::NullType>&; T7 = const boost::shared_ptr<const message_filters::NullType>&; T8 = const boost::shared_ptr<const message_filters::NullType>&; typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type = int]’
/usr/include/boost/function/function_template.hpp:1077:16:   required from ‘boost::function<R(T0, T1, T2, T3, T4, T5, T6, T7, T8)>::function(Functor, typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type) [with Functor = boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<void, void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&), boost::_bi::list2<boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >; R = void; T0 = const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&; T1 = const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&; T2 = const boost::shared_ptr<const message_filters::NullType>&; T3 = const boost::shared_ptr<const message_filters::NullType>&; T4 = const boost::shared_ptr<const message_filters::NullType>&; T5 = const boost::shared_ptr<const message_filters::NullType>&; T6 = const boost::shared_ptr<const message_filters::NullType>&; T7 = const boost::shared_ptr<const message_filters::NullType>&; T8 = const boost::shared_ptr<const message_filters::NullType>&; typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type = int]’
/opt/ros/kinetic/include/message_filters/signal9.h:281:40:   required from ‘message_filters::Connection message_filters::Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8>::addCallback(C&) [with C = const boost::_bi::bind_t<void, void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&), boost::_bi::list2<boost::arg<1>, boost::arg<2> > >; M0 = sensor_msgs::Imu_<std::allocator<void> >; M1 = geometry_msgs::PointStamped_<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/kinetic/include/message_filters/synchronizer.h:310:40:   required from ‘message_filters::Connection message_filters::Synchronizer<Policy>::registerCallback(const C&) [with C = boost::_bi::bind_t<void, void (*)(const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >&), boost::_bi::list2<boost::arg<1>, boost::arg<2> > >; Policy = message_filters::sync_policies::ExactTime<sensor_msgs::Imu_<std::allocator<void> >, geometry_msgs::PointStamped_<std::allocator<void> >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType>]’
/home/mango/tut_ws/src/totalstation/src/tf_ts2prism.cpp:39:55:   required from here
/usr/include/boost/bind/bind.hpp:313:35: error: invalid initialization of reference of type ‘const boost::shared_ptr<const geometry_msgs::PointStamped_<std::allocator<void> > >&’ from expression of type ‘const boost::shared_ptr<const sensor_msgs::Imu_<std::allocator<void> > >’
         unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_]);
                                   ^
totalstation/CMakeFiles/tf_ts2prism.dir/build.make:62: recipe for target 'totalstation/CMakeFiles/tf_ts2prism.dir/src/tf_ts2prism.cpp.o' failed
make[2]: *** [totalstation/CMakeFiles/tf_ts2prism.dir/src/tf_ts2prism.cpp.o] Error 1
CMakeFiles/Makefile2:2784: recipe for target 'totalstation/CMakeFiles/tf_ts2prism.dir/all' failed
make[1]: *** [totalstation/CMakeFiles/tf_ts2prism.dir/all] Error 2
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

Originally posted by rubot on ROS Answers with karma: 15 on 2018-07-12

Post score: 0

Improve this question
$\endgroup$

2 Answers 2

Reset to default
0
$\begingroup$

Rosanswers logo

Yes there is a package to help you achieve this quite easily.

The message_filters package specifically the time synchroniser filter can listen to two different topics and execute a callback with the messages of both topics when they arrive at a very similar time.

Hope this helps.

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-07-12

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by rubot on 2018-07-12:
Thanks a lot! This was exactly what i was searching for... unfortunately by using the line sync.registerCallback(boost::bind(&callback, _1, _2));

i get a huge error message containing:

/usr/include/boost/smart_ptr/shared_ptr.hpp: In instantiation of ‘boost::shared_ptr<T>::shared_ptr(Y*) [with

Comment by rubot on 2018-07-12:
By compiling the example code everything works fine, but when i add a sensor_msgs::Imu object to the message_filters::Subscriber<> boost starts to complain

Comment by PeteBlackerThe3rd on 2018-07-12:
Did you update the message types in the callback definition (line 9), the subscribers (lines 20,21) and the synchronizer (line 22)? If that doesn't solve your problem can you update the content of your original question with your source code and the full error message.

Comment by rubot on 2018-07-12:
yes, i did. i added my code and the error message to the question

Comment by PeteBlackerThe3rd on 2018-07-12:
I'm as confused as you are at the moment. Hopefully someone else can figure this one out!

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

Since my second problem had something to do with boost i decided to post an other question here. The problem was that i declared the message_filters::TimeSynchronizer with Imu first and then PointStamped on the second position, but in the callback function i wrote it the other way around, so boost couldn't handle the types.

So either it's possible to change my code to boost::bind(&callback, _2, _1) or change my callback to callback(const geometry_msgs::PointStampedConstPtr& msg, const sensor_msgs::ImuConstPtr& imuMsg)

Originally posted by rubot with karma: 15 on 2018-07-12

This answer was NOT ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.