Question

我试图将rf2o laser_to_odometry用于导航堆栈。但是，当我移动机器人时，tf方向错误。也就是说，当我将机器人向右移动时，tf会向左移动。

对于激光雷达的配置：我将激光雷达沿z方向旋转了90度，然后上下翻转，并且仅扫描了从-180度到0度的一半范围。

tf设置正确，我在创建地图args="0.2 0 0 1.57 3.14 0 /base_link /laser_frame 40时再次使用了hector slam检查，而且当我没有将激光雷达上下颠倒时也没有发生此问题（并且还更改了tf静态发布者回到args="0.2 0 0 0 0 0 /base_link /laser_frame 40）

以下是我在rf2o软件包的CLaserOdometry2DNode.cpp中找到的代码(also can be found here)。.这是否是错误的原因，导致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 tree

我试图了解该程序包，知道它在侦听tf（那里附加的代码）之后，将值传递给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();
}

并转换相对于tf的最终结果robot_pose_，如下所示：但是，我发现两个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()));

在rviz中也是tf enter image description here

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

非常感谢Vik的领导。因此，似乎在收听时tf尚未处于缓冲区中。等待转换后，它现在可以工作：

对于CLaserOdometry2DNode.cpp，我添加一行

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

在tf_listener.lookupTransform

之前

希望这对使用rf2o laser_to_odometry的用户有所帮助。

如何发布镜像的tf？

1 个答案: