Skip to content

Commit

Permalink
Merge branch 'ros-navigation:main' into feat/smac_planner_include_ori…
Browse files Browse the repository at this point in the history 
…entation_flexibility
  • Loading branch information
@stevedanomodolor
stevedanomodolor authored Oct 18, 2024
2 parents b18ba84 + 44a69cc commit 92e2ee8
Show file tree
Hide file tree
Showing 6 changed files with 406 additions and 116 deletions.
9 changes: 8 additions & 1 deletion ...n_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
View file
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
24 changes: 22 additions & 2 deletions nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
View file
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 @@ -253,7 +256,10 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
}
}

return current_path_.poses.back();
auto goal = current_path_.poses.back();
goal.header.frame_id = current_path_.header.frame_id;
goal.header.stamp = clock_->now();
return goal;
}

geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal()
Expand All @@ -263,6 +269,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal()
}

auto goal = current_path_.poses.back();
goal.header.frame_id = current_path_.header.frame_id;
goal.header.stamp = clock_->now();
return goal;
}
Expand Down Expand Up @@ -337,9 +344,20 @@ void RotationShimController::isCollisionFree(
}
}

bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path)
{
// Return true if rotating or if the current path is empty
if (in_rotation_ || current_path_.poses.empty()) {
return true;
}

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

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 @@ -374,6 +392,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();
}
}
}
Expand Down
60 changes: 59 additions & 1 deletion nav2_rotation_shim_controller/test/test_shim_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,11 @@ class RotationShimShim : public nav2_rotation_shim_controller::RotationShimContr
return getSampledPathPt();
}

bool isGoalChangedWrapper(const nav_msgs::msg::Path & path)
{
return isGoalChanged(path);
}

geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(geometry_msgs::msg::PoseStamped pt)
{
return transformPoseToBaseFrame(pt);
Expand Down Expand Up @@ -382,6 +387,57 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) {
EXPECT_EQ(cmd_vel.twist.angular.z, 1.8);
}

TEST(RotationShimControllerTest, isGoalChangedTest)
{
auto ctrl = std::make_shared<RotationShimShim>();
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
rclcpp_lifecycle::State state;
costmap->on_configure(state);
auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);

geometry_msgs::msg::TransformStamped transform;
transform.header.frame_id = "base_link";
transform.child_frame_id = "odom";
transform.transform.rotation.x = 0.0;
transform.transform.rotation.y = 0.0;
transform.transform.rotation.z = 0.0;
transform.transform.rotation.w = 1.0;
tf_broadcaster->sendTransform(transform);

// set a valid primary controller so we can do lifecycle
node->declare_parameter(
"PathFollower.primary_controller",
std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
node->declare_parameter(
"PathFollower.rotate_to_heading_once",
true);

auto controller = std::make_shared<RotationShimShim>();
controller->configure(node, name, tf, costmap);
controller->activate();

nav_msgs::msg::Path path;
path.header.frame_id = "base_link";
path.poses.resize(2);
path.poses.back().pose.position.x = 2.0;
path.poses.back().pose.position.y = 2.0;

// Test: Current path is empty, should return true
EXPECT_EQ(controller->isGoalChangedWrapper(path), true);

// Test: Last pose of the current path is the same, should return false
controller->setPlan(path);
EXPECT_EQ(controller->isGoalChangedWrapper(path), false);

// Test: Last pose of the current path differs, should return true
path.poses.back().pose.position.x = 3.0;
EXPECT_EQ(controller->isGoalChangedWrapper(path), true);
}

TEST(RotationShimControllerTest, testDynamicParameter)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
Expand Down Expand Up @@ -412,7 +468,8 @@ TEST(RotationShimControllerTest, testDynamicParameter)
rclcpp::Parameter("test.max_angular_accel", 7.0),
rclcpp::Parameter("test.simulate_ahead_time", 7.0),
rclcpp::Parameter("test.primary_controller", std::string("HI")),
rclcpp::Parameter("test.rotate_to_goal_heading", true)});
rclcpp::Parameter("test.rotate_to_goal_heading", true),
rclcpp::Parameter("test.rotate_to_heading_once", true)});

rclcpp::spin_until_future_complete(
node->get_node_base_interface(),
Expand All @@ -424,4 +481,5 @@ TEST(RotationShimControllerTest, testDynamicParameter)
EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0);
EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0);
EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true);
EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true);
}
110 changes: 88 additions & 22 deletions nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <string>

// ROS
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rviz_common/panel.hpp"
Expand All @@ -37,18 +38,30 @@ class QPushButton;
namespace nav2_rviz_plugins
{

class InitialDockThread;

/// Panel to interface to the docking server
class DockingPanel : public rviz_common::Panel
{
Q_OBJECT

public:
explicit DockingPanel(QWidget * parent = 0);
virtual ~DockingPanel();

void onInitialize() override;

/// Load and save configuration data
void load(const rviz_common::Config & config) override;
void save(rviz_common::Config config) const override;

private Q_SLOTS:
void startThread();
void onStartup();
void onDockingButtonPressed();
void onUndockingButtonPressed();
void onCancelDocking();
void onCancelUndocking();
void dockIdCheckbox();

private:
Expand All @@ -57,14 +70,6 @@ private Q_SLOTS:
using DockGoalHandle = rclcpp_action::ClientGoalHandle<Dock>;
using UndockGoalHandle = rclcpp_action::ClientGoalHandle<Undock>;

// Start the actions
void startDocking();
void startUndocking();

// Cancel the actions
void cancelDocking();
void cancelUndocking();

// The (non-spinning) client node used to invoke the action client
void timerEvent(QTimerEvent * event) override;

Expand All @@ -84,14 +89,33 @@ private Q_SLOTS:

// The (non-spinning) client node used to invoke the action client
rclcpp::Node::SharedPtr client_node_;

// The Node pointer that we need to keep alive for the duration of this plugin.
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;

// Timeout value when waiting for action servers to respond
std::chrono::milliseconds server_timeout_;

// A timer used to check on the completion status of the action
QBasicTimer action_timer_;

// The Dock and Undock action client
rclcpp_action::Client<Dock>::SharedPtr dock_client_;
rclcpp_action::Client<Undock>::SharedPtr undock_client_;

// Docking / Undocking action feedback subscribers
rclcpp::Subscription<Dock::Impl::FeedbackMessage>::SharedPtr docking_feedback_sub_;
rclcpp::Subscription<Undock::Impl::FeedbackMessage>::SharedPtr undocking_feedback_sub_;
rclcpp::Subscription<Dock::Impl::GoalStatusMessage>::SharedPtr docking_goal_status_sub_;
rclcpp::Subscription<Undock::Impl::GoalStatusMessage>::SharedPtr undocking_goal_status_sub_;

// Goal related state
DockGoalHandle::SharedPtr dock_goal_handle_;
UndockGoalHandle::SharedPtr undock_goal_handle_;

// Flags to indicate if the plugins have been loaded
bool plugins_loaded_ = false;
bool server_failed_ = false;
bool tried_once_ = false;
QBasicTimer timer_;

QVBoxLayout * main_layout_{nullptr};
QHBoxLayout * info_layout_{nullptr};
Expand Down Expand Up @@ -121,20 +145,62 @@ private Q_SLOTS:
bool undocking_in_progress_ = false;
bool use_dock_id_ = false;

// The Dock and Undock action client
rclcpp_action::Client<Dock>::SharedPtr dock_client_;
rclcpp_action::Client<Undock>::SharedPtr undock_client_;
DockGoalHandle::SharedPtr dock_goal_handle_;
UndockGoalHandle::SharedPtr undock_goal_handle_;
QStateMachine state_machine_;
InitialDockThread * initial_thread_;

// The Node pointer that we need to keep alive for the duration of this plugin.
std::shared_ptr<rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
QState * pre_initial_{nullptr};
QState * idle_{nullptr};
QState * docking_{nullptr};
QState * undocking_{nullptr};
QState * canceled_docking_{nullptr};
QState * canceled_undocking_{nullptr};
};

// Docking / Undocking action feedback subscribers
rclcpp::Subscription<Dock::Impl::FeedbackMessage>::SharedPtr docking_feedback_sub_;
rclcpp::Subscription<Undock::Impl::FeedbackMessage>::SharedPtr undocking_feedback_sub_;
rclcpp::Subscription<Dock::Impl::GoalStatusMessage>::SharedPtr docking_goal_status_sub_;
rclcpp::Subscription<Undock::Impl::GoalStatusMessage>::SharedPtr undocking_goal_status_sub_;
class InitialDockThread : public QThread
{
Q_OBJECT

public:
explicit InitialDockThread(
rclcpp_action::Client<nav2_msgs::action::DockRobot>::SharedPtr & dock_client,
rclcpp_action::Client<nav2_msgs::action::UndockRobot>::SharedPtr & undock_client)
: dock_client_(dock_client), undock_client_(undock_client)
{}

void run() override
{
while (!dock_active_) {
dock_active_ = dock_client_->wait_for_action_server(std::chrono::seconds(1));
}

while (!undock_active_) {
undock_active_ = undock_client_->wait_for_action_server(std::chrono::seconds(1));
}

if (dock_active_) {
emit dockingActive();
} else {
emit dockingInactive();
}

if (undock_active_) {
emit undockingActive();
} else {
emit undockingInactive();
}
}

signals:
void dockingActive();
void dockingInactive();
void undockingActive();
void undockingInactive();

private:
rclcpp_action::Client<nav2_msgs::action::DockRobot>::SharedPtr dock_client_;
rclcpp_action::Client<nav2_msgs::action::UndockRobot>::SharedPtr undock_client_;
bool dock_active_ = false;
bool undock_active_ = false;
};

} // namespace nav2_rviz_plugins
Expand Down
Loading

0 comments on commit 92e2ee8

Please sign in to comment.