ros - ミラー化された tf を公開するには?

Question

ナビゲーションスタックにrf2o laser_to_odometryを使用しようとしました。しかし、ロボットを動かすと tf が間違った方向に移動します。つまり、ロボットを右に動かすと、tf は左に移動します。

LIDAR の構成: Z 方向を中心に LIDAR を 90 度回転させ、上下逆さまにすると、-180 度から 0 度までの範囲の半分だけをスキャンします。

tf の設定は正しいです。マップの作成中に hector slam で再度確認しましたargs="0.2 0 0 1.57 3.14 0 /base_link /laser_frame 40。また、LIDAR を上下逆さまにしない限り、この問題は発生しません (また、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 の起動ファイル (ナビゲーションスタックの 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>

ros - ミラー化された tf を公開するには?

1 に答える 1

Related

Reference