Try110
commented
3 days ago I think freesapce_planner should be more strict in determining the end of a task than mission_planner, which can ensure that all tasks can be terminated; but if the free determination condition is changed too strict,
Checklist
Description
The stop judgment method for freespace_planner is inconsistent with that for mission_planner; The stop judgment of mission_planner takes into account the velocities in three directions of xyz, while the velocities in x-direction are considered in freesspace_planner
` //mission_planner bool VehicleStopCheckerBase::isVehicleStopped(const double stop_duration) const { if (twistbuffer.empty()) { return false; } constexpr double squared_stopvelocity = 1e-3 * 1e-3; const auto now = clock->now(); const auto time_buffer_back = now - twistbuffer.back().header.stamp; if (time_buffer_back.seconds() < stop_duration) { return false; } // Get velocities within stop_duration for (const auto & velocity : twistbuffer) { double x = velocity.twist.linear.x; double y = velocity.twist.linear.y; double z = velocity.twist.linear.z; double v = (x x) + (y y) + (z * z); if (squared_stop_velocity <= v) { return false; } const auto time_diff = now - velocity.header.stamp; if (time_diff.seconds() >= stop_duration) { break; } }
return true; }`
//freespace_planner bool isStopped(const std::deque<Odometry::ConstSharedPtr> &odom_buffer, const double th_stopped_velocity_mps) { for (const auto &odom : odom_buffer) { if (std::abs(odom->twist.twist.linear.x) > th_stopped_velocity_mps) { return false; } } return true; }Expected behavior
When Fressspace_planner determines that the task is over,mission_planner should also determine that the task is over, otherwise there will be no executor
Actual behavior
However, because this speed determination is inconsistent, and even the mission_planner is stricter than the freespace_planner, it will occasionally lead to the freespace_planner judging that the task has ended, but the mission_planner will not complete the task. Under these circumstances, the mission never ends.
Steps to reproduce
sometimes
Versions
No response
Possible causes
The stop judgment of mission_planner takes into account the velocities in three directions of xyz, while the velocities in x-direction are considered in freesspace_planner
` //mission_planner bool VehicleStopCheckerBase::isVehicleStopped(const double stop_duration) const { if (twistbuffer.empty()) { return false; } constexpr double squared_stopvelocity = 1e-3 * 1e-3; const auto now = clock->now(); const auto time_buffer_back = now - twistbuffer.back().header.stamp; if (time_buffer_back.seconds() < stop_duration) { return false; } // Get velocities within stop_duration for (const auto & velocity : twistbuffer) { double x = velocity.twist.linear.x; double y = velocity.twist.linear.y; double z = velocity.twist.linear.z; double v = (x x) + (y y) + (z * z); if (squared_stop_velocity <= v) { return false; } const auto time_diff = now - velocity.header.stamp; if (time_diff.seconds() >= stop_duration) { break; } }
return true; }`
//freespace_planner bool isStopped(const std::deque<Odometry::ConstSharedPtr> &odom_buffer, const double th_stopped_velocity_mps) { for (const auto &odom : odom_buffer) { if (std::abs(odom->twist.twist.linear.x) > th_stopped_velocity_mps) { return false; } } return true; }Additional context
No response