Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

spin without costmap #70

Merged
merged 1 commit into from
Apr 20, 2024
Merged

spin without costmap #70

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
BT::InputPort<double>("spin_dist", 1.57, "Spin distance"),
BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning"),
BT::InputPort<bool>("is_recovery", true, "True if recovery"),
BT::InputPort<bool>("check_local_costmap", true, "Check local costmap for collisions"),
BT::OutputPort<ActionResult::_error_code_type>(
"error_code_id", "The spin behavior error code")
});
Expand Down
3 changes: 3 additions & 0 deletions nav2_behavior_tree/plugins/action/spin_action.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,9 @@ void SpinAction::initialize()
goal_.target_yaw = dist;
goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
getInput("is_recovery", is_recovery_);
bool check_local_costmap;
getInput("check_local_costmap", check_local_costmap);
goal_.check_local_costmap = check_local_costmap;

initialized_ = true;
}
Expand Down
1 change: 1 addition & 0 deletions nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ class Spin : public TimedBehavior<SpinAction>
double prev_yaw_;
double relative_yaw_;
double simulate_ahead_time_;
bool check_local_costmap_;
rclcpp::Duration command_time_allowance_{0, 0};
rclcpp::Time end_time_;
};
Expand Down
3 changes: 2 additions & 1 deletion nav2_behaviors/plugins/spin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,7 @@ ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)

command_time_allowance_ = command->time_allowance;
end_time_ = steady_clock_.now() + command_time_allowance_;
check_local_costmap_ = command->check_local_costmap;

return ResultStatus{Status::SUCCEEDED, SpinActionGoal::NONE};
}
Expand Down Expand Up @@ -146,7 +147,7 @@ ResultStatus Spin::onCycleUpdate()
pose2d.y = current_pose.pose.position.y;
pose2d.theta = tf2::getYaw(current_pose.pose.orientation);

if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) {
if (check_local_costmap_ && !isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) {
stopRobot();
RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
return ResultStatus{Status::FAILED, SpinActionGoal::COLLISION_AHEAD};
Expand Down
1 change: 1 addition & 0 deletions nav2_msgs/action/Spin.action
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ uint16 COLLISION_AHEAD=703
#goal definition
float32 target_yaw
builtin_interfaces/Duration time_allowance
bool check_local_costmap
---
#result definition
builtin_interfaces/Duration total_elapsed_time
Expand Down