nav2_controller::SimpleGoalChecker not working properly on Xavier NX

edit

Hi, So I am running nav2 rolling on Xavier NX.

While running after 40 waypoints or so the goal checker stops working. So now the navigation never completes. The controller(RPP) works fine, it makes the bot stop near the goal, but as the goal_checker is not working so the navigation never completes and the bot stays there forever.

The analogy I followed:

• inside simple_goal 0 0 6.019 -0.570531 The log from this part, which is inside the file simple_goal_checker.cpp:

  bool SimpleGoalChecker::isGoalReached(
    const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
    const geometry_msgs::msg::Twist &)
  {
    if (check_xy_) {
      // RCLCPP_INFO(rclcpp::get_logger(),"inside simple_goal %f %f %f %f",query_pose.position.x, query_pose.position.x, goal_pose.position.x, goal_pose.position.y );
      std::cout<<"inside simple_goal"<<query_pose.position.x<< " "<<query_pose.position.y<<" "<<goal_pose.position.x<<" "<<goal_pose.position.y <<std::endl;
  
      double dx = query_pose.position.x - goal_pose.position.x,
        dy = query_pose.position.y - goal_pose.position.y;
      if (dx * dx + dy * dy > xy_goal_tolerance_sq_) {
        return false;
      }
      // We are within the window
      // If we are stateful, change the state.
      if (stateful_) {
        check_xy_ = false;
      }
    }
    double dyaw = angles::shortest_angular_distance(
      tf2::getYaw(query_pose.orientation),
      tf2::getYaw(goal_pose.orientation));
    return fabs(dyaw) < yaw_goal_tolerance_;
  }
  

  • inside controller.cpp !getRobotPose(pose) said: 0.000000 0.000000 The log from this part, which was inside the file controller_server.cpp:

       bool ControllerServer::isGoalReached()
        {
          geometry_msgs::msg::PoseStamped pose;
    
          if (!getRobotPose(pose)) {
            RCLCPP_INFO(get_logger(), "Unable to get pose");
            return false;
          }
          else{
            // RCLCPP_INFO(get_logger(), "inside controller.cpp !getRobotPose(pose) Did not return anything");
          }
    
          RCLCPP_INFO(get_logger(), "inside controller.cpp !getRobotPose(pose) said: %f %f", pose.pose.position.x, pose.pose.position.y);
    
          nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
          geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist);
          return goal_checkers_[current_goal_checker_]->isGoalReached(pose.pose, end_pose_, velocity);
        }
    

  • local_dist : The distance from the goal as seen by local planner

Logs:

• When the values passed to the goal checker when navigation is successful. C:\fakepath\Screenshot from 2021-12-17 19-18-08.png

As you can see, the goal_pose almost tends to zero. The tolerance here is 0.06m. Hence inside the region.The same is shown by the local_dist variable inside the controller server.

• When the values passed are not correct
  • C:\fakepath\Screenshot from 2021-12-17 19-19-46.png
  • C:\fakepath\Screenshot from 2021-12-17 19-20-03.png
  • C:\fakepath\Screenshot from 2021-12-17 19-20-18.png

As you can see in the pictures the values of inside simple_goal don't change continuously, but they change abruptly (happens in the middle of every picture). Where as the value of local_dist changes continuously.

And after the last picture the values don't change and the bot remains in one place only.

If any one has faced the same issue kindly help.

Note: This happens only on Xavier NX, but not on machines with higher compute like NUC and i7 gen laptops.

Thanks.