Added promixity BT node and BT tree #4620
Conversation
|
|
||
| } // namespace nav2_behavior_tree | ||
|
|
||
| #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_GOAL_NEARBY_CONDITION_HPP_ |
There was a problem hiding this comment.
What should I correct in this case?
| * @brief A BT::ConditionNode that returns SUCCESS when the IsGoalNearby | ||
| * service returns true and FAILURE otherwise | ||
| */ | ||
| class IsGoalNearbyCondition : public BT::ConditionNode |
| #include "behaviortree_cpp/bt_factory.h" | ||
| BT_REGISTER_NODES(factory) | ||
| { | ||
| factory.registerNodeType<nav2_behavior_tree::IsGoalNearbyCondition>("IsGoalNearby"); |
There was a problem hiding this comment.
Need to add this to the nav2 node index
| const nav_msgs::msg::Path& goal_path, | ||
| const double& prox_thr) | ||
| { | ||
| return nav2_util::geometry_utils::calculate_path_length(goal_path, 0) < prox_thr; |
There was a problem hiding this comment.
That's checking the last path length, but not the robot's proximity. If you replan every 10 seconds (or only on events) then this wouldn't tell you much about the robot's proximity to the goal based on the last path marker.
I think this either needs to
- find the path's closest point like we do in the Controller plugins [1] which means we need to track the last path index to know where to search starting from on each call up to some maximum distance
- just be based on a distance check from the robot's current pose -- which has problems if the path have overlapping segments from Navigate Through Poses.
There was a problem hiding this comment.
I am trying to implement the first idea but I am not sure if I've understood it correctly. It's should return a value to the closest point in the path (in case of straight line it would be a next point on the line, and in case of complex path not necessarily)?
Here is my current fragment of the code with that idea:
BT::NodeStatus IsGoalNearbyCondition::tick()
{
nav_msgs::msg::Path path;
double prox_thr;
getInput("path", path);
getInput("proximity_threshold", prox_thr);
geometry_msgs::msg::PoseStamped pose; // robot pose in map frame
nav2_util::getCurrentPose(
pose, *tf_buffer_,
"map", "base_link", 0.05);
geometry_msgs::msg::PoseStamped robot_pose; // robot_pose in path frame
if (!transformPose(path.header.frame_id, pose, robot_pose)) {
return BT::NodeStatus::FAILURE;
}
auto closest_pose_upper_bound =
nav2_util::geometry_utils::first_after_integrated_distance(
path.poses.begin(), path.poses.end(), max_robot_pose_search_dist_);
// First find the closest pose on the path to the robot
// bounded by when the path turns around (if it does) so we don't get a pose from a later
// portion of the path
auto closest_pose_it =
nav2_util::geometry_utils::min_by(
path.poses.begin() + last_closest_index_, closest_pose_upper_bound,
[&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
return nav2_util::geometry_utils::euclidean_distance(robot_pose, ps);
});
last_closest_index_ = std::distance(path.poses.begin(), closest_pose_it);
double distance_to_closest_point = nav2_util::geometry_utils::euclidean_distance(robot_pose, *closest_pose_it);
RCLCPP_INFO(node_->get_logger(), "Distance to closest point: %f", distance_to_closest_point);
if(distance_to_closest_point < prox_thr){
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
bool IsGoalNearbyCondition::transformPose(
const std::string frame,
const geometry_msgs::msg::PoseStamped & in_pose,
geometry_msgs::msg::PoseStamped & out_pose) const
{
if (in_pose.header.frame_id == frame) {
out_pose = in_pose;
return true;
}
try {
tf_buffer_->transform(in_pose, out_pose, frame);
out_pose.header.frame_id = frame;
return true;
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(node_->get_logger(), "Exception in transformPose: %s", ex.what());
}
return false;
}
I though it's even working but after few attempts I've got segmentation fault so still working on that
| </ReactiveFallback> | ||
| </RecoveryNode> | ||
| </BehaviorTree> | ||
| </root> |
| <Inverter> | ||
| <GlobalUpdatedGoal/> | ||
| </Inverter> | ||
| <IsGoalNearby path="{path}" proximity_threshold="2.0"/> |
|
@Jakubach following up here - have you had a chance to work on this? |
@SteveMacenski I had another job but will back to this probably this month or November. |
|
Great, thank you! |
Basic Info
Description of contribution in a few bullet points
BT Node that return if the robot is in the goal proximity
PR related to the Steve advice on robotics stack:
https://robotics.stackexchange.com/questions/112576/maneuvers-on-paths-end-point-with-navigation2-smac-lattice-planner/112577?noredirect=1#comment48615_112577
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: