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

Make Navigation Precise #565

Closed
wants to merge 1 commit into from
Closed

Make Navigation Precise #565

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 ...nners/backward_global_planner/include/backward_global_planner/backward_global_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ class BackwardGlobalPlanner : public nav2_core::GlobalPlanner
const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal);

private:
void updateParameters();
// rclcpp::Node::SharedPtr nh_;
rclcpp_lifecycle::LifecycleNode::SharedPtr nh_;

Expand Down
21 changes: 20 additions & 1 deletion ..._planners/backward_global_planner/src/backward_global_planner/backward_global_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ namespace backward_global_planner
*/
BackwardGlobalPlanner::BackwardGlobalPlanner()
{
skip_straight_motion_distance_ = 0.2;
skip_straight_motion_distance_ = 0.014;
puresSpinningRadStep_ = 1000; // rads
}

Expand All @@ -67,6 +67,8 @@ void BackwardGlobalPlanner::configure(
name_ = name;
tf_ = tf;
transform_tolerance_ = 0.1;
skip_straight_motion_distance_ = 0.014;
puresSpinningRadStep_ = 1000; // rads

// RCLCPP_INFO_NAMED(nh_->get_logger(), "Backwards", "BackwardGlobalPlanner initialize");
costmap_ros_ = costmap_ros;
Expand All @@ -78,8 +80,23 @@ void BackwardGlobalPlanner::configure(
nh_->create_publisher<visualization_msgs::msg::MarkerArray>("backward_planner/markers", 1);

declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
declareOrSet(nh_, name_ + ".pure_spinning_rad_step", puresSpinningRadStep_);
declareOrSet(nh_, name_ + ".skip_straight_motion_distance", skip_straight_motion_distance_);

}

void BackwardGlobalPlanner::updateParameters()
{
nh_->get_parameter(name_ + ".pure_spinning_rad_step", puresSpinningRadStep_);
nh_->get_parameter(name_ + ".skip_straight_motion_distance", skip_straight_motion_distance_);
nh_->get_parameter(name_ + ".transform_tolerance", transform_tolerance_);

RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner.pure_spinning_rad_step: " << puresSpinningRadStep_);
RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner.skip_straight_motion_distance: " << skip_straight_motion_distance_);
RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner.transform_tolerance: " << transform_tolerance_);
}


/**
******************************************************************************************************************
* cleanup()
Expand All @@ -95,6 +112,7 @@ void BackwardGlobalPlanner::cleanup() { this->cleanMarkers(); }
void BackwardGlobalPlanner::activate()
{
RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner] activating planner");
this->updateParameters();
planPub_->on_activate();
markersPub_->on_activate();
}
Expand Down Expand Up @@ -228,6 +246,7 @@ void BackwardGlobalPlanner::createDefaultBackwardPath(
nav_msgs::msg::Path BackwardGlobalPlanner::createPlan(
const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal)
{
this->updateParameters();
RCLCPP_INFO_STREAM(
nh_->get_logger(), "[BackwardGlobalPlanner] goal frame id: "
<< goal.header.frame_id << " pose: " << goal.pose.position);
Expand Down
1 change: 1 addition & 0 deletions ...planners/forward_global_planner/include/forward_global_planner/forward_global_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ class ForwardGlobalPlanner : public nav2_core::GlobalPlanner
const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal);

private:
void updateParameters();
// rclcpp::Node::SharedPtr nh_;
rclcpp_lifecycle::LifecycleNode::SharedPtr nh_;

Expand Down
33 changes: 30 additions & 3 deletions ...tom_planners/forward_global_planner/src/forward_global_planner/forward_global_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ namespace forward_global_planner
ForwardGlobalPlanner::ForwardGlobalPlanner()
// : nh_("~/ForwardGlobalPlanner")
{
skip_straight_motion_distance_ = 0.2; // meters
skip_straight_motion_distance_ = 0.01; // 0.2; // meters
puresSpinningRadStep_ = 1000; // rads
}

Expand All @@ -60,14 +60,33 @@ void ForwardGlobalPlanner::configure(

RCLCPP_INFO(nh_->get_logger(), "[Forward Global Planner] initializing");
planPub_ = nh_->create_publisher<nav_msgs::msg::Path>("global_plan", rclcpp::QoS(1));
skip_straight_motion_distance_ = 0.2; // meters
skip_straight_motion_distance_ = 0.2; //0.2 // meters
puresSpinningRadStep_ = 1000; // rads
transform_tolerance_ = 0.1;

declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
declareOrSet(nh_, name_ + ".pure_spinning_rad_step", puresSpinningRadStep_);
declareOrSet(nh_, name_ + ".skip_straight_motion_distance", skip_straight_motion_distance_);
}
void ForwardGlobalPlanner::updateParameters()
{
nh_->get_parameter(name_ + ".pure_spinning_rad_step", puresSpinningRadStep_);
nh_->get_parameter(name_ + ".skip_straight_motion_distance", skip_straight_motion_distance_);
nh_->get_parameter(name_ + ".transform_tolerance", transform_tolerance_);

RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardGlobalPlanner.pure_spinning_rad_step: " << puresSpinningRadStep_);
RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardGlobalPlanner.skip_straight_motion_distance: " << skip_straight_motion_distance_);
RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardGlobalPlanner.transform_tolerance: " << transform_tolerance_);
}

void ForwardGlobalPlanner::cleanup() {}

void ForwardGlobalPlanner::activate() { planPub_->on_activate(); }
void ForwardGlobalPlanner::activate()
{
RCLCPP_INFO_STREAM(nh_->get_logger(), "activating global planner ForwardGlobalPlanner");
this->updateParameters();
planPub_->on_activate();
}

void ForwardGlobalPlanner::deactivate()
{
Expand All @@ -79,6 +98,9 @@ void ForwardGlobalPlanner::deactivate()
nav_msgs::msg::Path ForwardGlobalPlanner::createPlan(
const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal)
{

this->updateParameters();

RCLCPP_INFO(nh_->get_logger(), "[Forward Global Planner] planning");

rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
Expand Down Expand Up @@ -108,6 +130,11 @@ nav_msgs::msg::Path ForwardGlobalPlanner::createPlan(

double length = sqrt(dx * dx + dy * dy);

RCLCPP_INFO_STREAM(
nh_->get_logger(),
"[Forward Global Planner] current plan length: " << length);


geometry_msgs::msg::PoseStamped prevState;
if (length > skip_straight_motion_distance_)
{
Expand Down
8 changes: 6 additions & 2 deletions ...custom_planners/forward_local_planner/src/forward_local_planner/forward_local_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ namespace forward_local_planner
* ForwardLocalPlanner()
******************************************************************************************************************
*/
ForwardLocalPlanner::ForwardLocalPlanner() : transform_tolerance_(0.1), waitingTimeout_(2s) {}
ForwardLocalPlanner::ForwardLocalPlanner() : transform_tolerance_(0.05), waitingTimeout_(2s) {}

ForwardLocalPlanner::~ForwardLocalPlanner() {}

Expand Down Expand Up @@ -117,7 +117,7 @@ void ForwardLocalPlanner::configure(
}

void ForwardLocalPlanner::updateParameters()
{
{
nh_->get_parameter(name_ + ".k_rho", k_rho_);
nh_->get_parameter(name_ + ".k_alpha", k_alpha_);
nh_->get_parameter(name_ + ".k_betta", k_betta_);
Expand Down Expand Up @@ -367,6 +367,8 @@ geometry_msgs::msg::TwistStamped ForwardLocalPlanner::computeVelocityCommands(
RCLCPP_DEBUG(
nh_->get_logger(), "[ForwardLocalPlanner] ----- COMPUTE VELOCITY COMMAND LOCAL PLANNER ---");

RCLCPP_INFO_STREAM(
nh_->get_logger(), "[ForwardLocalPlanner] plan_.size:" << (int)plan_.size());
bool ok = false;
while (!ok)
{
Expand All @@ -382,6 +384,8 @@ geometry_msgs::msg::TwistStamped ForwardLocalPlanner::computeVelocityCommands(
double dx = p.x - currentPose.pose.position.x;
double dy = p.y - currentPose.pose.position.y;
double dist = sqrt(dx * dx + dy * dy);
RCLCPP_INFO_STREAM(
nh_->get_logger(), "[ForwardLocalPlanner] dist:" << dist);

double pangle = tf2::getYaw(q);
double angle = tf2::getYaw(currentPose.pose.orientation);
Expand Down
Loading
Loading