Skip to content

Commit

Permalink
Added parameter rotate_to_heading_once
Browse files Browse the repository at this point in the history
Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com>
  • Loading branch information
ikhann committed Oct 16, 2024
1 parent 682e089 commit 631160a
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,13 @@ class RotationShimController : public nav2_core::Controller
const double & angular_distance_to_heading,
const geometry_msgs::msg::PoseStamped & pose);

/**
* @brief Checks if the goal has changed based on the given path.
* @param path The path to compare with the current goal.
* @return True if the goal has changed, false otherwise.
*/
bool isGoalChanged(const nav_msgs::msg::Path & path);

/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
Expand All @@ -171,7 +178,7 @@ class RotationShimController : public nav2_core::Controller
double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_;
double rotate_to_heading_angular_vel_, max_angular_accel_;
double control_duration_, simulate_ahead_time_;
bool rotate_to_goal_heading_, in_rotation_;
bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_;

// Dynamic parameters handler
std::mutex mutex_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@ void RotationShimController::configure(
node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING);
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false));
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false));

node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
Expand All @@ -86,6 +88,7 @@ void RotationShimController::configure(
control_duration_ = 1.0 / control_frequency;

node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_);
node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_);

try {
primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
Expand Down Expand Up @@ -340,9 +343,20 @@ void RotationShimController::isCollisionFree(
}
}

bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path)

Check warning on line 346 in nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp#L346

Added line #L346 was not covered by tests
{
// Return true if rotating or if the current path is empty
if (in_rotation_ || current_path_.poses.empty()) {

Check warning on line 349 in nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp#L349

Added line #L349 was not covered by tests
return true;
}

// Check if the last pose of the current and new paths differ
return current_path_.poses.back().pose != path.poses.back().pose;

Check warning on line 354 in nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp#L354

Added line #L354 was not covered by tests
}

void RotationShimController::setPlan(const nav_msgs::msg::Path & path)
{
path_updated_ = true;
path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true;
current_path_ = path;
primary_controller_->setPlan(path);
}
Expand Down Expand Up @@ -377,6 +391,8 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
} else if (type == ParameterType::PARAMETER_BOOL) {
if (name == plugin_name_ + ".rotate_to_goal_heading") {
rotate_to_goal_heading_ = parameter.as_bool();
} else if (name == plugin_name_ + ".rotate_to_heading_once") {
rotate_to_heading_once_ = parameter.as_bool();

Check warning on line 395 in nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp#L395

Added line #L395 was not covered by tests
}
}
}
Expand Down

0 comments on commit 631160a

Please sign in to comment.