Как опубликовать зеркальный тф?

Question

Я попытался использовать rf2o laser_to_odometry для стека навигации.Однако, когда я двигаю робота, tf движется в неправильной ориентации.то есть, когда я перемещаю робота вправо, tf идет влево.

Для конфигурации лидара: я повернул свой лидар на 90 градусов вокруг z и перевернул вверх дном, и он сканирует только половину диапазона,от -180 до 0 градусов.

tf настройка верна, я снова проверил с гектором шлема при создании карты, args="0.2 0 0 1.57 3.14 0 /base_link /laser_frame 40, также эта проблема не возникает, пока я не перевернул лидар вверх дном(а также изменил статический издатель tf на обратно args="0.2 0 0 0 0 0 /base_link /laser_frame 40)

Ниже приведен код (также можно найти здесь) Я нахожу в CLaserOdometry2DNode.cpp пакета rf2o... Будет ли здесь что-то не так, что tf неправильно читается в пакете, или есть какой-то способ отредактировать это, чтобы отразить tf (левое и правое, то есть зеркало вдоль оси y)?

спасибо, если кто-нибудь может помочь !!

    // Set laser pose on the robot (through tF)
    // This allow estimation of the odometry with respect to the robot base reference system.
    tf::StampedTransform transform;
    transform.setIdentity();
    try
    {
      tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
      retrieved = true;
    }
    catch (tf::TransformException &ex)
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      retrieved = false;
    }

    //TF:transform -> Eigen::Isometry3d

    const tf::Matrix3x3 &basis = transform.getBasis();
    Eigen::Matrix3d R;

    for(int r = 0; r < 3; r++)
      for(int c = 0; c < 3; c++)
        R(r,c) = basis[r][c];

    Pose3d laser_tf(R);

    const tf::Vector3 &t = transform.getOrigin();
    laser_tf.translation()(0) = t[0];
    laser_tf.translation()(1) = t[1];
    laser_tf.translation()(2) = t[2];

    setLaserPose(laser_tf);

    return retrieved;
  }

РЕДАКТИРОВАТЬ: Вот дерево TF Дерево TF

Я попытался понять пакет,зная, что после этого слушать ТФ, который являетсяode, прикрепленный там, передает значение setLaserPose:

void CLaserOdometry2D::setLaserPose(const Pose3d& laser_pose)
{
  //Set laser pose on the robot

  laser_pose_on_robot_     = laser_pose;
  laser_pose_on_robot_inv_ = laser_pose_on_robot_.inverse();
}

и преобразует окончательный результат, который представляет собой robot_pose_ относительно tf, см. ниже: Однако я считаю, что оба сообщения ROS_INFO_COND одинаковы,без разницы .. Кажется laser_pose_on_robot_inv_ не преобразует результат ... Могу ли я дать несколько советов о том, как с этим справиться?

  ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",
            laser_pose_.translation()(0),
            laser_pose_.translation()(1),
            rf2o::getYaw(laser_pose_.rotation()));

  //Compose Transformations
  robot_pose_ = laser_pose_ * laser_pose_on_robot_inv_;

  ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]",
            robot_pose_.translation()(0),
            robot_pose_.translation()(1),
            rf2o::getYaw(robot_pose_.rotation()));

также tf в rviz enterописание изображения здесь

файл запуска для rf2o (который является robot_configuration в стеке навигации)

<launch>
  <node name="ydlidar_node"  pkg="ydlidar"  type="ydlidar_node" output="screen">
    <param name="port"         type="string" value="/dev/ydlidar"/>  
    <param name="baudrate"     type="int"    value="115200"/>
    <param name="frame_id"     type="string" value="laser_frame"/>
    <param name="angle_fixed"  type="bool"   value="true"/>
    <param name="low_exposure"  type="bool"   value="false"/>
    <param name="heartbeat"    type="bool"   value="false"/>
    <param name="resolution_fixed"    type="bool"   value="true"/>
    <param name="angle_min"    type="double" value="-180" />
    <param name="angle_max"    type="double" value="0" />
    <param name="range_min"    type="double" value="0.08" />
    <param name="range_max"    type="double" value="16.0" />
    <param name="ignore_array" type="string" value="" />
    <param name="samp_rate"    type="int"    value="9"/>
    <param name="frequency"    type="double" value="7"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link"
    args="0 0 0 0 0 0 /base_footprint /base_link 40" />
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser_frame"
    args="0.3 0 0 1.57 3.14 0 /base_link /laser_frame 40" />

  <node pkg="rf2o_laser_odometry" type="rf2o_laser_odometry_node" name="rf2o_laser_odometry" output="screen">
    <param name="laser_scan_topic" value="/scan"/>        # topic where the lidar scans are being published
    <param name="publish_tf" value="true" />                   # wheter or not to publish the tf::transform (base->odom)
    <param name="base_frame_id" value="/base_footprint"/>      # frame_id (tf) of the mobile robot base. A tf transform from the laser_frame to the base_frame is mandatory
    <param name="odom_frame_id" value="/odom" />                # frame_id (tf) to publish the odometry estimations    
    <param name="init_pose_from_topic" value="" /> # (Odom topic) Leave empty to start at point (0,0)
    <param name="freq" value="40.0"/>                            # Execution frequency.
  </node>

  <node pkg="rviz" type="rviz" name="rviz" 
    args="-d $(find mobilet_2dnav)/rviz_cfg/configuration.rviz"/>

</launch>

Answer 1

Большое спасибо Вику за ведение. Таким образом, кажется, что tf еще должен быть в буфере при прослушивании Теперь он работает после ожидания преобразования:

Для CLaserOdometry2DNode.cpp, Я добавляю строку

tf_listener.waitForTransform("/base_footprint","/laser_frame", ros::Time(), ros::Duration(5.0));

до tf_listener.lookupTransform

надеюсь, это поможет тем, кто использует rf2o laser_to_odometry.