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 hectormapping node in rviz. I have a 3 wheel robot (with a caster wheel). I am using the /slamoutpose published from the hectormapping 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*q3),1 - 2*(pow(q1,2)+pow(q2,2)));
  angle_y = asin(2*(q0*q2 - q3*q1));
  angle_z = atan2(2*(q0*q3 + q1*q2),1 - 2*(pow(q2,2)+pow(q3,2)));
  transform.setOrigin(tf::Vector3(-msg->pose.position.x, -msg->pose.position.y, -msg->pose.position.z));
  tf::Quaternion rot;
  rot.setRPY(angle_x,angle_y,angle_z);
  transform.setRotation(rot);

So probably the problem is relevant to the topic (/slam_out_pose published by the hector_mapping node) from which I get the messages ?

EDITED 3rd:

I increased the rate of broadcasting and the behavior presented larger errors between the true orientation in gazebo and the orientation in rviz. I decreased the rate and the behavior still presented an error between the two orientations but much more smaller.

StampedTransform (const tf::Transform &input, const ros::Time &timestamp, const std::string &frame_id, const std::string &child_frame_id)

 broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(),  "map", "base_link"));