How can i apply two sensor messages on one frame?
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 ...