Skip to content

Commit

Permalink
Merge pull request #9 from botsandus/AUTO-607_path_distance_for_pruning
Browse files Browse the repository at this point in the history
AUTO-607 use path distance instead of euclidean distance for pruning path
  • Loading branch information
doisyg authored Mar 10, 2023
2 parents d1832e9 + 376069b commit 556b6ba
Show file tree
Hide file tree
Showing 14 changed files with 408 additions and 52 deletions.
7 changes: 6 additions & 1 deletion nav2_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,10 @@ set(dependencies
add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp)
ament_target_dependencies(simple_progress_checker ${dependencies})

add_library(rotation_progress_checker SHARED plugins/rotation_progress_checker.cpp)
target_link_libraries(rotation_progress_checker simple_progress_checker)
ament_target_dependencies(rotation_progress_checker ${dependencies})

add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp)
ament_target_dependencies(simple_goal_checker ${dependencies})

Expand Down Expand Up @@ -79,7 +83,7 @@ target_link_libraries(${executable_name} ${library_name})

rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer")

install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
install(TARGETS simple_progress_checker rotation_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
Expand All @@ -102,6 +106,7 @@ endif()

ament_export_include_directories(include)
ament_export_libraries(simple_progress_checker
rotation_progress_checker
simple_goal_checker
stopped_goal_checker
${library_name})
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
// Copyright (c) 2023 Dexory
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_CONTROLLER__PLUGINS__ROTATION_PROGRESS_CHECKER_HPP_
#define NAV2_CONTROLLER__PLUGINS__ROTATION_PROGRESS_CHECKER_HPP_

#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "nav2_controller/plugins/simple_progress_checker.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_core/progress_checker.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"

namespace nav2_controller
{
/**
* @class RotationProgressChecker
* @brief This plugin is used to check the position and the angle of the robot to make sure
* that it is actually progressing or rotating towards a goal.
*/

class RotationProgressChecker : public SimpleProgressChecker
{
public:
void initialize(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name) override;
bool check(geometry_msgs::msg::PoseStamped & current_pose) override;

protected:
/**
* @brief Calculates robots movement from baseline pose
* @param pose Current pose of the robot
* @return true, if movement is greater than radius_, or false
*/
bool is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose);

static double pose_angle_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &);

double required_movement_angle_;

// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
std::string plugin_name_;

/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
};
} // namespace nav2_controller

#endif // NAV2_CONTROLLER__PLUGINS__ROTATION_PROGRESS_CHECKER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,8 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker
*/
void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose);

static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &);

rclcpp::Clock::SharedPtr clock_;

double radius_;
Expand Down
5 changes: 5 additions & 0 deletions nav2_controller/plugins.xml
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,11 @@
<description>Checks if distance between current and previous pose is above a threshold</description>
</class>
</library>
<library path="rotation_progress_checker">
<class type="nav2_controller::RotationProgressChecker" base_class_type="nav2_core::ProgressChecker">
<description>Checks if distance and angle between current and previous pose is above a threshold</description>
</class>
</library>
<library path="simple_goal_checker">
<class type="nav2_controller::SimpleGoalChecker" base_class_type="nav2_core::GoalChecker">
<description>Checks if current pose is within goal window for x,y and yaw</description>
Expand Down
97 changes: 97 additions & 0 deletions nav2_controller/plugins/rotation_progress_checker.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,97 @@
// Copyright (c) 2023 Dexory
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "nav2_controller/plugins/rotation_progress_checker.hpp"
#include <cmath>
#include <string>
#include <memory>
#include <vector>
#include "angles/angles.h"
#include "nav_2d_utils/conversions.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav2_util/node_utils.hpp"
#include "pluginlib/class_list_macros.hpp"

using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;

namespace nav2_controller
{

void RotationProgressChecker::initialize(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name)
{
plugin_name_ = plugin_name;
SimpleProgressChecker::initialize(parent, plugin_name);
auto node = parent.lock();

nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5));
node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5);

// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(&RotationProgressChecker::dynamicParametersCallback, this, _1));
}

bool RotationProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
{
// relies on short circuit evaluation to not call is_robot_moved_enough if
// baseline_pose is not set.
geometry_msgs::msg::Pose2D current_pose2d;
current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);

if ((!baseline_pose_set_) || (RotationProgressChecker::is_robot_moved_enough(current_pose2d))) {
reset_baseline_pose(current_pose2d);
return true;
}
return !((clock_->now() - baseline_time_) > time_allowance_);
}

bool RotationProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose)
{
return pose_distance(pose, baseline_pose_) > radius_ ||
pose_angle_distance(pose, baseline_pose_) > required_movement_angle_;
}

double RotationProgressChecker::pose_angle_distance(
const geometry_msgs::msg::Pose2D & pose1,
const geometry_msgs::msg::Pose2D & pose2)
{
return abs(angles::shortest_angular_distance(pose1.theta,pose2.theta));
}

rcl_interfaces::msg::SetParametersResult
RotationProgressChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
{
rcl_interfaces::msg::SetParametersResult result;
for (auto parameter : parameters) {
const auto & type = parameter.get_type();
const auto & name = parameter.get_name();

if (type == ParameterType::PARAMETER_DOUBLE) {
if (name == plugin_name_ + ".required_movement_angle") {
required_movement_angle_ = parameter.as_double();
}
}
}
result.successful = true;
return result;
}

} // namespace nav2_controller

PLUGINLIB_EXPORT_CLASS(nav2_controller::RotationProgressChecker, nav2_core::ProgressChecker)
4 changes: 1 addition & 3 deletions nav2_controller/plugins/simple_progress_checker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,6 @@ using std::placeholders::_1;

namespace nav2_controller
{
static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &);

void SimpleProgressChecker::initialize(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name)
Expand Down Expand Up @@ -85,7 +83,7 @@ bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose
return pose_distance(pose, baseline_pose_) > radius_;
}

static double pose_distance(
double SimpleProgressChecker::pose_distance(
const geometry_msgs::msg::Pose2D & pose1,
const geometry_msgs::msg::Pose2D & pose2)
{
Expand Down
9 changes: 1 addition & 8 deletions nav2_mppi_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -75,14 +75,7 @@ install(DIRECTORY include/
DESTINATION include/
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
add_subdirectory(test)
# add_subdirectory(benchmark)
endif()


ament_export_libraries(${libraries})
ament_export_dependencies(${dependencies_pkgs})
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -248,7 +248,7 @@ class Optimizer

models::State state_;
models::ControlSequence control_sequence_;
std::array<mppi::models::Control, 2> control_history_;
std::array<mppi::models::Control, 4> control_history_;
models::Trajectories generated_trajectories_;
models::Path path_;
xt::xtensor<float, 1> costs_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,8 @@ class PathHandler
*/
nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);

bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);

protected:
/**
* @brief Transform a pose to another frame
Expand Down Expand Up @@ -127,19 +129,25 @@ class PathHandler
* @brief Prune global path to only interesting portions
* @param end Final path iterator
*/
void pruneGlobalPlan(const PathIterator end);
void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);

std::string name_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
ParametersHandler * parameters_handler_;

nav_msgs::msg::Path global_plan_up_to_inversion_;
nav_msgs::msg::Path global_plan_;
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};

double max_robot_pose_search_dist_{0};
double prune_distance_{0};
double transform_tolerance_{0};

bool enforce_inversion_{false};
bool inversion_remaining_{false};
double inversion_xy_tolerance_{0};
double inversion_yaw_tolerance{0};
};
} // namespace mppi

Expand Down
Loading

0 comments on commit 556b6ba

Please sign in to comment.