search

OUXT-Polaris / robotx_behavior_tree

5 stars 1 forks source link

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

Open sarubito opened 2 years ago

sarubito commented 2 years ago
  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 commented 2 years ago

バグの様子 Screenshot from 2022-09-18 00-27-21

hakuturu583 commented 2 years ago

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

hakuturu583 commented 2 years ago

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