move_to_gate_actionで、goal地点を計算した際の姿勢の演算結果が/move_base_simple/goalの表示と違う

[sarubito]

No description provided.

[sarubito]

1. action_node.hppのbetween関数の252行目と253行目の間に以下を追記

RCLCPP_INFO(get_logger(), "debug x : [%f]", v.x);
RCLCPP_INFO(get_logger(), "debug y : [%f]", v.y);

もしくは、259行目以下の条件文を以下のように変更した

if ((v.y * v_robot.x - v.x * v_robot.y) >= (-v.y * v_robot.x + v.x * v_robot.y)) {
      goal_rpy.z = std::atan2(-v.x, v.y);
      RCLCPP_INFO(get_logger(), "debug x : [%f]", goal_rpy.z);
    } else {
      goal_rpy.z = std::atan2(v.x, -v.y);
      RCLCPP_INFO(get_logger(), "debug y : [%f]", goal_rpy.z);
    }

2. ros2 launch navi_sim with_planner.launch.py behavior_config_filepath:=config/move_to_gate.yamlを実行
   252行目以下に追記する場合と259行目以下の条件文を変更する場合の2つのパターンどちらもバグが直る。

[sarubito]

バグの様子

[hakuturu583]

かなり不可解ですね、/move_base_simple/goalトピックを出しているノードは複数あるはずなので、そのうちどれが奇妙な出力をだしているか確認する必要があるかもしれません。

[hakuturu583]

なんだかこれ、90度ずれていますね