Problem on IMU frame tf (Cannot broadcast imu frame tf )
Recently I wanted to add my imu data to my robot. However, the imu is mounted at the front of the robot but not at the centre of it. Therefore, a tf transform is needed to get the correct data. I tried to write a broadcaster which subscribes to the imu data and then broadcasts the transform, but the transform is not stated in the tf_tree. When using the robot_localisation package to fuse the imu data with other odometry data, it gives out the warning that the imu frame does not exist which automatically mounts the data coming from the origin of the robot. Which is not what I want :( Here is my code.
sensor_msgs::Imu imu;
tf2_ros::StaticTransformBroadcaster br;
geometry_msgs::TransformStamped transform;
void imubroadcaster(const ins::ASENSING msg)
{
double degtorad = 3.141592654 / 180;
imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(msg.roll * degtorad, -1.0 * msg.pitch * degtorad, -1.0*(msg.azimuth + 90) * degtorad);
// setup trasnsform
transform.header.frame_id = "base_link";
transform.child_frame_id = "imu";
// setup origin of imu frame
transform.transform.translation.x = 0.4;
transform.transform.translation.y = 0.0;
transform.transform.translation.z = 0.0;
// getting q from imu data
transform.transform.rotation.x = imu.orientation.x;
transform.transform.rotation.y = imu.orientation.y;
transform.transform.rotation.z = imu.orientation.z;
transform.transform.rotation.w = imu.orientation.w;
transform.header.stamp = ros::Time::now();
// // broadcast transform
br.sendTransform(transform);
ROS_INFO("[%s]tgrerg", transform.transform.rotation.w);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_frame_broadcaster");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/INS/ASENSING_INS", 1000, imubroadcaster);
ros::Rate rate(10.0);
while (n.ok()){
rate.sleep();
printf("sending\n");
}
};
I tried to read whether data is written into transform or not in the line ROS_INFO, all the data are correctly written to it. But it just cannot broadcast the transform !!
Thanks for any help if provided !!