# 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)

Hello,

I have written the following broadcaster node:

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

void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
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));
}

int main(int argc, char** argv){

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.

Chris

EDITED:

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

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

void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
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){
}

int main(int argc, char** argv){

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 ...
edit retag close merge delete

Sort by » oldest newest most voted

Your base_link and map are in the wrong place. StampedTransform is defined as:

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


I think base_link should be the child_frame, so try this:

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


EDIT

I guess maybe you should check if your orientation msg is right. I mean, the orientation is represented by quaternion(x,y,z,w), and x*x + y*y + z*z + w*w = 1.

more

The child frame should be the map frame not the base_link frame. It's a tf from base_link to map, which is needed for autonomous exploration. That's why I use minuses, because it's like the map is moving in relation to the base_link not the opposite.

( 2016-08-22 03:31:21 -0500 )edit

I gave it a try ....the message is right and I have the exact same behavior.

( 2016-08-22 09:15:06 -0500 )edit