Problem from base_link to map transform during rotation(video) - Error between the true orientation of robot and the orientation taken from tf by rviz (rviz/RobotModel)

edit

Hello,

I have written the following broadcaster node:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>

void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
  static tf::TransformBroadcaster broadcaster;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(-msg->pose.position.x, -msg->pose.position.y, -msg->pose.position.z));
  transform.setRotation(tf::Quaternion(-msg->pose.orientation.x, -msg->pose.orientation.y, -msg->pose.orientation.z, msg->pose.orientation.w));
  broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "map"));
  ROS_INFO("Broadcasting between map and base_link successful !!!");
}

int main(int argc, char** argv){
  ros::init(argc, argv, "tf_broadcaster_node");

  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe("/slam_out_pose", 7, &poseCallback);
  ros::spin();
  return 0;
};

I used teleoperation for my robot in gazebo and tested the broadcasting of the tf. I displayed the RobotModel in the Map published from the hector_mapping node in rviz. I have a 3 wheel robot (with a caster wheel). I am using the /slam_out_pose published from the hector_mapping node to get the rotations and linear movement. I don't know if this is wrong or correct. The results can be viewed in this video. As you can see during the rotation of the robot the robotmodel in rviz which is taken from the tf by the rviz node, has totally wrong behavior. The robot in gazebo is rotating around the z axis that comes from it's origin (around itself) and the RobotModel in rviz rotates in another way.

Could you please tell me how to fix this?

The problem is in the line

  transform.setRotation(tf::Quaternion(-msg->pose.orientation.x, -msg->pose.orientation.y, -msg->pose.orientation.z, msg->pose.orientation.w));

but I don't know how to fix this.

Thank you for your answers in advance.

Chris

EDITED:

I also tried this code (after Shay's suggestion):

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <math.h>

void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
  static tf::TransformBroadcaster broadcaster;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(-msg->pose.position.x, -msg->pose.position.y, -msg->pose.position.z));
  transform.setRotation(tf::Quaternion(-msg->pose.orientation.x, -msg->pose.orientation.y, -msg->pose.orientation.z, msg->pose.orientation.w));
  if(pow(msg->pose.orientation.x,2)+pow(msg->pose.orientation.y,2)+pow(msg->pose.orientation.z,2)+pow(msg->pose.orientation.w,2) == 1){
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "map"));
    ROS_INFO("Broadcasting from base_link to map successful !!!");}
}

int main(int argc, char** argv){
  ros::init(argc, argv, "tf_broadcaster_node");

  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe("/slam_out_pose", 7, &poseCallback);
  ros::spin();
  return 0;
};

The orientation message is right and the problem still remains. The RobotModel has the exact same behavior.

EDITED 2nd:

I also tried the use of RPY angles and the result remains the same.

  tf::Transform transform;
  q0 = msg->pose.orientation.w;
  q1 = -msg->pose.orientation.x;
  q2 = -msg->pose.orientation.y;
  q3 = -msg->pose.orientation.z;
  angle_x = atan2(2*(q0*q1 + q2 ...