Ask Your Question
0

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)

asked 2016-08-22 03:12:05 -0500

patrchri gravatar image

updated 2016-08-26 08:03:36 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-08-22 03:26:02 -0500

Shay gravatar image

updated 2016-08-22 08:06:03 -0500

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.

edit flag offensive delete link more

Comments

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.

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

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

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-08-22 03:12:05 -0500

Seen: 814 times

Last updated: Aug 26 '16