1

我尝试将rf2o laser_to_odometry用于导航堆栈。但是，当我移动机器人时，tf 的方向错误。即当我将机器人向右移动时，tf 向左移动。

对于激光雷达的配置：我将激光雷达绕 z 方向旋转 90 度并倒置，它只扫描一半范围，从 -180 到 0 度。

tf 设置是正确的，我在创建地图时再次检查了 hector slam，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）

以下是我在 rf2o 包的 CLaserOdometry2DNode.cpp 中找到的代码（也可以在此处找到） 。这里是否有问题导致包中的 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 树

我试图理解这个包，知道在它监听 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 在此处 输入图像描述

rf2o 的启动文件（导航堆栈中的 robots_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>
4

1 回答 1

1

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

对于 CLaserOdometry2DNode.cpp，我添加了一行

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

之前tf_listener.lookupTransform

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

于 2019-04-11T09:42:46.593 回答