diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 397023a..95d4ba5 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -520,8 +520,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( const double distance_allow_rotate = min_journey_squared_; const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1); allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; - robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose; - allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 3.0; + // robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose; + // allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 3.0; allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);