Laser rotates with odom

edit

I can't figure out this strange thing that I am getting with TF.

I am using a different localization to AMCL and that localization publishes Map to Base TF. If I will use that and make a seperate TF from Map to Odom with this kind of TF tree:

I get this good TF where laser beams do not move with the robot (fixed frame: Laser):

LINK to video

Now when I disable the Map to Base TF and write my own TF from Map to Odom using the TF provided by the localizer (which again is Map to Base):

(msg is the TransformStamped message of TF between Map and Base):

tf2::Transform tf_map_odom;

    tf_map_odom.setOrigin(tf2::Vector3(
        msg->transform.translation.x,
        msg->transform.translation.y, 0));

    tf2::Quaternion q(msg->transform.rotation.x,
        msg->transform.rotation.y,
        msg->transform.rotation.z,
        msg->transform.rotation.w);

    //tf::quaternionMsgToTF(this->_odom.pose.pose.orientation, qq);
    tf2::Quaternion qq(this->_odom.pose.pose.orientation.x,
        _odom.pose.pose.orientation.y,
        _odom.pose.pose.orientation.z,
        _odom.pose.pose.orientation.w);
    q = q * qq.inverse();

    tf_map_odom.setRotation(q);

    //const tf2::Stamped<tf2::Transform> tf_stamped_map_odom(tf_map_odom, msg->header.stamp, "map");

    //this->_map_to_odom = toMsg(tf_stamped_map_odom);

    this->_map_to_odom.header.stamp = msg->header.stamp;
    this->_map_to_odom.header.frame_id = "map";
    this->_map_to_odom.child_frame_id = "odom";
    this->_map_to_odom.transform.translation.x = tf_map_odom.getOrigin().getX();
    this->_map_to_odom.transform.translation.y = tf_map_odom.getOrigin().getY();
    this->_map_to_odom.transform.translation.z = tf_map_odom.getOrigin().getZ();

    this->_map_to_odom.transform.rotation.x = tf_map_odom.getRotation().getX();
    this->_map_to_odom.transform.rotation.y = tf_map_odom.getRotation().getY();
    this->_map_to_odom.transform.rotation.z = tf_map_odom.getRotation().getZ();
    this->_map_to_odom.transform.rotation.w = tf_map_odom.getRotation().getW();


    _tfb.sendTransform(this->_map_to_odom);

I get this behaviour (where Laser beams move together with Robot):

LINK to the video

and this TF:

What am I doing wrong?