diff --git a/README.md b/README.md index f50483db7eb..1d5b83ab57b 100644 --- a/README.md +++ b/README.md @@ -9,8 +9,8 @@ For detailed instructions on how to: - [Getting Started](https://navigation.ros.org/getting_started/index.html) - [Concepts](https://navigation.ros.org/concepts/index.html) -- [Build](https://navigation.ros.org/build_instructions/index.html#build) -- [Install](https://navigation.ros.org/build_instructions/index.html#install) +- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build) +- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install) - [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html) - [Configure](https://navigation.ros.org/configuration/index.html) - [Navigation Plugins](https://navigation.ros.org/plugins/index.html) diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp index c3b406cfd43..713478d24d3 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp @@ -49,7 +49,7 @@ typedef struct // Return a zero vector -pf_vector_t pf_vector_zero(); +pf_vector_t pf_vector_zero(void); // Check for NAN or INF in any component // int pf_vector_finite(pf_vector_t a); @@ -71,7 +71,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); // Return a zero matrix -pf_matrix_t pf_matrix_zero(); +pf_matrix_t pf_matrix_zero(void); // Check for NAN or INF in any component // int pf_matrix_finite(pf_matrix_t a); diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index de4e3247d10..9d1f5a82892 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -463,7 +463,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) // Workspace double m[4], c[2][2]; - size_t count; double weight; // Cluster the samples @@ -474,7 +473,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) for (i = 0; i < set->cluster_max_count; i++) { cluster = set->clusters + i; - cluster->count = 0; cluster->weight = 0; cluster->mean = pf_vector_zero(); cluster->cov = pf_matrix_zero(); @@ -490,7 +488,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) } // Initialize overall filter stats - count = 0; weight = 0.0; set->mean = pf_vector_zero(); set->cov = pf_matrix_zero(); @@ -521,10 +518,8 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) cluster = set->clusters + cidx; - cluster->count += 1; cluster->weight += sample->weight; - count += 1; weight += sample->weight; // Compute mean diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c index a7a5cd39c61..00fa3840607 100644 --- a/nav2_amcl/src/pf/pf_vector.c +++ b/nav2_amcl/src/pf/pf_vector.c @@ -35,7 +35,7 @@ // Return a zero vector -pf_vector_t pf_vector_zero() +pf_vector_t pf_vector_zero(void) { pf_vector_t c; @@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b) // Return a zero matrix -pf_matrix_t pf_matrix_zero() +pf_matrix_t pf_matrix_zero(void) { int i, j; pf_matrix_t c; diff --git a/nav2_amcl/src/sensors/laser/beam_model.cpp b/nav2_amcl/src/sensors/laser/beam_model.cpp index ebbfedcc7be..b6f281cbf78 100644 --- a/nav2_amcl/src/sensors/laser/beam_model.cpp +++ b/nav2_amcl/src/sensors/laser/beam_model.cpp @@ -72,6 +72,12 @@ BeamModel::sensorFunction(LaserData * data, pf_sample_set_t * set) step = (data->range_count - 1) / (self->max_beams_ - 1); for (i = 0; i < data->range_count; i += step) { obs_range = data->ranges[i][0]; + + // Check for NaN + if (isnan(obs_range)) { + continue; + } + obs_bearing = data->ranges[i][1]; // Compute the range according to the map diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index b2c30c432f0..84e16e7d4e9 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/ +For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 6f0c9bfb914..596fda97911 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -85,9 +85,9 @@ class BehaviorTreeEngine /** * @brief Function to explicitly reset all BT nodes to initial state - * @param root_node Pointer to BT root node + * @param tree Tree to halt */ - void haltAllActions(BT::TreeNode * root_node); + void haltAllActions(BT::Tree & tree); protected: // The factory that will be used to dynamically construct the behavior tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 64cc20b4e9f..e6eef2992f4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -162,7 +162,7 @@ class BtActionNode : public BT::ActionNodeBase } /** - * @brief Function to perform some user-defined operation whe the action is aborted. + * @brief Function to perform some user-defined operation when the action is aborted. * @return BT::NodeStatus Returns FAILURE by default, user may override return another value */ virtual BT::NodeStatus on_aborted() @@ -206,7 +206,8 @@ class BtActionNode : public BT::ActionNodeBase // if new goal was sent and action server has not yet responded // check the future goal handle if (future_goal_handle_) { - auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + auto elapsed = + (node_->now() - time_goal_sent_).template to_chrono(); if (!is_future_goal_handle_complete(elapsed)) { // return RUNNING if there is still some time before timeout happens if (elapsed < server_timeout_) { @@ -237,7 +238,8 @@ class BtActionNode : public BT::ActionNodeBase { goal_updated_ = false; send_new_goal(); - auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + auto elapsed = + (node_->now() - time_goal_sent_).template to_chrono(); if (!is_future_goal_handle_complete(elapsed)) { if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; @@ -266,7 +268,7 @@ class BtActionNode : public BT::ActionNodeBase // Action related failure that should not fail the tree, but the node return BT::NodeStatus::FAILURE; } else { - // Internal exception to propogate to the tree + // Internal exception to propagate to the tree throw e; } } @@ -300,6 +302,7 @@ class BtActionNode : public BT::ActionNodeBase void halt() override { if (should_cancel_goal()) { + auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) @@ -308,6 +311,16 @@ class BtActionNode : public BT::ActionNodeBase node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); } + + if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), + "Failed to get result for %s in node halt!", action_name_.c_str()); + } + + on_cancelled(); } setStatus(BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 2dcb21e42bb..467e4c80d0b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -177,10 +177,11 @@ class BtActionServer /** * @brief Function to halt the current tree. It will interrupt the execution of RUNNING nodes * by calling their halt() implementation (only for Async nodes that may return RUNNING) + * This should already done for all the exit states of the action but preemption */ void haltTree() { - tree_.rootNode()->halt(); + tree_.haltTree(); } protected: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 3e2ee0aee8e..8d06ee7075f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -203,7 +203,7 @@ bool BtActionServer::on_cleanup() plugin_lib_names_.clear(); current_bt_xml_filename_.clear(); blackboard_.reset(); - bt_->haltAllActions(tree_.rootNode()); + bt_->haltAllActions(tree_); bt_.reset(); return true; } @@ -231,6 +231,11 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Create the Behavior Tree from the XML input try { tree_ = bt_->createTreeFromFile(filename, blackboard_); + for (auto & blackboard : tree_.blackboard_stack) { + blackboard->set("node", client_node_); + blackboard->set("server_timeout", default_server_timeout_); + blackboard->set("bt_loop_duration", bt_loop_duration_); + } } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); return false; @@ -276,7 +281,7 @@ void BtActionServer::executeCallback() // Make sure that the Bt is not in a running state from a previous execution // note: if all the ControlNodes are implemented correctly, this is not needed. - bt_->haltAllActions(tree_.rootNode()); + bt_->haltAllActions(tree_); // Give server an opportunity to populate the result message or simple give // an indication that the action is complete. diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index d76aa693ff6..179c93e4d23 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -116,7 +116,7 @@ class BtCancelActionNode : public BT::ActionNodeBase return basic; } - void halt() + void halt() override { } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 26efba04c92..a027ac7760c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -183,7 +183,7 @@ class BtServiceNode : public BT::ActionNodeBase */ virtual BT::NodeStatus check_future() { - auto elapsed = (node_->now() - sent_time_).to_chrono(); + auto elapsed = (node_->now() - sent_time_).template to_chrono(); auto remaining = server_timeout_ - elapsed; if (remaining > std::chrono::milliseconds(0)) { @@ -199,7 +199,7 @@ class BtServiceNode : public BT::ActionNodeBase if (rc == rclcpp::FutureReturnCode::TIMEOUT) { on_wait_for_result(); - elapsed = (node_->now() - sent_time_).to_chrono(); + elapsed = (node_->now() - sent_time_).template to_chrono(); if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index 184bf1836c0..fcc02258a78 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -55,7 +55,7 @@ class RemovePassedGoals : public BT::ActionNodeBase double viapoint_achieved_radius_; double transform_tolerance_; std::shared_ptr tf_; - std::string global_frame_, robot_base_frame_; + std::string robot_base_frame_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index f452d24d320..5143f11351e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -53,7 +53,7 @@ class WaitAction : public BtActionNode { return providedBasicPorts( { - BT::InputPort("wait_duration", 1, "Wait time") + BT::InputPort("wait_duration", 1.0, "Wait time") }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index 674a8887b40..acbca8c960f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -92,7 +92,7 @@ class GoalReachedCondition : public BT::ConditionNode bool initialized_; double goal_reached_tol_; double transform_tolerance_; - std::string global_frame_, robot_base_frame_; + std::string robot_base_frame_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index fc768a3ab9d..7a5816973dc 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -37,8 +37,6 @@ RemovePassedGoals::RemovePassedGoals( auto node = config().blackboard->get("node"); node->get_parameter("transform_tolerance", transform_tolerance_); - global_frame_ = BT::deconflictPortAndParamFrame( - node, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } @@ -59,7 +57,7 @@ inline BT::NodeStatus RemovePassedGoals::tick() geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, *tf_, global_frame_, robot_base_frame_, + current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_, transform_tolerance_)) { return BT::NodeStatus::FAILURE; diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index d2b0e6484be..f08fed31476 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "nav2_behavior_tree/plugins/action/wait_action.hpp" @@ -26,16 +27,16 @@ WaitAction::WaitAction( const BT::NodeConfiguration & conf) : BtActionNode(xml_tag_name, action_name, conf) { - int duration; + double duration; getInput("wait_duration", duration); if (duration <= 0) { RCLCPP_WARN( node_->get_logger(), "Wait duration is negative or zero " - "(%i). Setting to positive.", duration); + "(%f). Setting to positive.", duration); duration *= -1; } - goal_.time.sec = duration; + goal_.time = rclcpp::Duration::from_seconds(duration); } void WaitAction::on_tick() diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e3c52928864..d81df60a9ec 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -33,8 +33,6 @@ GoalReachedCondition::GoalReachedCondition( { auto node = config().blackboard->get("node"); - global_frame_ = BT::deconflictPortAndParamFrame( - node, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } @@ -73,17 +71,17 @@ void GoalReachedCondition::initialize() bool GoalReachedCondition::isGoalReached() { - geometry_msgs::msg::PoseStamped current_pose; + geometry_msgs::msg::PoseStamped goal; + getInput("goal", goal); + geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) + current_pose, *tf_, goal.header.frame_id, robot_base_frame_, transform_tolerance_)) { RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); return false; } - geometry_msgs::msg::PoseStamped goal; - getInput("goal", goal); double dx = goal.pose.position.x - current_pose.pose.position.x; double dy = goal.pose.position.y - current_pose.pose.position.y; diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index 211254dafe9..a898dec9473 100644 --- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp @@ -95,7 +95,7 @@ void IsStuckCondition::logStuck(const std::string & msg) const return; } - RCLCPP_INFO(node_->get_logger(), msg.c_str()); + RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str()); prev_msg = msg; } diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index ddc67534469..8daca5afede 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -39,7 +39,7 @@ TransformAvailableCondition::TransformAvailableCondition( RCLCPP_FATAL( node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.", child_frame_.c_str(), parent_frame_.c_str()); - exit(-1); + throw std::runtime_error("TransformAvailableCondition: Child or parent frames not provided!"); } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index e5c96eba2c2..f47dfed38a8 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -43,7 +43,7 @@ SpeedController::SpeedController( if (min_rate_ <= 0.0 || max_rate_ <= 0.0) { std::string err_msg = "SpeedController node cannot have rate <= 0.0"; - RCLCPP_FATAL(node_->get_logger(), err_msg.c_str()); + RCLCPP_FATAL(node_->get_logger(), "%s", err_msg.c_str()); throw BT::BehaviorTreeException(err_msg); } diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 7bfc003cf1c..947778da5d6 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -55,7 +55,12 @@ BehaviorTreeEngine::run( onLoop(); - loopRate.sleep(); + if (!loopRate.sleep()) { + RCLCPP_WARN( + rclcpp::get_logger("BehaviorTreeEngine"), + "Behavior Tree tick rate %0.2f was exceeded!", + 1.0 / (loopRate.period().count() * 1.0e-9)); + } } } catch (const std::exception & ex) { RCLCPP_ERROR( @@ -85,22 +90,10 @@ BehaviorTreeEngine::createTreeFromFile( // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state void -BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node) +BehaviorTreeEngine::haltAllActions(BT::Tree & tree) { - if (!root_node) { - return; - } - // this halt signal should propagate through the entire tree. - root_node->halt(); - - // but, just in case... - auto visitor = [](BT::TreeNode * node) { - if (node->status() == BT::NodeStatus::RUNNING) { - node->halt(); - } - }; - BT::applyRecursiveVisitor(root_node, visitor); + tree.haltTree(); } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index d95cbd4a848..12c25d30d6f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -54,11 +54,17 @@ class FibonacciActionServer : public rclcpp::Node sleep_duration_ = sleep_duration; } + void setServerLoopRate(std::chrono::nanoseconds server_loop_rate) + { + server_loop_rate_ = server_loop_rate; + } + protected: rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID &, std::shared_ptr) { + RCLCPP_INFO(this->get_logger(), "Goal is received.."); if (sleep_duration_ > 0ms) { std::this_thread::sleep_for(sleep_duration_); } @@ -73,6 +79,13 @@ class FibonacciActionServer : public rclcpp::Node void handle_accepted( const std::shared_ptr> handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&FibonacciActionServer::execute, this, _1), handle}.detach(); + } + + void execute( + const std::shared_ptr> handle) { // this needs to return quickly to avoid blocking the executor, so spin up a new thread if (handle) { @@ -88,8 +101,17 @@ class FibonacciActionServer : public rclcpp::Node sequence.push_back(0); sequence.push_back(1); + rclcpp::Rate rate(server_loop_rate_); for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + if (handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Goal is canceling."); + handle->canceled(result); + return; + } + + RCLCPP_INFO(this->get_logger(), "Goal is feedbacking."); sequence.push_back(sequence[i] + sequence[i - 1]); + rate.sleep(); } handle->succeed(result); @@ -99,6 +121,7 @@ class FibonacciActionServer : public rclcpp::Node protected: rclcpp_action::Server::SharedPtr action_server_; std::chrono::milliseconds sleep_duration_; + std::chrono::nanoseconds server_loop_rate_; }; class FibonacciAction : public nav2_behavior_tree::BtActionNode @@ -121,6 +144,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset>("sequence", result_.result->sequence); + config().blackboard->set("on_cancelled_triggered", true); + return BT::NodeStatus::SUCCESS; + } + static BT::PortsList providedPorts() { return providedBasicPorts({BT::InputPort("order", "Fibonacci order")}); @@ -144,6 +174,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("on_cancelled_triggered", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -220,6 +251,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -255,15 +287,22 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // start a new execution cycle with the previous BT to ensure previous state doesn't leak into // the new cycle - // halt BT for a new execution cycle + // halt BT for a new execution cycle, + // get if the on_cancelled is triggered from blackboard and assert + // that the on_cancelled triggers after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a large action server goal handling duration action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -300,6 +339,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the action server will take 100ms before accepting the goal action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -327,14 +367,21 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the new cycle // halt BT for a new execution cycle + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancelled never can trigger after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(25ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -348,6 +395,90 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) EXPECT_EQ(result, BT::NodeStatus::SUCCESS); } +TEST_F(BTActionNodeTestFixture, test_server_cancel) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // setting a server timeout smaller than the time the action server will take to accept the goal + // to simulate a server timeout scenario + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 50ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // to keep track of the number of ticks it took to reach expected tick count + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(100ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 5 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 5); + + // send new goal to the action server for a new execution cycle + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 1000ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // reset state variable + ticks = 0; + config_->blackboard->set("on_cancelled_triggered", false); + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 7 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 7); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 75a115b8f58..2df39599a98 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -117,18 +117,18 @@ TEST_F(WaitActionTestFixture, test_ports) )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 1); + EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 1.0); xml_txt = R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 10); + EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 10.0); } TEST_F(WaitActionTestFixture, test_tick) @@ -137,7 +137,7 @@ TEST_F(WaitActionTestFixture, test_tick) R"( - + )"; diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp index 44986f8fdb9..1580a8d1bc0 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp @@ -45,7 +45,7 @@ class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT R"( - + )"; diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index cd32a9f48c4..de4aec7532e 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -93,7 +93,7 @@ class DriveOnHeading : public TimedBehavior * @brief Loop function to run behavior * @return Status of behavior */ - ResultStatus onCycleUpdate() + ResultStatus onCycleUpdate() override { rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index b5ec19ba626..c7beffd7862 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -105,13 +105,13 @@ BehaviorServer::loadBehaviorPlugins() auto node = shared_from_this(); for (size_t i = 0; i != behavior_ids_.size(); i++) { - behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]); try { + behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]); RCLCPP_INFO( get_logger(), "Creating behavior plugin %s of type %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str()); behaviors_.push_back(plugin_loader_.createUniqueInstance(behavior_types_[i])); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), "Failed to create behavior %s of type %s." " Exception: %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str(), diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 89d3e136cea..f39a0b8e887 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -17,15 +17,19 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, GroupAction, - IncludeLaunchDescription, SetEnvironmentVariable) +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ParameterFile -from nav2_common.launch import RewrittenYaml, ReplaceString +from nav2_common.launch import ReplaceString, RewrittenYaml def generate_launch_description(): @@ -51,8 +55,7 @@ def generate_launch_description(): # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Only it applys when `use_namespace` is True. # '' keyword shall be replaced by 'namespace' launch argument @@ -61,113 +64,134 @@ def generate_launch_description(): params_file = ReplaceString( source_file=params_file, replacements={'': ('/', namespace)}, - condition=IfCondition(use_namespace)) + condition=IfCondition(use_namespace), + ) configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack') + description='Whether to apply a namespace to the navigation stack', + ) declare_slam_cmd = DeclareLaunchArgument( - 'slam', - default_value='False', - description='Whether run a SLAM') + 'slam', default_value='False', description='Whether run a SLAM' + ) declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value='', - description='Full path to map yaml file to load') + 'map', default_value='', description='Full path to map yaml file to load' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Whether to use composed bringup') + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') + 'log_level', default_value='info', description='log level' + ) # Specify the actions - bringup_cmd_group = GroupAction([ - PushROSNamespace( - condition=IfCondition(use_namespace), - namespace=namespace), - - Node( - condition=IfCondition(use_composition), - name='nav2_container', - package='rclcpp_components', - executable='component_container_isolated', - parameters=[configured_params, {'autostart': autostart}], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - output='screen'), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), - condition=IfCondition(slam), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'use_respawn': use_respawn, - 'params_file': params_file}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, - 'localization_launch.py')), - condition=IfCondition(PythonExpression(['not ', slam])), - launch_arguments={'namespace': namespace, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container'}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container'}.items()), - ]) + bringup_cmd_group = GroupAction( + [ + PushROSNamespace(condition=IfCondition(use_namespace), namespace=namespace), + Node( + condition=IfCondition(use_composition), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + output='screen', + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'slam_launch.py') + ), + condition=IfCondition(slam), + launch_arguments={ + 'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'use_respawn': use_respawn, + 'params_file': params_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'localization_launch.py') + ), + condition=IfCondition(PythonExpression(['not ', slam])), + launch_arguments={ + 'namespace': namespace, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container', + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'navigation_launch.py') + ), + launch_arguments={ + 'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container', + }.items(), + ), + ] + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index fc1499a0f27..dbff594bc71 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -14,10 +14,16 @@ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, - IncludeLaunchDescription, LogInfo) +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + LogInfo, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution @@ -30,9 +36,9 @@ def generate_launch_description(): Launch arguments consist of robot name(which is namespace) and pose for initialization. Keep general yaml format for pose information. - ex) robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" - ex) robots:="robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; - robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}" + ex) robots:='robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}' + ex) robots:='robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; + robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}' """ # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') @@ -55,47 +61,64 @@ def generate_launch_description(): declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world file to load') + description='Full path to world file to load', + ) declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', - description='The simulator to use (gazebo or gzserver)') + description='The simulator to use (gazebo or gzserver)', + ) declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load') + description='Full path to map file to load', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + default_value=os.path.join( + bringup_dir, 'params', 'nav2_multirobot_params_all.yaml' + ), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='false', - description='Automatically startup the stacks') + 'autostart', + default_value='false', + description='Automatically startup the stacks', + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), - description='Full path to the RVIZ config file to use.') + description='Full path to the RVIZ config file to use.', + ) declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher') + description='Whether to start the robot state publisher', + ) declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) # Start Gazebo with plugin providing the robot spawning service start_gazebo_cmd = ExecuteProcess( - cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', world], - output='screen') + cmd=[ + simulator, + '--verbose', + '-s', + 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', + world, + ], + output='screen', + ) robots_list = ParseMultiRobotPose('robots').value() @@ -103,39 +126,53 @@ def generate_launch_description(): bringup_cmd_group = [] for robot_name in robots_list: init_pose = robots_list[robot_name] - group = GroupAction([ - LogInfo(msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose)]), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), - condition=IfCondition(use_rviz), - launch_arguments={'namespace': TextSubstitution(text=robot_name), - 'use_namespace': 'True', - 'rviz_config': rviz_config_file}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, - 'launch', - 'tb3_simulation_launch.py')), - launch_arguments={'namespace': robot_name, - 'use_namespace': 'True', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - 'use_simulator': 'False', - 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub, - 'x_pose': TextSubstitution(text=str(init_pose['x'])), - 'y_pose': TextSubstitution(text=str(init_pose['y'])), - 'z_pose': TextSubstitution(text=str(init_pose['z'])), - 'roll': TextSubstitution(text=str(init_pose['roll'])), - 'pitch': TextSubstitution(text=str(init_pose['pitch'])), - 'yaw': TextSubstitution(text=str(init_pose['yaw'])), - 'robot_name':TextSubstitution(text=robot_name), }.items()) - ]) + group = GroupAction( + [ + LogInfo( + msg=[ + 'Launching namespace=', + robot_name, + ' init_pose=', + str(init_pose), + ] + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py') + ), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': TextSubstitution(text=robot_name), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') + ), + launch_arguments={ + 'namespace': robot_name, + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(init_pose['x'])), + 'y_pose': TextSubstitution(text=str(init_pose['y'])), + 'z_pose': TextSubstitution(text=str(init_pose['z'])), + 'roll': TextSubstitution(text=str(init_pose['roll'])), + 'pitch': TextSubstitution(text=str(init_pose['pitch'])), + 'yaw': TextSubstitution(text=str(init_pose['yaw'])), + 'robot_name': TextSubstitution(text=robot_name), + }.items(), + ), + ] + ) bringup_cmd_group.append(group) @@ -157,16 +194,27 @@ def generate_launch_description(): ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['map yaml: ', map_yaml_file])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['params yaml: ', params_file])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['rviz config file: ', rviz_config_file])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['using robot state pub: ', use_robot_state_pub])) - ld.add_action(LogInfo(condition=IfCondition(log_settings), - msg=['autostart: ', autostart])) + ld.add_action( + LogInfo(condition=IfCondition(log_settings), msg=['map yaml: ', map_yaml_file]) + ) + ld.add_action( + LogInfo(condition=IfCondition(log_settings), msg=['params yaml: ', params_file]) + ) + ld.add_action( + LogInfo( + condition=IfCondition(log_settings), + msg=['rviz config file: ', rviz_config_file], + ) + ) + ld.add_action( + LogInfo( + condition=IfCondition(log_settings), + msg=['using robot state pub: ', use_robot_state_pub], + ) + ) + ld.add_action( + LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart]) + ) for cmd in bringup_cmd_group: ld.add_action(cmd) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 9a0219c885a..42afb5c2cb5 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -52,66 +52,78 @@ def generate_launch_description(): # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value='', - description='Full path to map yaml file to load') + 'map', default_value='', description='Full path to map yaml file to load' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True') + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') + 'log_level', default_value='info', description='log level' + ) load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', use_sim_time), Node( - condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')), + condition=IfCondition( + EqualsSubstitution(LaunchConfiguration('map'), '') + ), package='nav2_map_server', executable='map_server', name='map_server', @@ -120,19 +132,22 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('map'), '')), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('map'), '') + ), package='nav2_map_server', executable='map_server', name='map_server', output='screen', respawn=use_respawn, respawn_delay=2.0, - parameters=[configured_params, - {'yaml_filename': map_yaml_file}], + parameters=[configured_params, {'yaml_filename': map_yaml_file}], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_amcl', executable='amcl', @@ -142,16 +157,17 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - ] + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], ) # LoadComposableNode for map server twice depending if we should use the # value of map from a CLI or launch default or user defined value in the @@ -164,27 +180,35 @@ def generate_launch_description(): SetParameter('use_sim_time', use_sim_time), LoadComposableNodes( target_container=container_name_full, - condition=IfCondition(EqualsSubstitution(LaunchConfiguration('map'), '')), + condition=IfCondition( + EqualsSubstitution(LaunchConfiguration('map'), '') + ), composable_node_descriptions=[ ComposableNode( package='nav2_map_server', plugin='nav2_map_server::MapServer', name='map_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ], ), LoadComposableNodes( target_container=container_name_full, - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('map'), '')), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('map'), '') + ), composable_node_descriptions=[ ComposableNode( package='nav2_map_server', plugin='nav2_map_server::MapServer', name='map_server', - parameters=[configured_params, - {'yaml_filename': map_yaml_file}], - remappings=remappings), + parameters=[ + configured_params, + {'yaml_filename': map_yaml_file}, + ], + remappings=remappings, + ), ], ), LoadComposableNodes( @@ -195,16 +219,19 @@ def generate_launch_description(): plugin='nav2_amcl::AmclNode', name='amcl', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_localization', - parameters=[{'autostart': autostart, - 'node_names': lifecycle_nodes}]), + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), ], - ) - ] + ), + ], ) # Create the launch description and populate diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index f1f17c50e6f..2f438c66e03 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -40,13 +40,15 @@ def generate_launch_description(): use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') - lifecycle_nodes = ['controller_server', - 'smoother_server', - 'planner_server', - 'behavior_server', - 'bt_navigator', - 'waypoint_follower', - 'velocity_smoother'] + lifecycle_nodes = [ + 'controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother', + ] # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -54,58 +56,68 @@ def generate_launch_description(): # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'autostart': autostart} + param_substitutions = {'autostart': autostart} configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites=param_substitutions, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True') + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') + 'log_level', default_value='info', description='log level' + ) load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', use_composition])), @@ -119,7 +131,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), Node( package='nav2_smoother', executable='smoother_server', @@ -129,7 +142,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_planner', executable='planner_server', @@ -139,7 +153,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_behaviors', executable='behavior_server', @@ -149,7 +164,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_bt_navigator', executable='bt_navigator', @@ -159,7 +175,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_waypoint_follower', executable='waypoint_follower', @@ -169,7 +186,8 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_velocity_smoother', executable='velocity_smoother', @@ -179,17 +197,18 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}]), - ] + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], ) load_composable_nodes = GroupAction( @@ -204,53 +223,62 @@ def generate_launch_description(): plugin='nav2_controller::ControllerServer', name='controller_server', parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), ComposableNode( package='nav2_smoother', plugin='nav2_smoother::SmootherServer', name='smoother_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_planner', plugin='nav2_planner::PlannerServer', name='planner_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_behaviors', plugin='behavior_server::BehaviorServer', name='behavior_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_bt_navigator', plugin='nav2_bt_navigator::BtNavigator', name='bt_navigator', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_waypoint_follower', plugin='nav2_waypoint_follower::WaypointFollower', name='waypoint_follower', parameters=[configured_params], - remappings=remappings), + remappings=remappings, + ), ComposableNode( package='nav2_velocity_smoother', plugin='nav2_velocity_smoother::VelocitySmoother', name='velocity_smoother', parameters=[configured_params], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')], + ), ComposableNode( package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_navigation', - parameters=[{'autostart': autostart, - 'node_names': lifecycle_nodes}]), + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), ], - ) - ] + ), + ], ) # Create the launch description and populate diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index c87ae01b410..dee33f26d63 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -39,18 +39,23 @@ def generate_launch_description(): declare_namespace_cmd = DeclareLaunchArgument( 'namespace', default_value='navigation', - description=('Top-level namespace. The value will be used to replace the ' - ' keyword on the rviz config file.')) + description=( + 'Top-level namespace. The value will be used to replace the ' + ' keyword on the rviz config file.' + ), + ) declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack') + description='Whether to apply a namespace to the navigation stack', + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), - description='Full path to the RVIZ config file to use') + description='Full path to the RVIZ config file to use', + ) # Launch rviz start_rviz_cmd = Node( @@ -58,11 +63,13 @@ def generate_launch_description(): package='rviz2', executable='rviz2', arguments=['-d', rviz_config_file], - output='screen') + output='screen', + ) namespaced_rviz_config_file = ReplaceString( source_file=rviz_config_file, - replacements={'': ('/', namespace)}) + replacements={'': ('/', namespace)}, + ) start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), @@ -71,24 +78,31 @@ def generate_launch_description(): namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', - remappings=[('/map', 'map'), - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ('/goal_pose', 'goal_pose'), - ('/clicked_point', 'clicked_point'), - ('/initialpose', 'initialpose')]) + remappings=[ + ('/map', 'map'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('/goal_pose', 'goal_pose'), + ('/clicked_point', 'clicked_point'), + ('/initialpose', 'initialpose'), + ], + ) exit_event_handler = RegisterEventHandler( condition=UnlessCondition(use_namespace), event_handler=OnProcessExit( target_action=start_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), + ), + ) exit_event_handler_namespaced = RegisterEventHandler( condition=IfCondition(use_namespace), event_handler=OnProcessExit( target_action=start_namespaced_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), + ), + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index abb3c13d623..a806d260ce5 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -49,36 +49,43 @@ def generate_launch_description(): source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='True', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='True', + description='Automatically startup the nav2 stack', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') + 'log_level', default_value='info', description='log level' + ) # Nodes launching commands @@ -92,33 +99,40 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, arguments=['--ros-args', '--log-level', log_level], - parameters=[configured_params]), + parameters=[configured_params], + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_slam', output='screen', arguments=['--ros-args', '--log-level', log_level], - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - ]) + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ] + ) # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load # the default file - has_slam_toolbox_params = HasNodeParams(source_file=params_file, - node_name='slam_toolbox') + has_slam_toolbox_params = HasNodeParams( + source_file=params_file, node_name='slam_toolbox' + ) start_slam_toolbox_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(slam_launch_file), launch_arguments={'use_sim_time': use_sim_time}.items(), - condition=UnlessCondition(has_slam_toolbox_params)) + condition=UnlessCondition(has_slam_toolbox_params), + ) start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( PythonLaunchDescriptionSource(slam_launch_file), - launch_arguments={'use_sim_time': use_sim_time, - 'slam_params_file': params_file}.items(), - condition=IfCondition(has_slam_toolbox_params)) + launch_arguments={ + 'use_sim_time': use_sim_time, + 'slam_params_file': params_file, + }.items(), + condition=IfCondition(has_slam_toolbox_params), + ) ld = LaunchDescription() diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index e391fa863c1..6598ac94ba8 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -19,7 +19,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -51,12 +55,14 @@ def generate_launch_description(): use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') world = LaunchConfiguration('world') - pose = {'x': LaunchConfiguration('x_pose', default='-2.00'), - 'y': LaunchConfiguration('y_pose', default='-0.50'), - 'z': LaunchConfiguration('z_pose', default='0.01'), - 'R': LaunchConfiguration('roll', default='0.00'), - 'P': LaunchConfiguration('pitch', default='0.00'), - 'Y': LaunchConfiguration('yaw', default='0.00')} + pose = { + 'x': LaunchConfiguration('x_pose', default='-2.00'), + 'y': LaunchConfiguration('y_pose', default='-0.50'), + 'z': LaunchConfiguration('z_pose', default='0.01'), + 'R': LaunchConfiguration('roll', default='0.00'), + 'P': LaunchConfiguration('pitch', default='0.00'), + 'Y': LaunchConfiguration('yaw', default='0.00'), + } robot_name = LaunchConfiguration('robot_name') robot_sdf = LaunchConfiguration('robot_sdf') @@ -66,78 +72,84 @@ def generate_launch_description(): # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_namespace_cmd = DeclareLaunchArgument( 'use_namespace', default_value='false', - description='Whether to apply a namespace to the navigation stack') + description='Whether to apply a namespace to the navigation stack', + ) declare_slam_cmd = DeclareLaunchArgument( - 'slam', - default_value='False', - description='Whether run a SLAM') + 'slam', default_value='False', description='Whether run a SLAM' + ) declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value=os.path.join( - bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load') + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load', + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Whether to use composed bringup') + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', - default_value=os.path.join( - bringup_dir, 'rviz', 'nav2_default_view.rviz'), - description='Full path to the RVIZ config file to use') + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use', + ) declare_use_simulator_cmd = DeclareLaunchArgument( 'use_simulator', default_value='True', - description='Whether to start the simulator') + description='Whether to start the simulator', + ) declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher') + description='Whether to start the robot state publisher', + ) declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='True', - description='Whether to execute gzclient)') + 'headless', default_value='True', description='Whether to execute gzclient)' + ) declare_world_cmd = DeclareLaunchArgument( 'world', @@ -146,30 +158,40 @@ def generate_launch_description(): # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), # worlds/turtlebot3_worlds/waffle.model') default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world model file to load') + description='Full path to world model file to load', + ) declare_robot_name_cmd = DeclareLaunchArgument( - 'robot_name', - default_value='turtlebot3_waffle', - description='name of the robot') + 'robot_name', default_value='turtlebot3_waffle', description='name of the robot' + ) declare_robot_sdf_cmd = DeclareLaunchArgument( 'robot_sdf', default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), - description='Full path to robot sdf file to spawn the robot in gazebo') + description='Full path to robot sdf file to spawn the robot in gazebo', + ) # Specify the actions start_gazebo_server_cmd = ExecuteProcess( condition=IfCondition(use_simulator), - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', world], - cwd=[launch_dir], output='screen') + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', + world, + ], + cwd=[launch_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression( - [use_simulator, ' and not ', headless])), + condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])), cmd=['gzclient'], - cwd=[launch_dir], output='screen') + cwd=[launch_dir], + output='screen', + ) urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: @@ -182,41 +204,62 @@ def generate_launch_description(): name='robot_state_publisher', namespace=namespace, output='screen', - parameters=[{'use_sim_time': use_sim_time, - 'robot_description': robot_description}], - remappings=remappings) + parameters=[ + {'use_sim_time': use_sim_time, 'robot_description': robot_description} + ], + remappings=remappings, + ) start_gazebo_spawner_cmd = Node( package='gazebo_ros', executable='spawn_entity.py', output='screen', arguments=[ - '-entity', robot_name, - '-file', robot_sdf, - '-robot_namespace', namespace, - '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], - '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]) + '-entity', + robot_name, + '-file', + robot_sdf, + '-robot_namespace', + namespace, + '-x', + pose['x'], + '-y', + pose['y'], + '-z', + pose['z'], + '-R', + pose['R'], + '-P', + pose['P'], + '-Y', + pose['Y'], + ], + ) rviz_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), condition=IfCondition(use_rviz), - launch_arguments={'namespace': namespace, - 'use_namespace': use_namespace, - 'rviz_config': rviz_config_file}.items()) + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'rviz_config': rviz_config_file, + }.items(), + ) bringup_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'bringup_launch.py')), - launch_arguments={'namespace': namespace, - 'use_namespace': use_namespace, - 'slam': slam, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': params_file, - 'autostart': autostart, - 'use_composition': use_composition, - 'use_respawn': use_respawn}.items()) + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + }.items(), + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py index 069ae5bb3d3..2319def79ab 100644 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -25,8 +25,13 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, - IncludeLaunchDescription, LogInfo) +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + LogInfo, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution @@ -39,10 +44,25 @@ def generate_launch_description(): # Names and poses of the robots robots = [ - {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01, - 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}, - {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01, - 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}] + { + 'name': 'robot1', + 'x_pose': 0.0, + 'y_pose': 0.5, + 'z_pose': 0.01, + 'roll': 0.0, + 'pitch': 0.0, + 'yaw': 0.0, + }, + { + 'name': 'robot2', + 'x_pose': 0.0, + 'y_pose': -0.5, + 'z_pose': 0.01, + 'roll': 0.0, + 'pitch': 0.0, + 'yaw': 0.0, + }, + ] # Simulation settings world = LaunchConfiguration('world') @@ -61,108 +81,145 @@ def generate_launch_description(): declare_world_cmd = DeclareLaunchArgument( 'world', default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), - description='Full path to world file to load') + description='Full path to world file to load', + ) declare_simulator_cmd = DeclareLaunchArgument( 'simulator', default_value='gazebo', - description='The simulator to use (gazebo or gzserver)') + description='The simulator to use (gazebo or gzserver)', + ) declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map file to load') + description='Full path to map file to load', + ) declare_robot1_params_file_cmd = DeclareLaunchArgument( 'robot1_params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_1.yaml'), - description='Full path to the ROS2 parameters file to use for robot1 launched nodes') + default_value=os.path.join( + bringup_dir, 'params', 'nav2_multirobot_params_1.yaml' + ), + description='Full path to the ROS2 parameters file to use for robot1 launched nodes', + ) declare_robot2_params_file_cmd = DeclareLaunchArgument( 'robot2_params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_2.yaml'), - description='Full path to the ROS2 parameters file to use for robot2 launched nodes') + default_value=os.path.join( + bringup_dir, 'params', 'nav2_multirobot_params_2.yaml' + ), + description='Full path to the ROS2 parameters file to use for robot2 launched nodes', + ) declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='false', - description='Automatically startup the stacks') + 'autostart', + default_value='false', + description='Automatically startup the stacks', + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), - description='Full path to the RVIZ config file to use.') + description='Full path to the RVIZ config file to use.', + ) declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 'use_robot_state_pub', default_value='True', - description='Whether to start the robot state publisher') + description='Whether to start the robot state publisher', + ) declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) # Start Gazebo with plugin providing the robot spawning service start_gazebo_cmd = ExecuteProcess( - cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', world], - output='screen') + cmd=[ + simulator, + '--verbose', + '-s', + 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', + world, + ], + output='screen', + ) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: params_file = LaunchConfiguration(f"{robot['name']}_params_file") - group = GroupAction([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), - condition=IfCondition(use_rviz), - launch_arguments={'namespace': TextSubstitution(text=robot['name']), - 'use_namespace': 'True', - 'rviz_config': rviz_config_file}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, - 'launch', - 'tb3_simulation_launch.py')), - launch_arguments={'namespace': robot['name'], - 'use_namespace': 'True', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'autostart': autostart, - 'use_rviz': 'False', - 'use_simulator': 'False', - 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub, - 'x_pose': TextSubstitution(text=str(robot['x_pose'])), - 'y_pose': TextSubstitution(text=str(robot['y_pose'])), - 'z_pose': TextSubstitution(text=str(robot['z_pose'])), - 'roll': TextSubstitution(text=str(robot['roll'])), - 'pitch': TextSubstitution(text=str(robot['pitch'])), - 'yaw': TextSubstitution(text=str(robot['yaw'])), - 'robot_name':TextSubstitution(text=robot['name']), }.items()), - - LogInfo( - condition=IfCondition(log_settings), - msg=['Launching ', robot['name']]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' map yaml: ', map_yaml_file]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' params yaml: ', params_file]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' rviz config file: ', rviz_config_file]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' using robot state pub: ', use_robot_state_pub]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' autostart: ', autostart]) - ]) + group = GroupAction( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py') + ), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': TextSubstitution(text=robot['name']), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py') + ), + launch_arguments={ + 'namespace': robot['name'], + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(robot['x_pose'])), + 'y_pose': TextSubstitution(text=str(robot['y_pose'])), + 'z_pose': TextSubstitution(text=str(robot['z_pose'])), + 'roll': TextSubstitution(text=str(robot['roll'])), + 'pitch': TextSubstitution(text=str(robot['pitch'])), + 'yaw': TextSubstitution(text=str(robot['yaw'])), + 'robot_name': TextSubstitution(text=robot['name']), + }.items(), + ), + LogInfo( + condition=IfCondition(log_settings), + msg=['Launching ', robot['name']], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' map yaml: ', map_yaml_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' params yaml: ', params_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' rviz config file: ', rviz_config_file], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[ + robot['name'], + ' using robot state pub: ', + use_robot_state_pub, + ], + ), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' autostart: ', autostart], + ), + ] + ) nav_instances_cmds.append(group) diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index 0a00a60a4f5..b1d470435ef 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -37,7 +37,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 8616a88a848..9257836be88 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -29,7 +29,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 6843f3d8592..228c7ad223d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -38,7 +38,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index e2bbeb12c73..9de1f28e910 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -20,7 +20,7 @@ - + @@ -38,7 +38,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index bd4e1c9aca8..aac9a72ae0e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -35,7 +35,7 @@ - + diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 2605fc86497..aee07de14c9 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -32,7 +32,7 @@ namespace nav2_bt_navigator { /** - * @class NavigateToPoseNavigator + * @class NavigateThroughPosesNavigator * @brief A navigator for navigating to a a bunch of intermediary poses */ class NavigateThroughPosesNavigator @@ -104,8 +104,9 @@ class NavigateThroughPosesNavigator /** * @brief Goal pose initialization on the blackboard + * @return bool if goal was initialized successfully to be processed */ - void initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal); + bool initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal); rclcpp::Time start_time_; std::string goals_blackboard_id_; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index d256ebf0145..447d02cc449 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -72,7 +72,7 @@ class NavigateToPoseNavigator * @brief Get action name for this navigator * @return string Name of action server */ - std::string getName() {return std::string("navigate_to_pose");} + std::string getName() override {return std::string("navigate_to_pose");} /** * @brief Get navigator's default BT @@ -116,8 +116,9 @@ class NavigateToPoseNavigator /** * @brief Goal pose initialization on the blackboard * @param goal Action template's goal message to process + * @return bool if goal was initialized successfully to be processed */ - void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); + bool initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); rclcpp::Time start_time_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index c83ccd8b23d..82beb939994 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -161,8 +161,8 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) // Load navigator plugins for (size_t i = 0; i != navigator_ids.size(); i++) { - std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]); try { + std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]); RCLCPP_INFO( get_logger(), "Creating navigator id %s of type %s", navigator_ids[i].c_str(), navigator_type.c_str()); @@ -173,11 +173,10 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) { return nav2_util::CallbackReturn::FAILURE; } - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( - get_logger(), "Failed to create navigator id %s of type %s." - " Exception: %s", navigator_ids[i].c_str(), navigator_type.c_str(), - ex.what()); + get_logger(), "Failed to create navigator id %s." + " Exception: %s", navigator_ids[i].c_str(), ex.what()); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 92f53f9919a..348ca0860fe 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -81,9 +81,7 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) return false; } - initializeGoalPoses(goal); - - return true; + return initializeGoalPoses(goal); } void @@ -113,10 +111,14 @@ NavigateThroughPosesNavigator::onLoop() } geometry_msgs::msg::PoseStamped current_pose; - nav2_util::getCurrentPose( - current_pose, *feedback_utils_.tf, - feedback_utils_.global_frame, feedback_utils_.robot_frame, - feedback_utils_.transform_tolerance); + if (!nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR(logger_, "Robot pose is not available."); + return; + } try { // Get current path points @@ -185,7 +187,13 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file - initializeGoalPoses(bt_action_server_->acceptPendingGoal()); + if (!initializeGoalPoses(bt_action_server_->acceptPendingGoal())) { + RCLCPP_WARN( + logger_, + "Preemption request was rejected since the goal poses could not be " + "transformed. For now, continuing to track the last goal until completion."); + bt_action_server_->terminatePendingGoal(); + } } else { RCLCPP_WARN( logger_, @@ -198,13 +206,27 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) } } -void +bool NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal) { - if (goal->poses.size() > 0) { + Goals goal_poses = goal->poses; + for (auto & goal_pose : goal_poses) { + if (!nav2_util::transformPoseInTargetFrame( + goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR( + logger_, + "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.", + goal_pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str()); + return false; + } + } + + if (goal_poses.size() > 0) { RCLCPP_INFO( logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)", - goal->poses.size(), goal->poses.back().pose.position.x, goal->poses.back().pose.position.y); + goal_poses.size(), goal_poses.back().pose.position.x, goal_poses.back().pose.position.y); } // Reset state for new action feedback @@ -213,7 +235,9 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goals_blackboard_id_, goal->poses); + blackboard->set(goals_blackboard_id_, std::move(goal_poses)); + + return true; } } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index cbd686cc040..a8298fc36f7 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -94,9 +94,7 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) return false; } - initializeGoalPose(goal); - - return true; + return initializeGoalPose(goal); } void @@ -114,10 +112,14 @@ NavigateToPoseNavigator::onLoop() auto feedback_msg = std::make_shared(); geometry_msgs::msg::PoseStamped current_pose; - nav2_util::getCurrentPose( - current_pose, *feedback_utils_.tf, - feedback_utils_.global_frame, feedback_utils_.robot_frame, - feedback_utils_.transform_tolerance); + if (!nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR(logger_, "Robot pose is not available."); + return; + } auto blackboard = bt_action_server_->getBlackboard(); @@ -187,7 +189,13 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) // if pending goal requests the same BT as the current goal, accept the pending goal // if pending goal has an empty behavior_tree field, it requests the default BT file // accept the pending goal if the current goal is running the default BT file - initializeGoalPose(bt_action_server_->acceptPendingGoal()); + if (!initializeGoalPose(bt_action_server_->acceptPendingGoal())) { + RCLCPP_WARN( + logger_, + "Preemption request was rejected since the goal pose could not be " + "transformed. For now, continuing to track the last goal until completion."); + bt_action_server_->terminatePendingGoal(); + } } else { RCLCPP_WARN( logger_, @@ -200,19 +208,35 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) } } -void +bool NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) { geometry_msgs::msg::PoseStamped current_pose; - nav2_util::getCurrentPose( - current_pose, *feedback_utils_.tf, - feedback_utils_.global_frame, feedback_utils_.robot_frame, - feedback_utils_.transform_tolerance); + if (!nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR(logger_, "Initial robot pose is not available."); + return false; + } + + geometry_msgs::msg::PoseStamped goal_pose; + if (!nav2_util::transformPoseInTargetFrame( + goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, + feedback_utils_.transform_tolerance)) + { + RCLCPP_ERROR( + logger_, + "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.", + goal->pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str()); + return false; + } RCLCPP_INFO( logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)", current_pose.pose.position.x, current_pose.pose.position.y, - goal->pose.pose.position.x, goal->pose.pose.position.y); + goal_pose.pose.position.x, goal_pose.pose.position.y); // Reset state for new action feedback start_time_ = clock_->now(); @@ -220,7 +244,9 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goal_blackboard_id_, goal->pose); + blackboard->set(goal_blackboard_id_, goal_pose); + + return true; } void diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index f31c063041c..8cef0f609fc 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(nav2_common REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) ### Header ### @@ -37,6 +38,7 @@ set(dependencies nav2_util nav2_costmap_2d nav2_msgs + visualization_msgs ) set(monitor_executable_name collision_monitor) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index c4c5906b22a..7791265179f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -28,6 +28,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/msg/collision_detector_state.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" @@ -147,6 +148,9 @@ class CollisionDetector : public nav2_util::LifecycleNode /// @brief collision monitor state publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; + /// @brief Collision points marker publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + collision_points_marker_pub_; /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 0383dee3634..09a13658092 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -107,6 +108,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @param cmd_vel_in_topic Output name of cmd_vel_in topic * @param cmd_vel_out_topic Output name of cmd_vel_out topic * is required. + * @param state_topic topic name for publishing collision monitor state * @return True if all parameters were obtained or false in failure case */ bool getParameters( @@ -210,6 +212,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; + /// @brief Collision points marker publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + collision_points_marker_pub_; + /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index d5af32c2772..bfe8d09ed33 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -69,8 +69,9 @@ class PointCloud : public Source * @param curr_time Current node time for data interpolation * @param data Array where the data from source to be added. * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot */ - void getData( + bool getData( const rclcpp::Time & curr_time, std::vector & data) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index d18f974b336..7667ec860cf 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -86,6 +86,11 @@ class Polygon * @return Action type for current polygon */ ActionType getActionType() const; + /** + * @brief Obtains polygon enabled state + * @return Whether polygon is enabled + */ + bool getEnabled() const; /** * @brief Obtains polygon minimum points to enter inside polygon causing the action * @return Minimum number of data readings within a zone to trigger the action @@ -205,6 +210,13 @@ class Polygon */ void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + /** * @brief Checks if point is inside polygon * @param point Given point to check @@ -218,6 +230,8 @@ class Polygon nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of polygon @@ -236,6 +250,8 @@ class Polygon double time_before_collision_; /// @brief Time step for robot movement simulation double simulation_time_step_; + /// @brief Whether polygon is enabled + bool enabled_; /// @brief Polygon subscription rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 777b8e404ce..1dc15195f6e 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -69,8 +69,9 @@ class Range : public Source * @param curr_time Current node time for data interpolation * @param data Array where the data from source to be added. * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot */ - void getData( + bool getData( const rclcpp::Time & curr_time, std::vector & data) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index c3f7a11f3fa..efaf82e0e37 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -69,8 +69,9 @@ class Scan : public Source * @param curr_time Current node time for data interpolation * @param data Array where the data from source to be added. * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot */ - void getData( + bool getData( const rclcpp::Time & curr_time, std::vector & data) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 8bc750cc717..865a3c023b9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -69,12 +69,37 @@ class Source * @param curr_time Current node time for data interpolation * @param data Array where the data from source to be added. * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot */ - virtual void getData( + virtual bool getData( const rclcpp::Time & curr_time, std::vector & data) const = 0; + /** + * @brief Obtains source enabled state + * @return Whether source is enabled + */ + bool getEnabled() const; + + /** + * @brief Obtains the name of the data source + * @return Name of the data source + */ + std::string getSourceName() const; + + /** + * @brief Obtains the source_timeout parameter of the data source + * @return source_timeout parameter value of the data source + */ + rclcpp::Duration getSourceTimeout() const; + protected: + /** + * @brief Source configuration routine. + * @return True in case of everything is configured correctly, or false otherwise + */ + bool configure(); + /** * @brief Supporting routine obtaining ROS-parameters common for all data sources * @param source_topic Output name of source subscription topic @@ -91,12 +116,21 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + // ----- Variables ----- /// @brief Collision Monitor node nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of data source @@ -116,6 +150,8 @@ class Source /// @brief Whether to correct source data towards to base frame movement, /// considering the difference between current time and latest source time bool base_shift_correction_; + /// @brief Whether source is enabled + bool enabled_; }; // class Source } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index 5440cfedc36..c16bec55a4d 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -37,8 +37,7 @@ def generate_launch_description(): # Constant parameters lifecycle_nodes = ['collision_detector'] autostart = True - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Launch arguments # 1. Create the launch configuration variables @@ -51,33 +50,41 @@ def generate_launch_description(): # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + default_value=os.path.join( + package_dir, 'params', 'collision_monitor_params.yaml' + ), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Use composed bringup if True') + 'use_composition', + default_value='True', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True) + convert_types=True, + ) # Declare node launching commands load_nodes = GroupAction( @@ -85,25 +92,29 @@ def generate_launch_description(): actions=[ SetParameter('use_sim_time', use_sim_time), PushROSNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_collision_detector', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + remappings=remappings, + ), Node( package='nav2_collision_monitor', executable='collision_detector', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 parameters=[configured_params], - remappings=remappings) - ] + remappings=remappings, + ), + ], ) load_composable_nodes = GroupAction( @@ -111,8 +122,11 @@ def generate_launch_description(): actions=[ SetParameter('use_sim_time', use_sim_time), PushROSNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), LoadComposableNodes( target_container=container_name_full, composable_node_descriptions=[ @@ -120,18 +134,22 @@ def generate_launch_description(): package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_collision_detector', - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[ + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + remappings=remappings, + ), ComposableNode( package='nav2_collision_monitor', plugin='nav2_collision_monitor::CollisionDetector', name='collision_detector', parameters=[configured_params], - remappings=remappings) + remappings=remappings, + ), ], - ) - ] + ), + ], ) ld = LaunchDescription() diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index f4d0a953e3a..b00b43b2aa1 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -37,8 +37,7 @@ def generate_launch_description(): # Constant parameters lifecycle_nodes = ['collision_monitor'] autostart = True - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] # Launch arguments # 1. Create the launch configuration variables @@ -51,35 +50,44 @@ def generate_launch_description(): # 2. Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + 'namespace', default_value='', description='Top-level namespace' + ) declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='True', - description='Use simulation (Gazebo) clock if true') + description='Use simulation (Gazebo) clock if true', + ) declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + default_value=os.path.join( + package_dir, 'params', 'collision_monitor_params.yaml' + ), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Use composed bringup if True') + 'use_composition', + default_value='True', + description='Use composed bringup if True', + ) declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'container_name', + default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition', + ) configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, - convert_types=True), - allow_substs=True) + convert_types=True, + ), + allow_substs=True, + ) # Declare node launching commands load_nodes = GroupAction( @@ -87,25 +95,29 @@ def generate_launch_description(): actions=[ SetParameter('use_sim_time', use_sim_time), PushROSNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_collision_monitor', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + remappings=remappings, + ), Node( package='nav2_collision_monitor', executable='collision_monitor', output='screen', emulate_tty=True, # https://github.com/ros2/launch/issues/188 parameters=[configured_params], - remappings=remappings) - ] + remappings=remappings, + ), + ], ) load_composable_nodes = GroupAction( @@ -113,8 +125,11 @@ def generate_launch_description(): actions=[ SetParameter('use_sim_time', use_sim_time), PushROSNamespace( - condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), - namespace=namespace), + condition=IfCondition( + NotEqualsSubstitution(LaunchConfiguration('namespace'), '') + ), + namespace=namespace, + ), LoadComposableNodes( target_container=container_name_full, composable_node_descriptions=[ @@ -122,18 +137,22 @@ def generate_launch_description(): package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', name='lifecycle_manager_collision_monitor', - parameters=[{'autostart': autostart}, - {'node_names': lifecycle_nodes}], - remappings=remappings), + parameters=[ + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + remappings=remappings, + ), ComposableNode( package='nav2_collision_monitor', plugin='nav2_collision_monitor::CollisionMonitor', name='collision_monitor', parameters=[configured_params], - remappings=remappings) + remappings=remappings, + ), ], - ) - ] + ), + ], ) ld = LaunchDescription() diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index a63c6b2fd59..4889ea77f16 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -21,6 +21,7 @@ nav2_util nav2_costmap_2d nav2_msgs + visualization_msgs ament_cmake_gtest ament_lint_common diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index 218ce452397..68b4cd7b507 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -14,12 +14,15 @@ collision_detector: min_points: 4 visualize: True polygon_pub_topic: "polygon_front" + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 57c0f48c426..ec7aa6d6b19 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -22,6 +22,7 @@ collision_monitor: min_points: 4 visualize: True polygon_pub_topic: "polygon_stop" + enabled: True PolygonSlow: type: "polygon" points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] @@ -30,6 +31,7 @@ collision_monitor: slowdown_ratio: 0.3 visualize: True polygon_pub_topic: "polygon_slowdown" + enabled: True PolygonLimit: type: "polygon" points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] @@ -39,6 +41,7 @@ collision_monitor: angular_limit: 0.5 visualize: True polygon_pub_topic: "polygon_limit" + enabled: True FootprintApproach: type: "polygon" action_type: "approach" @@ -46,7 +49,8 @@ collision_monitor: time_before_collision: 2.0 simulation_time_step: 0.1 min_points: 6 - visualize: True + visualize: False + enabled: True polygon_pub_topic: "polygon_approach" velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stop"] # third priority rotation: @@ -81,8 +85,10 @@ collision_monitor: scan: type: "scan" topic: "scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 98433ead04c..b6c871c540f 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -55,6 +55,9 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) state_pub_ = this->create_publisher( "collision_detector_state", rclcpp::SystemDefaultsQoS()); + collision_points_marker_pub_ = this->create_publisher( + "~/collision_points_marker", 1); + // Obtaining ROS parameters if (!getParameters()) { return nav2_util::CallbackReturn::FAILURE; @@ -70,6 +73,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) // Activating lifecycle publisher state_pub_->on_activate(); + collision_points_marker_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -97,6 +101,7 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Deactivating lifecycle publishers state_pub_->on_deactivate(); + collision_points_marker_pub_->on_deactivate(); // Deactivating polygons for (std::shared_ptr polygon : polygons_) { @@ -115,6 +120,7 @@ CollisionDetector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Cleaning up"); state_pub_.reset(); + collision_points_marker_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -224,7 +230,6 @@ bool CollisionDetector::configurePolygons( polygon_name.c_str()); return false; } - } } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); @@ -302,15 +307,56 @@ void CollisionDetector::process() // Points array collected from different data sources in a robot base frame std::vector collision_points; + std::unique_ptr state_msg = + std::make_unique(); + // Fill collision_points array from different data sources for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); + if (source->getEnabled()) { + if (!source->getData(curr_time, collision_points) && + source->getSourceTimeout().seconds() != 0.0) + { + RCLCPP_WARN( + get_logger(), + "Invalid source %s detected." + " Either due to data not published yet, or to lack of new data received within the" + " sensor timeout, or if impossible to transform data to base frame", + source->getSourceName().c_str()); + } + } } - std::unique_ptr state_msg = - std::make_unique(); + if (collision_points_marker_pub_->get_subscription_count() > 0) { + // visualize collision points with markers + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker marker; + marker.header.frame_id = get_parameter("base_frame_id").as_string(); + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = "collision_points"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.color.r = 1.0; + marker.color.a = 1.0; + marker.lifetime = rclcpp::Duration(0, 0); + + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x; + p.y = point.y; + p.z = 0.0; + marker.points.push_back(p); + } + marker_array->markers.push_back(marker); + collision_points_marker_pub_->publish(std::move(marker_array)); + } for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } state_msg->polygons.push_back(polygon->getName()); state_msg->detections.push_back( polygon->getPointsInside( @@ -326,7 +372,9 @@ void CollisionDetector::process() void CollisionDetector::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 5042f37c72c..0f65dd27d17 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -68,11 +68,15 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1)); cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); + if (!state_topic.empty()) { state_pub_ = this->create_publisher( state_topic, 1); } + collision_points_marker_pub_ = this->create_publisher( + "~/collision_points_marker", 1); + return nav2_util::CallbackReturn::SUCCESS; } @@ -86,6 +90,7 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/) if (state_pub_) { state_pub_->on_activate(); } + collision_points_marker_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -126,6 +131,7 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) if (state_pub_) { state_pub_->on_deactivate(); } + collision_points_marker_pub_->on_deactivate(); // Destroying bond connection destroyBond(); @@ -141,6 +147,7 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/) cmd_vel_in_sub_.reset(); cmd_vel_out_pub_.reset(); state_pub_.reset(); + collision_points_marker_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -371,17 +378,59 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) // Points array collected from different data sources in a robot base frame std::vector collision_points; - // Fill collision_points array from different data sources - for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); - } - // By default - there is no action Action robot_action{DO_NOTHING, cmd_vel_in, ""}; // Polygon causing robot action (if any) std::shared_ptr action_polygon; + // Fill collision_points array from different data sources + for (std::shared_ptr source : sources_) { + if (source->getEnabled()) { + if (!source->getData(curr_time, collision_points) && + source->getSourceTimeout().seconds() != 0.0) + { + action_polygon = nullptr; + robot_action.polygon_name = "invalid source"; + robot_action.action_type = STOP; + robot_action.req_vel.x = 0.0; + robot_action.req_vel.y = 0.0; + robot_action.req_vel.tw = 0.0; + break; + } + } + } + + if (collision_points_marker_pub_->get_subscription_count() > 0) { + // visualize collision points with markers + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker marker; + marker.header.frame_id = get_parameter("base_frame_id").as_string(); + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = "collision_points"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.color.r = 1.0; + marker.color.a = 1.0; + marker.lifetime = rclcpp::Duration(0, 0); + + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x; + p.y = point.y; + p.z = 0.0; + marker.points.push_back(p); + } + marker_array->markers.push_back(marker); + collision_points_marker_pub_->publish(std::move(marker_array)); + } + for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } if (robot_action.action_type == STOP) { // If robot already should stop, do nothing break; @@ -409,7 +458,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) notifyActionState(robot_action, action_polygon); } - // Publish requred robot velocity + // Publish required robot velocity publishVelocity(robot_action); // Publish polygons for better visualization @@ -506,10 +555,18 @@ void CollisionMonitor::notifyActionState( const Action & robot_action, const std::shared_ptr action_polygon) const { if (robot_action.action_type == STOP) { - RCLCPP_INFO( - get_logger(), - "Robot to stop due to %s polygon", - action_polygon->getName().c_str()); + if (robot_action.polygon_name == "invalid source") { + RCLCPP_WARN( + get_logger(), + "Robot to stop due to invalid source." + " Either due to data not published yet, or to lack of new data received within the" + " sensor timeout, or if impossible to transform data to base frame"); + } else { + RCLCPP_INFO( + get_logger(), + "Robot to stop due to %s polygon", + action_polygon->getName().c_str()); + } } else if (robot_action.action_type == SLOWDOWN) { RCLCPP_INFO( get_logger(), @@ -545,7 +602,9 @@ void CollisionMonitor::notifyActionState( void CollisionMonitor::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 4582703b2f4..fb3fca201d0 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -49,6 +49,7 @@ PointCloud::~PointCloud() void PointCloud::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; @@ -64,17 +65,17 @@ void PointCloud::configure() std::bind(&PointCloud::dataCallback, this, std::placeholders::_1)); } -void PointCloud::getData( +bool PointCloud::getData( const rclcpp::Time & curr_time, std::vector & data) const { // Ignore data from the source if it is not being published yet or // not published for a long time if (data_ == nullptr) { - return; + return false; } if (!sourceValid(data_->header.stamp, curr_time)) { - return; + return false; } tf2::Transform tf_transform; @@ -87,7 +88,7 @@ void PointCloud::getData( base_frame_id_, curr_time, global_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } else { // Obtaining the transform to get data from source frame to base frame without time shift @@ -98,7 +99,7 @@ void PointCloud::getData( data_->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } @@ -117,6 +118,7 @@ void PointCloud::getData( data.push_back({p_v3_b.x(), p_v3_b.y()}); } } + return true; } void PointCloud::getParameters(std::string & source_topic) diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index e9ec5bbca60..8491ec31b61 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -49,6 +49,7 @@ Polygon::~Polygon() polygon_sub_.reset(); polygon_pub_.reset(); poly_.clear(); + dyn_params_handler_.reset(); } bool Polygon::configure() @@ -103,6 +104,10 @@ bool Polygon::configure() polygon_pub_topic, polygon_qos); } + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1)); + return true; } @@ -130,6 +135,11 @@ ActionType Polygon::getActionType() const return action_type_; } +bool Polygon::getEnabled() const +{ + return enabled_; +} + int Polygon::getMinPoints() const { return min_points_; @@ -341,6 +351,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return false; } + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool(); + nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".min_points", rclcpp::ParameterValue(4)); min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int(); @@ -537,6 +551,26 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m polygon_ = *msg; } +rcl_interfaces::msg::SetParametersResult +Polygon::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == polygon_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) { RCLCPP_INFO( diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index cd070d70be6..45540a9c4d0 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -49,6 +49,7 @@ Range::~Range() void Range::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; @@ -64,17 +65,17 @@ void Range::configure() std::bind(&Range::dataCallback, this, std::placeholders::_1)); } -void Range::getData( +bool Range::getData( const rclcpp::Time & curr_time, std::vector & data) const { // Ignore data from the source if it is not being published yet or // not being published for a long time if (data_ == nullptr) { - return; + return false; } if (!sourceValid(data_->header.stamp, curr_time)) { - return; + return false; } // Ignore data, if its range is out of scope of range sensor abilities @@ -83,7 +84,7 @@ void Range::getData( logger_, "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", source_name_.c_str(), data_->range, data_->min_range, data_->max_range); - return; + return false; } tf2::Transform tf_transform; @@ -96,7 +97,7 @@ void Range::getData( base_frame_id_, curr_time, global_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } else { // Obtaining the transform to get data from source frame to base frame without time shift @@ -107,7 +108,7 @@ void Range::getData( data_->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } @@ -141,6 +142,8 @@ void Range::getData( // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); + + return true; } void Range::getParameters(std::string & source_topic) diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 731ee8baa8b..45f1e62d701 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -47,6 +47,7 @@ Scan::~Scan() void Scan::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; @@ -63,17 +64,17 @@ void Scan::configure() std::bind(&Scan::dataCallback, this, std::placeholders::_1)); } -void Scan::getData( +bool Scan::getData( const rclcpp::Time & curr_time, std::vector & data) const { // Ignore data from the source if it is not being published yet or // not being published for a long time if (data_ == nullptr) { - return; + return false; } if (!sourceValid(data_->header.stamp, curr_time)) { - return; + return false; } tf2::Transform tf_transform; @@ -86,7 +87,7 @@ void Scan::getData( base_frame_id_, curr_time, global_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } else { // Obtaining the transform to get data from source frame to base frame without time shift @@ -97,7 +98,7 @@ void Scan::getData( data_->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } @@ -117,6 +118,7 @@ void Scan::getData( } angle += data_->angle_increment; } + return true; } void Scan::dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg) diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index df539495f25..a27876f05b3 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -43,6 +43,17 @@ Source::~Source() { } +bool Source::configure() +{ + auto node = node_.lock(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Source::dynamicParametersCallback, this, std::placeholders::_1)); + + return true; +} + void Source::getCommonParameters(std::string & source_topic) { auto node = node_.lock(); @@ -54,6 +65,16 @@ void Source::getCommonParameters(std::string & source_topic) node, source_name_ + ".topic", rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner source_topic = node->get_parameter(source_name_ + ".topic").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool(); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".source_timeout", + rclcpp::ParameterValue(source_timeout_.seconds())); // node source_timeout by default + source_timeout_ = rclcpp::Duration::from_seconds( + node->get_parameter(source_name_ + ".source_timeout").as_double()); } bool Source::sourceValid( @@ -63,7 +84,7 @@ bool Source::sourceValid( // Source is considered as not valid, if latest received data timestamp is earlier // than current time by source_timeout_ interval const rclcpp::Duration dt = curr_time - source_time; - if (dt > source_timeout_) { + if (source_timeout_.seconds() != 0.0 && dt > source_timeout_) { RCLCPP_WARN( logger_, "[%s]: Latest source and current collision monitor node timestamps differ on %f seconds. " @@ -75,4 +96,39 @@ bool Source::sourceValid( return true; } +bool Source::getEnabled() const +{ + return enabled_; +} + +std::string Source::getSourceName() const +{ + return source_name_; +} + +rclcpp::Duration Source::getSourceTimeout() const +{ + return source_timeout_; +} + +rcl_interfaces::msg::SetParametersResult +Source::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == source_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 3400f1d50d4..b0b926fde05 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -32,6 +32,7 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "tf2_ros/transform_broadcaster.h" @@ -45,11 +46,11 @@ static constexpr double EPSILON = 1e-5; static const char BASE_FRAME_ID[]{"base_link"}; static const char SOURCE_FRAME_ID[]{"base_source"}; static const char ODOM_FRAME_ID[]{"odom"}; -static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; static const char STATE_TOPIC[]{"collision_detector_state"}; +static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"}; static const int MIN_POINTS{1}; static const double SIMULATION_TIME_STEP{0.01}; static const double TRANSFORM_TOLERANCE{0.5}; @@ -145,6 +146,8 @@ class Tester : public ::testing::Test const rclcpp::Time & stamp); bool waitState(const std::chrono::nanoseconds & timeout); void stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg); + bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); + void collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg); protected: // CollisionDetector node @@ -157,6 +160,11 @@ class Tester : public ::testing::Test rclcpp::Subscription::SharedPtr state_sub_; nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; + + // CollisionMonitor collision points markers + rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; + }; // Tester Tester::Tester() @@ -173,6 +181,10 @@ Tester::Tester() state_sub_ = cd_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), std::bind(&Tester::stateCallback, this, std::placeholders::_1)); + + collision_points_marker_sub_ = cd_->create_subscription( + COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1)); } Tester::~Tester() @@ -180,6 +192,7 @@ Tester::~Tester() scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + collision_points_marker_sub_.reset(); cd_.reset(); } @@ -197,11 +210,30 @@ bool Tester::waitState(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) +{ + collision_points_marker_msg_ = nullptr; + rclcpp::Time start_time = cd_->now(); + while (rclcpp::ok() && cd_->now() - start_time <= rclcpp::Duration(timeout)) { + if (collision_points_marker_msg_) { + return true; + } + rclcpp::spin_some(cd_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg) { state_msg_ = msg; } +void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg) +{ + collision_points_marker_msg_ = msg; +} + void Tester::setCommonParameters() { cd_->declare_parameter( @@ -679,6 +711,33 @@ TEST_F(Tester, testPointcloudDetection) cd_->stop(); } +TEST_F(Tester, testCollisionPointsMarkers) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + setVectors({}, {SCAN_NAME}); + + // Start Collision Monitor node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_NE(collision_points_marker_msg_->markers[0].points.size(), 0u); + // Stop Collision Monitor node + cd_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index e11f3b1b3d9..94801a3d2a3 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -32,6 +32,7 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include "tf2_ros/transform_broadcaster.h" @@ -48,6 +49,7 @@ static const char ODOM_FRAME_ID[]{"odom"}; static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; static const char STATE_TOPIC[]{"collision_monitor_state"}; +static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_monitor/collision_points_marker"}; static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; @@ -160,11 +162,16 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp); bool waitCmdVel(const std::chrono::nanoseconds & timeout); + bool waitFuture( + rclcpp::Client::SharedFuture, + const std::chrono::nanoseconds & timeout); bool waitActionState(const std::chrono::nanoseconds & timeout); + bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout); protected: void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg); void actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPtr msg); + void collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg); // CollisionMonitor node std::shared_ptr cm_; @@ -186,6 +193,13 @@ class Tester : public ::testing::Test // CollisionMonitor Action state rclcpp::Subscription::SharedPtr action_state_sub_; nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; + + // CollisionMonitor collision points markers + rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; + + // Service client for setting CollisionMonitor parameters + rclcpp::Client::SharedPtr parameters_client_; }; // Tester Tester::Tester() @@ -211,6 +225,13 @@ Tester::Tester() action_state_sub_ = cm_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), std::bind(&Tester::actionStateCallback, this, std::placeholders::_1)); + collision_points_marker_sub_ = cm_->create_subscription( + COLLISION_POINTS_MARKERS_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::collisionPointsMarkerCallback, this, std::placeholders::_1)); + parameters_client_ = + cm_->create_client( + std::string( + cm_->get_name()) + "/set_parameters"); } Tester::~Tester() @@ -225,6 +246,7 @@ Tester::~Tester() cmd_vel_out_sub_.reset(); action_state_sub_.reset(); + collision_points_marker_sub_.reset(); cm_.reset(); } @@ -308,6 +330,11 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".type", "unknown")); } + cm_->declare_parameter( + polygon_name + ".enabled", rclcpp::ParameterValue(true)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".enabled", true)); + cm_->declare_parameter( polygon_name + ".action_type", rclcpp::ParameterValue(at)); cm_->set_parameter( @@ -539,6 +566,7 @@ void Tester::publishCmdVel(const double x, const double y, const double tw) // Reset cmd_vel_out_ before calling CollisionMonitor::process() cmd_vel_out_ = nullptr; action_state_ = nullptr; + collision_points_marker_msg_ = nullptr; std::unique_ptr msg = std::make_unique(); @@ -579,6 +607,22 @@ bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitFuture( + rclcpp::Client::SharedFuture result_future, + const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + std::future_status status = result_future.wait_for(10ms); + if (status == std::future_status::ready) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) { rclcpp::Time start_time = cm_->now(); @@ -592,6 +636,19 @@ bool Tester::waitActionState(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cm_->now(); + while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { + if (collision_points_marker_msg_) { + return true; + } + rclcpp::spin_some(cm_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg) { cmd_vel_out_ = msg; @@ -602,6 +659,11 @@ void Tester::actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPt action_state_ = msg; } +void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray::SharedPtr msg) +{ + collision_points_marker_msg_ = msg; +} + TEST_F(Tester, testProcessStopSlowdownLimit) { rclcpp::Time curr_time = cm_->now(); @@ -846,6 +908,7 @@ TEST_F(Tester, testCrossOver) // 1. Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). // Robot should approach the obstacle. publishPointCloud(2.5, curr_time); + publishRange(2.5, curr_time); ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); publishCmdVel(3.0, 0.0, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); @@ -890,6 +953,87 @@ TEST_F(Tester, testCrossOver) cm_->stop(); } +TEST_F(Tester, testSourceTimeout) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner circle + // as robot footprint for approach. + setCommonParameters(); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Approach", CIRCLE, 1.0, "approach"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + addSource(RANGE_NAME, RANGE); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). + // Robot should approach the obstacle. + publishPointCloud(2.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); + publishCmdVel(3.0, 3.0, 3.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // Range configured but not published, range source should be considered invalid + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "invalid source"); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testSourceTimeoutOverride) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner circle + // as robot footprint for approach. + setCommonParameters(); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Approach", CIRCLE, 1.0, "approach"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + addSource(RANGE_NAME, RANGE); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); + cm_->set_parameter(rclcpp::Parameter("source_timeout", 0.0)); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). + // Robot should approach the obstacle. + publishPointCloud(2.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); + publishCmdVel(3.0, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s + double change_ratio = (1.5 / 3.0) / TIME_BEFORE_COLLISION; + // Range configured but not published, range source should be considered invalid + // but as we set the source_timeout of the Range source to 0.0, its validity check is overidden + ASSERT_NEAR( + cmd_vel_out_->linear.x, 3.0 * change_ratio, 3.0 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, APPROACH); + ASSERT_EQ(action_state_->polygon_name, "Approach"); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testCeasePublishZeroVel) { rclcpp::Time curr_time = cm_->now(); @@ -961,6 +1105,116 @@ TEST_F(Tester, testCeasePublishZeroVel) cm_->stop(); } +TEST_F(Tester, testPolygonNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when polygon is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "Stop"); + + // Disable polygon by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = "Stop.enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when polygon is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + +TEST_F(Tester, testSourceNotEnabled) +{ + // Set Collision Monitor parameters. + // Create a STOP polygon + setCommonParameters(); + addPolygon("Stop", POLYGON, 1.0, "stop"); + // Create a Scan source + addSource(SCAN_NAME, SCAN); + setVectors({"Stop"}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Check that robot stops when source is enabled + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, STOP); + ASSERT_EQ(action_state_->polygon_name, "Stop"); + + // Disable source by calling service + auto set_parameters_msg = std::make_shared(); + auto parameter_msg = std::make_shared(); + parameter_msg->name = std::string(SCAN_NAME) + ".enabled"; + parameter_msg->value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + parameter_msg->value.bool_value = false; + set_parameters_msg->parameters.push_back(*parameter_msg); + auto result_future = parameters_client_->async_send_request(set_parameters_msg).future.share(); + ASSERT_TRUE(waitFuture(result_future, 2s)); + + // Check that robot does not stop when source is disabled + curr_time = cm_->now(); + sendTransforms(curr_time); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testProcessNonActive) { rclcpp::Time curr_time = cm_->now(); @@ -1022,13 +1276,42 @@ TEST_F(Tester, testSourcesNotSet) setCommonParameters(); addPolygon("Stop", POLYGON, 1.0, "stop"); addSource(SCAN_NAME, SCAN); - cm_->declare_parameter("polygons", rclcpp::ParameterValue({"Stop"})); + cm_->declare_parameter("polygons", rclcpp::ParameterValue(std::vector{"Stop"})); cm_->set_parameter(rclcpp::Parameter("polygons", std::vector{"Stop"})); // Check that Collision Monitor node can not be configured for this parameters set cm_->cant_configure(); } +TEST_F(Tester, testCollisionPointsMarkers) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + setVectors({}, {SCAN_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + + publishCmdVel(0.5, 0.2, 0.1); + publishScan(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + ASSERT_TRUE(waitCollisionPointsMarker(500ms)); + ASSERT_NE(collision_points_marker_msg_->markers[0].points.size(), 0u); + // Stop Collision Monitor node + cm_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index 7eba78e1928..61ea85dd793 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -13,6 +13,13 @@ # limitations under the License. from .has_node_params import HasNodeParams -from .rewritten_yaml import RewrittenYaml -from .replace_string import ReplaceString from .parse_multirobot_pose import ParseMultiRobotPose +from .replace_string import ReplaceString +from .rewritten_yaml import RewrittenYaml + +__all__ = [ + 'HasNodeParams', + 'RewrittenYaml', + 'ReplaceString', + 'ParseMultiRobotPose', +] diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py index 92f96790b5b..41f005bbc50 100644 --- a/nav2_common/nav2_common/launch/has_node_params.py +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -15,46 +15,48 @@ from typing import List from typing import Text -import yaml import launch +import yaml -import sys # delete this class HasNodeParams(launch.Substitution): - """ - Substitution that checks if a param file contains parameters for a node - - Used in launch system - """ + """ + Substitution that checks if a param file contains parameters for a node. - def __init__(self, - source_file: launch.SomeSubstitutionsType, - node_name: Text) -> None: - super().__init__() + Used in launch system """ + + def __init__( + self, source_file: launch.SomeSubstitutionsType, node_name: Text + ) -> None: + super().__init__() + """ Construct the substitution :param: source_file the parameter YAML file :param: node_name the name of the node to check """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop - self.__source_file = normalize_to_list_of_substitutions(source_file) - self.__node_name = node_name + from launch.utilities import ( + normalize_to_list_of_substitutions, + ) # import here to avoid loop + + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__node_name = node_name - @property - def name(self) -> List[launch.Substitution]: - """Getter for name.""" - return self.__source_file + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return '' + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' - def perform(self, context: launch.LaunchContext) -> Text: - yaml_filename = launch.utilities.perform_substitutions(context, self.name) - data = yaml.safe_load(open(yaml_filename, 'r')) + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + data = yaml.safe_load(open(yaml_filename, 'r')) - if self.__node_name in data.keys(): - return "True" - return "False" + if self.__node_name in data.keys(): + return 'True' + return 'False' diff --git a/nav2_common/nav2_common/launch/parse_multirobot_pose.py b/nav2_common/nav2_common/launch/parse_multirobot_pose.py index 9025f967fc5..bb1725d36de 100644 --- a/nav2_common/nav2_common/launch/parse_multirobot_pose.py +++ b/nav2_common/nav2_common/launch/parse_multirobot_pose.py @@ -12,19 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -import yaml import sys -from typing import Text, Dict +from typing import Dict, Text + +import yaml -class ParseMultiRobotPose(): - """ - Parsing argument using sys module - """ +class ParseMultiRobotPose: + """Parsing argument using sys module.""" def __init__(self, target_argument: Text): """ - Parse arguments for multi-robot's pose + Parse arguments for multi-robot's pose. for example, `ros2 launch nav2_bringup bringup_multirobot_launch.py @@ -42,23 +41,19 @@ def __init__(self, target_argument: Text): self.__args: Text = self.__parse_argument(target_argument) def __parse_argument(self, target_argument: Text) -> Text: - """ - get value of target argument - """ + """Get value of target argument.""" if len(sys.argv) > 4: argv = sys.argv[4:] for arg in argv: - if arg.startswith(target_argument + ":="): - return arg.replace(target_argument + ":=", "") - return "" + if arg.startswith(target_argument + ':='): + return arg.replace(target_argument + ':=', '') + return '' def value(self) -> Dict: - """ - get value of target argument - """ + """Get value of target argument.""" args = self.__args - parsed_args = list() if len(args) == 0 else args.split(';') - multirobots = dict() + parsed_args = [] if len(args) == 0 else args.split(';') + multirobots = {} for arg in parsed_args: key_val = arg.strip().split('=') if len(key_val) != 2: diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py index 9d22f1732df..076ac16f21f 100644 --- a/nav2_common/nav2_common/launch/replace_string.py +++ b/nav2_common/nav2_common/launch/replace_string.py @@ -12,76 +12,87 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Dict -from typing import List -from typing import Text -from typing import Optional import tempfile +from typing import Dict, List, Optional, Text + import launch -class ReplaceString(launch.Substitution): - """ - Substitution that replaces strings on a given file. - Used in launch system - """ +class ReplaceString(launch.Substitution): + """ + Substitution that replaces strings on a given file. - def __init__(self, - source_file: launch.SomeSubstitutionsType, - replacements: Dict, - condition: Optional[launch.Condition] = None) -> None: - super().__init__() + Used in launch system + """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop - self.__source_file = normalize_to_list_of_substitutions(source_file) - self.__replacements = {} - for key in replacements: - self.__replacements[key] = normalize_to_list_of_substitutions(replacements[key]) - self.__condition = condition + def __init__( + self, + source_file: launch.SomeSubstitutionsType, + replacements: Dict, + condition: Optional[launch.Condition] = None, + ) -> None: + super().__init__() - @property - def name(self) -> List[launch.Substitution]: - """Getter for name.""" - return self.__source_file + from launch.utilities import ( + normalize_to_list_of_substitutions, + ) # import here to avoid loop - @property - def condition(self) -> Optional[launch.Condition]: - """Getter for condition.""" - return self.__condition + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__replacements = {} + for key in replacements: + self.__replacements[key] = normalize_to_list_of_substitutions( + replacements[key] + ) + self.__condition = condition - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return '' + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file - def perform(self, context: launch.LaunchContext) -> Text: - yaml_filename = launch.utilities.perform_substitutions(context, self.name) - if self.__condition is None or self.__condition.evaluate(context): - output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) - replacements = self.resolve_replacements(context) - try: - input_file = open(yaml_filename, 'r') - self.replace(input_file, output_file, replacements) - except Exception as err: # noqa: B902 - print('ReplaceString substitution error: ', err) - finally: - input_file.close() - output_file.close() - return output_file.name - else: - return yaml_filename + @property + def condition(self) -> Optional[launch.Condition]: + """Getter for condition.""" + return self.__condition - def resolve_replacements(self, context): - resolved_replacements = {} - for key in self.__replacements: - resolved_replacements[key] = launch.utilities.perform_substitutions(context, self.__replacements[key]) - return resolved_replacements + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' - def replace(self, input_file, output_file, replacements): - for line in input_file: - for key, value in replacements.items(): - if isinstance(key, str) and isinstance(value, str): - if key in line: - line = line.replace(key, value) + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + if self.__condition is None or self.__condition.evaluate(context): + output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) + replacements = self.resolve_replacements(context) + try: + input_file = open(yaml_filename, 'r') + self.replace(input_file, output_file, replacements) + except Exception as err: # noqa: B902 + print('ReplaceString substitution error: ', err) + finally: + input_file.close() + output_file.close() + return output_file.name else: - raise TypeError('A provided replacement pair is not a string. Both key and value should be strings.') - output_file.write(line) + return yaml_filename + + def resolve_replacements(self, context): + resolved_replacements = {} + for key in self.__replacements: + resolved_replacements[key] = launch.utilities.perform_substitutions( + context, self.__replacements[key] + ) + return resolved_replacements + + def replace(self, input_file, output_file, replacements): + for line in input_file: + for key, value in replacements.items(): + if isinstance(key, str) and isinstance(value, str): + if key in line: + line = line.replace(key, value) + else: + raise TypeError( + 'A provided replacement pair is not a string. Both key and value should be' + 'strings.' + ) + output_file.write(line) diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 071c8848641..c28dc88cc00 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -12,14 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Dict -from typing import List -from typing import Text -from typing import Optional - -import yaml import tempfile +from typing import Dict, List, Optional, Text + import launch +import yaml class DictItemReference: @@ -41,12 +38,14 @@ class RewrittenYaml(launch.Substitution): Used in launch system """ - def __init__(self, + def __init__( + self, source_file: launch.SomeSubstitutionsType, param_rewrites: Dict, root_key: Optional[launch.SomeSubstitutionsType] = None, key_rewrites: Optional[Dict] = None, - convert_types = False) -> None: + convert_types=False, + ) -> None: super().__init__() """ Construct the substitution @@ -58,17 +57,24 @@ def __init__(self, :param: convert_types whether to attempt converting the string to a number or boolean """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop + from launch.utilities import ( + normalize_to_list_of_substitutions, + ) # import here to avoid loop + self.__source_file = normalize_to_list_of_substitutions(source_file) self.__param_rewrites = {} self.__key_rewrites = {} self.__convert_types = convert_types self.__root_key = None for key in param_rewrites: - self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key]) + self.__param_rewrites[key] = normalize_to_list_of_substitutions( + param_rewrites[key] + ) if key_rewrites is not None: for key in key_rewrites: - self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) + self.__key_rewrites[key] = normalize_to_list_of_substitutions( + key_rewrites[key] + ) if root_key is not None: self.__root_key = normalize_to_list_of_substitutions(root_key) @@ -100,10 +106,14 @@ def perform(self, context: launch.LaunchContext) -> Text: def resolve_rewrites(self, context): resolved_params = {} for key in self.__param_rewrites: - resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key]) + resolved_params[key] = launch.utilities.perform_substitutions( + context, self.__param_rewrites[key] + ) resolved_keys = {} for key in self.__key_rewrites: - resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key]) + resolved_keys[key] = launch.utilities.perform_substitutions( + context, self.__key_rewrites[key] + ) return resolved_params, resolved_keys def substitute_params(self, yaml, param_rewrites): @@ -126,7 +136,7 @@ def add_params(self, yaml, param_rewrites): # add new total path parameters yaml_paths = self.pathify(yaml) for path in param_rewrites: - if not path in yaml_paths: + if not path in yaml_paths: # noqa: E713 new_val = self.convert(param_rewrites[path]) yaml_keys = path.split('.') if 'ros__parameters' in yaml_keys: @@ -139,9 +149,13 @@ def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): break key = yaml_key_list.pop(0) if isinstance(yaml, list): - yaml[int(key)] = self.updateYamlPathVals(yaml[int(key)], yaml_key_list, rewrite_val) + yaml[int(key)] = self.updateYamlPathVals( + yaml[int(key)], yaml_key_list, rewrite_val + ) else: - yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) + yaml[key] = self.updateYamlPathVals( + yaml.get(key, {}), yaml_key_list, rewrite_val + ) return yaml def substitute_keys(self, yaml, key_rewrites): @@ -167,10 +181,10 @@ def getYamlLeafKeys(self, yamlData): def pathify(self, d, p=None, paths=None, joinchar='.'): if p is None: paths = {} - self.pathify(d, "", paths, joinchar=joinchar) + self.pathify(d, '', paths, joinchar=joinchar) return paths pn = p - if p != "": + if p != '': pn += joinchar if isinstance(d, dict): for k in d: @@ -191,9 +205,9 @@ def convert(self, text_value): pass # try converting to bool - if text_value.lower() == "true": + if text_value.lower() == 'true': return True - if text_value.lower() == "false": + if text_value.lower() == 'false': return False # nothing else worked so fall through and return text diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 463af33fd08..1ccae77f774 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -100,7 +100,7 @@ class SmootherTest : public ::testing::Test SmootherTest() {SetUp();} ~SmootherTest() {} - void SetUp() + void SetUp() override { node_lifecycle_ = std::make_shared( diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 69b06e3128e..0386c06fc7d 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -142,7 +142,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) progress_checker_ids_[i].c_str(), progress_checker_types_[i].c_str()); progress_checker->initialize(node, progress_checker_ids_[i]); progress_checkers_.insert({progress_checker_ids_[i], progress_checker}); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); @@ -242,7 +242,10 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_ros_->activate(); + const auto costmap_ros_state = costmap_ros_->activate(); + if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + return nav2_util::CallbackReturn::FAILURE; + } ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); @@ -594,7 +597,7 @@ void ControllerServer::computeAndPublishVelocity() // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & e) { if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { - RCLCPP_WARN(this->get_logger(), e.what()); + RCLCPP_WARN(this->get_logger(), "%s", e.what()); cmd_vel_2d.twist.angular.x = 0; cmd_vel_2d.twist.angular.y = 0; cmd_vel_2d.twist.angular.z = 0; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index 6fbb5352440..7ae3f4d1c76 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp @@ -72,5 +72,5 @@ static constexpr unsigned char LETHAL_OBSTACLE = 254; static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; static constexpr unsigned char MAX_NON_OBSTACLE = 252; static constexpr unsigned char FREE_SPACE = 0; -} +} // namespace nav2_costmap_2d #endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 46a71c8e76d..f319e52f330 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -360,7 +360,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool always_send_full_costmap_{false}; std::string footprint_; float footprint_padding_{0}; - std::string global_frame_; ///< The global frame for the costmap + std::string global_frame_; ///< The global frame for the costmap int map_height_meters_{0}; double map_publish_frequency_{0}; double map_update_frequency_{0}; @@ -374,11 +374,12 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector filter_names_; std::vector filter_types_; double resolution_{0}; - std::string robot_base_frame_; ///< The frame_id of the robot base + std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; - bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap + bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap bool track_unknown_space_{false}; - double transform_tolerance_{0}; ///< The timeout before transform errors + double transform_tolerance_{0}; ///< The timeout before transform errors + double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors // Derived parameters bool use_radius_{false}; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp index db7ae8fce84..888455168d3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp @@ -50,7 +50,7 @@ class Image * Share image data between new and old object. * Changing data in a new object will affect the given one and vice versa */ - Image(Image & other); + Image(const Image & other); /** * @brief Create image from the other (move constructor) @@ -132,7 +132,7 @@ Image::Image(size_t rows, size_t columns, T * data, size_t step) } template -Image::Image(Image & other) +Image::Image(const Image & other) : data_start_{other.data_start_}, rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp index 4527e07fdd3..99d24240d2c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include namespace nav2_costmap_2d { @@ -77,7 +79,7 @@ void morphologyOperation( const Image & input, Image & output, const Image & shape, AggregateFn aggregate); -using ShapeBuffer3x3 = std::array; +using ShapeBuffer3x3 = std::array; // NOLINT inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity); } // namespace imgproc_impl @@ -95,7 +97,7 @@ inline void dilate( const Image & input, Image & output, ConnectivityType connectivity, Max && max_function) { - using namespace imgproc_impl; + using namespace imgproc_impl; // NOLINT ShapeBuffer3x3 shape_buffer; Image shape = createShape(shape_buffer, connectivity); morphologyOperation(input, output, shape, max_function); @@ -376,7 +378,7 @@ struct EquivalenceLabelTreesBase struct LabelOverflow : public std::runtime_error { - LabelOverflow(const std::string & message) + explicit LabelOverflow(const std::string & message) : std::runtime_error(message) {} }; @@ -464,7 +466,6 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase { Label k = 1; for (Label i = 1; i < next_free_; ++i) { - if (labels_[i] < i) { labels_[i] = labels_[labels_[i]]; } else { @@ -504,7 +505,7 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase * '~' - row continuation in the same style */ max_labels = (rows * columns) / 3 + 1; } - ++max_labels; // add zero label + ++max_labels; // add zero label max_labels = std::min(max_labels, size_t(std::numeric_limits + + mppi critic for aligning to path (legacy) + + mppi critic for tracking the path in the correct heading diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index fe17906bae4..fd6eeab0128 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -92,6 +92,7 @@ class ObstaclesCritic : public CriticFunction float possibly_inscribed_cost_; float collision_margin_distance_; float near_goal_distance_; + float circumscribed_cost_{0}, circumscribed_radius_{0}; unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index a5499955575..f7ad2fda6a9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp new file mode 100644 index 00000000000..65d9d0d61e7 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::PathAlignLegacyCritic + * @brief Critic objective function for aligning to the path. Note: + * High settings of this will follow the path more precisely, but also makes it + * difficult (or impossible) to deviate in the presence of dynamic obstacles. + * This is an important critic to tune and consider in tandem with Obstacle. + * This is the initial 'Legacy' implementation before replacement Oct 2023. + */ +class PathAlignLegacyCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to trajectories path alignment + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + size_t offset_from_furthest_{0}; + int trajectory_point_step_{0}; + float threshold_to_consider_{0}; + float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index 6eb1b36384c..a811d998e41 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -25,8 +25,9 @@ #include #include "nav2_mppi_controller/models/optimizer_settings.hpp" -#include -#include +#include "nav2_mppi_controller/tools/parameters_handler.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/state.hpp" namespace mppi { @@ -47,8 +48,12 @@ class NoiseGenerator * @brief Initialize noise generator with settings and model types * @param settings Settings of controller * @param is_holonomic If base is holonomic + * @param name Namespace for configs + * @param param_handler Get parameters util */ - void initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic); + void initialize( + mppi::models::OptimizerSettings & settings, + bool is_holonomic, const std::string & name, ParametersHandler * param_handler); /** * @brief Shutdown noise generator thread @@ -99,7 +104,7 @@ class NoiseGenerator std::thread noise_thread_; std::condition_variable noise_cond_; std::mutex noise_lock_; - bool active_{false}, ready_{false}; + bool active_{false}, ready_{false}, regenerate_noises_{false}; }; } // namespace mppi diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 6baa27873eb..4a67bc276cc 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -308,14 +308,16 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) const auto dists = dx * dx + dy * dy; - size_t max_id_by_trajectories = 0; + size_t max_id_by_trajectories = 0, min_id_by_path = 0; float min_distance_by_path = std::numeric_limits::max(); + float cur_dist = 0.0f; for (size_t i = 0; i < dists.shape(0); i++) { - size_t min_id_by_path = 0; + min_id_by_path = 0; for (size_t j = 0; j < dists.shape(1); j++) { - if (dists(i, j) < min_distance_by_path) { - min_distance_by_path = dists(i, j); + cur_dist = dists(i, j); + if (cur_dist < min_distance_by_path) { + min_distance_by_path = cur_dist; min_id_by_path = j; } } @@ -688,6 +690,23 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) return first_after_inversion; } +/** + * @brief Compare to trajectory points to find closest path point along integrated distances + * @param vec Vect to check + * @return dist Distance to look for + */ +inline size_t findClosestPathPt(const std::vector & vec, float dist, size_t init = 0) +{ + auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist); + if (iter == vec.begin()) { + return 0; + } + if (dist - *(iter - 1) < *iter - dist) { + return iter - 1 - vec.begin(); + } + return iter - vec.begin(); +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 5cb4d4cf053..2bfdfd945e7 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -4,8 +4,8 @@ nav2_mppi_controller 1.2.0 nav2_mppi_controller - Aleksei Budyakov Steve Macenski + Aleksei Budyakov MIT ament_cmake diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 5f8dcffd261..6c831312633 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -83,9 +83,12 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( auto start = std::chrono::system_clock::now(); #endif - std::lock_guard lock(*parameters_handler_->getLock()); + std::lock_guard param_lock(*parameters_handler_->getLock()); nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock costmap_lock(*(costmap->getMutex())); + geometry_msgs::msg::TwistStamped cmd = optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 62bfdf0676d..604ee24eb1d 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -35,11 +35,7 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - if (!utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.path)) { return; diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 852db11529d..f61d6fd2b21 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -35,11 +35,7 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - if (!utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.path)) { return; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 80ac77e10f5..b8d6c898709 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -55,6 +55,13 @@ float ObstaclesCritic::findCircumscribedCost( { double result = -1.0; bool inflation_layer_found = false; + + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + // check if the costmap has an inflation layer for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin(); layer != costmap->getLayeredCostmap()->getPlugins()->end(); @@ -66,7 +73,6 @@ float ObstaclesCritic::findCircumscribedCost( } inflation_layer_found = true; - const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); const double resolution = costmap->getCostmap()->getResolution(); result = inflation_layer->computeCost(circum_radius / resolution); inflation_scale_factor_ = static_cast(inflation_layer->getCostScalingFactor()); @@ -83,7 +89,10 @@ float ObstaclesCritic::findCircumscribedCost( "significantly slow down planning times and not avoid anything but absolute collisions!"); } - return static_cast(result); + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; } float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost) @@ -108,6 +117,11 @@ void ObstaclesCritic::score(CriticData & data) return; } + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + } + // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) { @@ -152,7 +166,7 @@ void ObstaclesCritic::score(CriticData & data) } if (!trajectory_collide) {all_trajectories_collide = false;} - raw_cost[i] = static_cast(trajectory_collide ? collision_cost_ : traj_cost); + raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost; } data.costs += xt::pow( diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 4ff12d4e553..cd913e5b0ee 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -54,7 +54,8 @@ void PathAlignCritic::score(CriticData & data) // Don't apply when first getting bearing w.r.t. the path utils::setPathFurthestPointIfNotSet(data); - if (*data.furthest_reached_path_point < offset_from_furthest_) { + const size_t path_segments_count = *data.furthest_reached_path_point; // up to furthest only + if (path_segments_count < offset_from_furthest_) { return; } @@ -70,59 +71,64 @@ void PathAlignCritic::score(CriticData & data) } } - const auto & T_x = data.trajectories.x; - const auto & T_y = data.trajectories.y; - const auto & T_yaw = data.trajectories.yaws; - const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points - const size_t batch_size = T_x.shape(0); - const size_t time_steps = T_x.shape(1); - const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); - const size_t path_segments_count = data.path.x.shape(0) - 1; + const size_t batch_size = data.trajectories.x.shape(0); + const size_t time_steps = data.trajectories.x.shape(1); auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); - if (path_segments_count < 1) { - return; + // Find integrated distance in the path + std::vector path_integrated_distances(path_segments_count, 0.0f); + float dx = 0.0f, dy = 0.0f; + for (unsigned int i = 1; i != path_segments_count; i++) { + dx = P_x(i) - P_x(i - 1); + dy = P_y(i) - P_y(i - 1); + float curr_dist = sqrtf(dx * dx + dy * dy); + path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist; } - float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; - float min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; - + float traj_integrated_distance = 0.0f; + float summed_path_dist = 0.0f, dyaw = 0.0f; + float num_samples = 0.0f; + float Tx = 0.0f, Ty = 0.0f; + size_t path_pt = 0; for (size_t t = 0; t < batch_size; ++t) { - summed_dist = 0.0f; + traj_integrated_distance = 0.0f; + summed_path_dist = 0.0f; + num_samples = 0.0f; + const auto T_x = xt::view(data.trajectories.x, t, xt::all()); + const auto T_y = xt::view(data.trajectories.y, t, xt::all()); for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - min_dist_sq = std::numeric_limits::max(); - min_s = 0; - - // Find closest path segment to the trajectory point - for (size_t s = 0; s < path_segments_count - 1; s++) { - xt::xtensor_fixed> P; - dx = P_x(s) - T_x(t, p); - dy = P_y(s) - T_y(t, p); - if (use_path_orientations_) { - dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); - dist_sq = dx * dx + dy * dy + dyaw * dyaw; - } else { - dist_sq = dx * dx + dy * dy; - } - if (dist_sq < min_dist_sq) { - min_dist_sq = dist_sq; - min_s = s; - } - } + Tx = T_x(p); + Ty = T_y(p); + dx = Tx - T_x(p - trajectory_point_step_); + dy = Ty - T_y(p - trajectory_point_step_); + traj_integrated_distance += sqrtf(dx * dx + dy * dy); + path_pt = utils::findClosestPathPt( + path_integrated_distances, traj_integrated_distance, path_pt); // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles - if (min_s != 0 && (*data.path_pts_valid)[min_s]) { - summed_dist += sqrtf(min_dist_sq); + if ((*data.path_pts_valid)[path_pt]) { + dx = P_x(path_pt) - Tx; + dy = P_y(path_pt) - Ty; + num_samples += 1.0f; + if (use_path_orientations_) { + const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all()); + dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p)); + summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); + } else { + summed_path_dist += sqrtf(dx * dx + dy * dy); + } } } - - cost[t] = summed_dist / traj_pts_eval; + if (num_samples > 0) { + cost[t] = summed_path_dist / num_samples; + } else { + cost[t] = 0.0f; + } } data.costs += xt::pow(std::move(cost) * weight_, power_); diff --git a/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp new file mode 100644 index 00000000000..a04f4a9fa95 --- /dev/null +++ b/nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_mppi_controller/critics/path_align_legacy_critic.hpp" + +#include +#include + +namespace mppi::critics +{ + +using namespace xt::placeholders; // NOLINT +using xt::evaluation_strategy::immediate; + +void PathAlignLegacyCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 10.0); + + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07); + getParam(offset_from_furthest_, "offset_from_furthest", 20); + getParam(trajectory_point_step_, "trajectory_point_step", 4); + getParam( + threshold_to_consider_, + "threshold_to_consider", 0.5); + getParam(use_path_orientations_, "use_path_orientations", false); + + RCLCPP_INFO( + logger_, + "ReferenceTrajectoryCritic instantiated with %d power and %f weight", + power_, weight_); +} + +void PathAlignLegacyCritic::score(CriticData & data) +{ + // Don't apply close to goal, let the goal critics take over + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { + return; + } + + // Don't apply when first getting bearing w.r.t. the path + utils::setPathFurthestPointIfNotSet(data); + if (*data.furthest_reached_path_point < offset_from_furthest_) { + return; + } + + // Don't apply when dynamic obstacles are blocking significant proportions of the local path + utils::setPathCostsIfNotSet(data, costmap_ros_); + const size_t closest_initial_path_point = utils::findPathTrajectoryInitialPoint(data); + unsigned int invalid_ctr = 0; + const float range = *data.furthest_reached_path_point - closest_initial_path_point; + for (size_t i = closest_initial_path_point; i < *data.furthest_reached_path_point; i++) { + if (!(*data.path_pts_valid)[i]) {invalid_ctr++;} + if (static_cast(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) { + return; + } + } + + const auto & T_x = data.trajectories.x; + const auto & T_y = data.trajectories.y; + const auto & T_yaw = data.trajectories.yaws; + + const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points + const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points + const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points + + const size_t batch_size = T_x.shape(0); + const size_t time_steps = T_x.shape(1); + const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_); + const size_t path_segments_count = data.path.x.shape(0) - 1; + auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); + + if (path_segments_count < 1) { + return; + } + + float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; + float min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; + + for (size_t t = 0; t < batch_size; ++t) { + summed_dist = 0.0f; + for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { + min_dist_sq = std::numeric_limits::max(); + min_s = 0; + + // Find closest path segment to the trajectory point + for (size_t s = 0; s < path_segments_count - 1; s++) { + xt::xtensor_fixed> P; + dx = P_x(s) - T_x(t, p); + dy = P_y(s) - T_y(t, p); + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); + dist_sq = dx * dx + dy * dy + dyaw * dyaw; + } else { + dist_sq = dx * dx + dy * dy; + } + if (dist_sq < min_dist_sq) { + min_dist_sq = dist_sq; + min_s = s; + } + } + + // The nearest path point to align to needs to be not in collision, else + // let the obstacle critic take over in this region due to dynamic obstacles + if (min_s != 0 && (*data.path_pts_valid)[min_s]) { + summed_dist += sqrtf(min_dist_sq); + } + } + + cost[t] = summed_dist / traj_pts_eval; + } + + data.costs += xt::pow(std::move(cost) * weight_, power_); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::PathAlignLegacyCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 18b6900177a..08e755dd6af 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -33,12 +33,9 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 28eb71b48b6..6b492d13ba0 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -31,11 +31,9 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) + { return; } diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 09ee4ab92d7..b7c452aa6a4 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -23,12 +23,22 @@ namespace mppi { -void NoiseGenerator::initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic) +void NoiseGenerator::initialize( + mppi::models::OptimizerSettings & settings, bool is_holonomic, + const std::string & name, ParametersHandler * param_handler) { settings_ = settings; is_holonomic_ = is_holonomic; active_ = true; - noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + + auto getParam = param_handler->getParamGetter(name); + getParam(regenerate_noises_, "regenerate_noises", false); + + if (regenerate_noises_) { + noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + } else { + generateNoisedControls(); + } } void NoiseGenerator::shutdown() @@ -44,7 +54,7 @@ void NoiseGenerator::shutdown() void NoiseGenerator::generateNextNoises() { // Trigger the thread to run in parallel to this iteration - // to generate the next iteration's noises. + // to generate the next iteration's noises (if applicable). { std::unique_lock guard(noise_lock_); ready_ = true; @@ -76,7 +86,12 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h xt::noalias(noises_wz_) = xt::zeros({settings_.batch_size, settings_.time_steps}); ready_ = true; } - noise_cond_.notify_all(); + + if (regenerate_noises_) { + noise_cond_.notify_all(); + } else { + generateNoisedControls(); + } } void NoiseGenerator::noiseThread() diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 6d0a3ceb6c1..3b053f45771 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -49,7 +50,7 @@ void Optimizer::initialize( getParams(); critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_); - noise_generator_.initialize(settings_, isHolonomic()); + noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_); reset(); } @@ -268,7 +269,7 @@ void Optimizer::integrateStateVelocities( xt::xtensor & trajectory, const xt::xtensor & sequence) const { - double initial_yaw = tf2::getYaw(state_.pose.pose.orientation); + float initial_yaw = tf2::getYaw(state_.pose.pose.orientation); const auto vx = xt::view(sequence, xt::all(), 0); const auto vy = xt::view(sequence, xt::all(), 2); @@ -278,8 +279,7 @@ void Optimizer::integrateStateVelocities( auto traj_y = xt::view(trajectory, xt::all(), 1); auto traj_yaws = xt::view(trajectory, xt::all(), 2); - xt::noalias(traj_yaws) = - utils::normalize_angles(xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw); + xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw; auto && yaw_cos = xt::xtensor::from_shape(traj_yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(traj_yaws.shape()); @@ -307,10 +307,10 @@ void Optimizer::integrateStateVelocities( models::Trajectories & trajectories, const models::State & state) const { - const double initial_yaw = tf2::getYaw(state.pose.pose.orientation); + const float initial_yaw = tf2::getYaw(state.pose.pose.orientation); xt::noalias(trajectories.yaws) = - utils::normalize_angles(xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw); + xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw; const auto yaws_cutted = xt::view(trajectories.yaws, xt::all(), xt::range(0, -1)); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 8b5efae8da4..f88249aed14 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -25,6 +25,7 @@ #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" +#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" @@ -586,7 +587,111 @@ TEST(CriticTests, PathAlignCritic) path.x(21) = 0.9; generated_trajectories.x = 0.66 * xt::ones({1000, 30}); critic.score(data); - // 0.04 * 1000 * 10 weight * 4 num pts eval / 4 normalization term + // 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term + EXPECT_NEAR(xt::sum(costs, immediate)(), 6600.0, 1e-2); + + // provide state pose and path far enough to enable, with data to pass condition + // but path is blocked in collision + auto * costmap = costmap_ros->getCostmap(); + // island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution + for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m + for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m + costmap->setCost(i, j, 254); + } + } + + data.path_pts_valid.reset(); // Recompute on new path + costs = xt::zeros({1000}); + path.x = 1.5 * xt::ones({22}); + path.y = 1.5 * xt::ones({22}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); +} + +TEST(CriticTests, PathAlignLegacyCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + state.reset(1000, 30); + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + generated_trajectories.reset(1000, 30); + models::Path path; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally + data.goal_checker = &goal_checker; + + // Initialization testing + + // Make sure initializes correctly + PathAlignLegacyCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide state poses and path close within positional tolerances + state.pose.pose.position.x = 1.0; + path.reset(10); + path.x(9) = 0.85; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable + // but data furthest point reached is 0 and offset default is 20, so returns + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // but with empty trajectories and paths, should still be zero + *data.furthest_reached_path_point = 21; + path.x(9) = 0.15; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path far enough to enable, with data to pass condition + // and with a valid path to pass invalid path condition + state.pose.pose.position.x = 0.0; + data.path_pts_valid.reset(); // Recompute on new path + path.reset(22); + path.x(0) = 0; + path.x(1) = 0.1; + path.x(2) = 0.2; + path.x(3) = 0.3; + path.x(4) = 0.4; + path.x(5) = 0.5; + path.x(6) = 0.6; + path.x(7) = 0.7; + path.x(8) = 0.8; + path.x(9) = 0.9; + path.x(10) = 0.9; + path.x(11) = 0.9; + path.x(12) = 0.9; + path.x(13) = 0.9; + path.x(14) = 0.9; + path.x(15) = 0.9; + path.x(16) = 0.9; + path.x(17) = 0.9; + path.x(18) = 0.9; + path.x(19) = 0.9; + path.x(20) = 0.9; + path.x(21) = 0.9; + generated_trajectories.x = 0.66 * xt::ones({1000, 30}); + critic.score(data); + // 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2); // provide state pose and path far enough to enable, with data to pass condition diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 3788f2b8a30..485cc375b09 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -17,7 +17,9 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_mppi_controller/tools/noise_generator.hpp" +#include "nav2_mppi_controller/tools/parameters_handler.hpp" #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/models/control_sequence.hpp" @@ -42,13 +44,21 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) settings.batch_size = 100; settings.time_steps = 25; - generator.initialize(settings, false); + auto node = std::make_shared("node"); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); + ParametersHandler handler(node); + + generator.initialize(settings, false, "test_name", &handler); + generator.reset(settings, false); generator.shutdown(); } TEST(NoiseGeneratorTest, NoiseGeneratorMain) { // Tests shuts down internal thread cleanly + auto node = std::make_shared("node"); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); + ParametersHandler handler(node); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; @@ -70,7 +80,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) state.reset(settings.batch_size, settings.time_steps); // Request an update with no noise yet generated, should result in identical outputs - generator.initialize(settings, false); + generator.initialize(settings, false, "test_name", &handler); generator.reset(settings, false); // sets initial sizing and zeros out noises generator.setNoisedControls(state, control_sequence); EXPECT_EQ(state.cvx(0), 0); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 7c7f8a76914..7f54a946eba 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -389,7 +389,7 @@ TEST(OptimizerTests, shiftControlSequenceTests) // Test shiftControlSequence by setting the 2nd value to something unique to neighbors auto & sequence = optimizer_tester.grabControlSequence(); - sequence.reset({100}); + sequence.reset(100); sequence.vx(0) = 9999; sequence.vx(1) = 6; sequence.vx(2) = 888; diff --git a/nav2_mppi_controller/test/utils/factory.hpp b/nav2_mppi_controller/test/utils/factory.hpp index 29404651845..804cecb1519 100644 --- a/nav2_mppi_controller/test/utils/factory.hpp +++ b/nav2_mppi_controller/test/utils/factory.hpp @@ -36,7 +36,9 @@ namespace detail { -auto setHeader(auto && msg, auto node, std::string frame) + +template +void setHeader(TMessage && msg, TNode node, std::string frame) { auto time = node->get_clock()->now(); msg.header.frame_id = frame; @@ -145,9 +147,10 @@ getDummyNode(rclcpp::NodeOptions options, std::string node_name = std::string("d return node; } +template std::shared_ptr getDummyOptimizer( - auto node, auto costmap_ros, - auto * params_handler) + TNode node, TCostMap costmap_ros, + TParamHandler * params_handler) { std::shared_ptr optimizer = std::make_shared(); std::weak_ptr weak_ptr_node{node}; @@ -157,9 +160,10 @@ std::shared_ptr getDummyOptimizer( return optimizer; } +template mppi::PathHandler getDummyPathHandler( - auto node, auto costmap_ros, auto tf_buffer, - auto * params_handler) + TNode node, TCostMap costmap_ros, TFBuffer tf_buffer, + TParamHandler * params_handler) { mppi::PathHandler path_handler; std::weak_ptr weak_ptr_node{node}; @@ -169,9 +173,10 @@ mppi::PathHandler getDummyPathHandler( return path_handler; } +template std::shared_ptr getDummyController( - auto node, auto tf_buffer, - auto costmap_ros) + TNode node, TFBuffer tf_buffer, + TCostMap costmap_ros) { auto controller = std::make_shared(); std::weak_ptr weak_ptr_node{node}; @@ -187,8 +192,9 @@ auto getDummyTwist() return twist; } +template geometry_msgs::msg::PoseStamped -getDummyPointStamped(auto & node, std::string frame = std::string("odom")) +getDummyPointStamped(TNode & node, std::string frame = std::string("odom")) { geometry_msgs::msg::PoseStamped point; detail::setHeader(point, node, frame); @@ -196,7 +202,8 @@ getDummyPointStamped(auto & node, std::string frame = std::string("odom")) return point; } -geometry_msgs::msg::PoseStamped getDummyPointStamped(auto & node, TestPose pose) +template +geometry_msgs::msg::PoseStamped getDummyPointStamped(TNode & node, TestPose pose) { geometry_msgs::msg::PoseStamped point = getDummyPointStamped(node); point.pose.position.x = pose.x; @@ -205,14 +212,16 @@ geometry_msgs::msg::PoseStamped getDummyPointStamped(auto & node, TestPose pose) return point; } -nav_msgs::msg::Path getDummyPath(auto node, std::string frame = std::string("odom")) +template +nav_msgs::msg::Path getDummyPath(TNode node, std::string frame = std::string("odom")) { nav_msgs::msg::Path path; detail::setHeader(path, node, frame); return path; } -auto getDummyPath(size_t points_count, auto node) +template +auto getDummyPath(size_t points_count, TNode node) { auto path = getDummyPath(node); @@ -223,7 +232,8 @@ auto getDummyPath(size_t points_count, auto node) return path; } -nav_msgs::msg::Path getIncrementalDummyPath(auto node, TestPathSettings s) +template +nav_msgs::msg::Path getIncrementalDummyPath(TNode node, TestPathSettings s) { auto path = getDummyPath(node); diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp index 4c59089ef8e..0093492706f 100644 --- a/nav2_mppi_controller/test/utils/utils.hpp +++ b/nav2_mppi_controller/test/utils/utils.hpp @@ -31,7 +31,8 @@ using namespace std::chrono_literals; // NOLINT -void waitSome(const std::chrono::nanoseconds & duration, auto & node) +template +void waitSome(const std::chrono::nanoseconds & duration, TNode & node) { rclcpp::Time start_time = node->now(); while (rclcpp::ok() && node->now() - start_time <= rclcpp::Duration(duration)) { @@ -86,8 +87,9 @@ void printMap(const nav2_costmap_2d::Costmap2D & costmap) * @param trajectory trajectory container (xt::tensor) to be printed. * @param goal_point goal point to be printed. */ +template void printMapWithTrajectoryAndGoal( - nav2_costmap_2d::Costmap2D & costmap, const auto & trajectory, + nav2_costmap_2d::Costmap2D & costmap, const TTrajectory & trajectory, const geometry_msgs::msg::PoseStamped & goal) { const unsigned int trajectory_cost = 1; @@ -174,7 +176,8 @@ void addObstacle(nav2_costmap_2d::Costmap2D * costmap, TestObstaclesSettings s) * @return true - if the trajectory crosses an obstacle on the map, false - if * not */ -bool inCollision(const auto & trajectory, const nav2_costmap_2d::Costmap2D & costmap) +template +bool inCollision(const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap) { unsigned int point_mx = 0; unsigned int point_my = 0; @@ -198,8 +201,9 @@ unsigned char getCost(const nav2_costmap_2d::Costmap2D & costmap, double x, doub return costmap.getCost(point_mx, point_my); } +template bool isGoalReached( - const auto & trajectory, const nav2_costmap_2d::Costmap2D & costmap, + const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap, const geometry_msgs::msg::PoseStamped & goal) { unsigned int trajectory_j = 0; diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index ef5f2a3d590..edeb804b836 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -329,7 +329,7 @@ TEST(UtilsTests, SmootherTest) noisey_sequence.vy = 0.0 * xt::ones({30}); noisey_sequence.wz = 0.3 * xt::ones({30}); - // Make the sequence noisey + // Make the sequence noisy auto noises = xt::random::randn({30}, 0.0, 0.2); noisey_sequence.vx += noises; noisey_sequence.vy += noises; @@ -367,7 +367,7 @@ TEST(UtilsTests, SmootherTest) EXPECT_NEAR(history[3].wz, 0.23, 0.02); // Check that path is smoother - float smoothed_val, original_val; + float smoothed_val{0}, original_val{0}; for (unsigned int i = 1; i != noisey_sequence.vx.shape(0) - 1; i++) { smoothed_val += fabs(noisey_sequence.vx(i) - 0.2); smoothed_val += fabs(noisey_sequence.vy(i) - 0.0); diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 53d72760364..c65119698a2 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -228,8 +228,6 @@ NavfnPlanner::makePlan( plan.header.stamp = clock_->now(); plan.header.frame_id = global_frame_; - // TODO(orduno): add checks for start and goal reference frame -- should be in global frame - double wx = start.position.x; double wy = start.position.y; @@ -266,9 +264,6 @@ NavfnPlanner::makePlan( map_goal[0] = mx; map_goal[1] = my; - // TODO(orduno): Explain why we are providing 'map_goal' to setStart(). - // Same for setGoal, seems reversed. Computing backwards? - planner_->setStart(map_goal); planner_->setGoal(map_start); if (use_astar_) { diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 2add3d91fdc..84edc6e4df3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -110,7 +110,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) planner_ids_[i].c_str(), planner_types_[i].c_str()); planner->configure(node, planner_ids_[i], tf_, costmap_ros_); planners_.insert({planner_ids_[i], planner}); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); @@ -174,7 +174,10 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) plan_publisher_->on_activate(); action_server_pose_->activate(); action_server_poses_->activate(); - costmap_ros_->activate(); + const auto costmap_ros_state = costmap_ros_->activate(); + if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + return nav2_util::CallbackReturn::FAILURE; + } PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index fc1d7389993..350b1926da4 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -90,7 +90,6 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | -| `use_interpolation` | Enables interpolation between poses on the path for lookahead point selection. Helps sparse paths to avoid inducing discontinuous commanded velocities. Set this to false for a potential performance boost, at the expense of smooth control. | Example fully-described XML with default parameter values: @@ -138,7 +137,6 @@ controller_server: rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 max_robot_pose_search_dist: 10.0 - use_interpolation: false cost_scaling_dist: 0.3 cost_scaling_gain: 1.0 inflation_cost_scaling_factor: 3.0 diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 8962003d446..39f5de6fa0c 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -56,7 +56,6 @@ struct Parameters double rotate_to_heading_min_angle; bool allow_reversing; double max_robot_pose_search_dist; - bool use_interpolation; bool use_collision_detection; double transform_tolerance; }; diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 92187376632..016a626c288 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -90,9 +90,6 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x / 2.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_interpolation", - rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); @@ -162,9 +159,6 @@ ParameterHandler::ParameterHandler( params_.max_robot_pose_search_dist = std::numeric_limits::max(); } - node->get_parameter( - plugin_name_ + ".use_interpolation", - params_.use_interpolation); node->get_parameter( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index c40852c445e..9434b105f4c 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -157,6 +157,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity { std::lock_guard lock_reinit(param_handler_->getMutex()); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + // Update for the current goal checker's state geometry_msgs::msg::Pose pose_tolerance; geometry_msgs::msg::Twist vel_tolerance; @@ -322,7 +325,7 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin // If the no pose is not far enough, take the last pose if (goal_pose_it == transformed_plan.poses.end()) { goal_pose_it = std::prev(transformed_plan.poses.end()); - } else if (params_->use_interpolation && goal_pose_it != transformed_plan.poses.begin()) { + } else if (goal_pose_it != transformed_plan.poses.begin()) { // Find the point on the line segment between the two poses // that is exactly the lookahead distance away from the robot pose (the origin) // This can be found with a closed form for the intersection of a segment and a circle @@ -416,12 +419,27 @@ double RegulatedPurePursuitController::findVelocitySignChange( /* Checking for the existance of cusp, in the path, using the dot product and determine it's distance from the robot. If there is no cusp in the path, then just determine the distance to the goal location. */ - if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) { + const double dot_prod = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_prod < 0.0) { // returning the distance if there is a cusp // The transformed path is in the robots frame, so robot is at the origin return hypot( transformed_plan.poses[pose_id].pose.position.x, transformed_plan.poses[pose_id].pose.position.y); + } else if ( + (hypot(oa_x, oa_y) == 0.0 && + transformed_plan.poses[pose_id - 1].pose.orientation != + transformed_plan.poses[pose_id].pose.orientation) + || + (hypot(ab_x, ab_y) == 0.0 && + transformed_plan.poses[pose_id].pose.orientation != + transformed_plan.poses[pose_id + 1].pose.orientation)) + { + // returning the distance since the points overlap + // but are not simply duplicate points (e.g. in place rotation) + return hypot( + transformed_plan.poses[pose_id].pose.position.x, + transformed_plan.poses[pose_id].pose.position.y); } } diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 09892755d93..1557dad2d2d 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -379,26 +379,7 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) auto pt = ctrl->getLookAheadPointWrapper(dist, path); EXPECT_EQ(pt.pose.position.x, 1.0); - // test getting next closest point without interpolation - node->set_parameter( - rclcpp::Parameter( - name + ".use_interpolation", - rclcpp::ParameterValue(false))); - ctrl->configure(node, name, tf, costmap); - dist = 3.8; - pt = ctrl->getLookAheadPointWrapper(dist, path); - EXPECT_EQ(pt.pose.position.x, 4.0); - - // test end of path - dist = 100.0; - pt = ctrl->getLookAheadPointWrapper(dist, path); - EXPECT_EQ(pt.pose.position.x, 9.0); - // test interpolation - node->set_parameter( - rclcpp::Parameter( - name + ".use_interpolation", - rclcpp::ParameterValue(true))); ctrl->configure(node, name, tf, costmap); dist = 3.8; pt = ctrl->getLookAheadPointWrapper(dist, path); diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 7d32283db7c..afd32381459 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -140,6 +140,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands nav2_core::GoalChecker * goal_checker) { if (path_updated_) { + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + std::lock_guard lock_reinit(mutex_); try { geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt()); diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index e1bc30f80ec..1c172436e68 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -109,7 +109,7 @@ TEST(RotationShimControllerTest, lifecycleTransitions) ctrl->activate(); - ctrl->setSpeedLimit(50.0, 2.0); + ctrl->setSpeedLimit(50.0, true); ctrl->deactivate(); ctrl->cleanup(); diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py index cd37de2bdff..3324b0ee4e1 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -18,7 +18,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -39,24 +43,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -64,28 +70,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='example_assisted_teleop', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py index e933ed421cd..b4fe9b586fd 100644 --- a/nav2_simple_commander/launch/follow_path_example_launch.py +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='example_follow_path', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py index 43456f6398b..285f26099ea 100644 --- a/nav2_simple_commander/launch/inspection_demo_launch.py +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='demo_inspection', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py index 0c9fedaad99..e8b0fefba0b 100644 --- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='example_nav_through_poses', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py index d4259494092..29aeddebd16 100644 --- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='example_nav_to_pose', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py index 5fce5701069..8505b6bbc32 100644 --- a/nav2_simple_commander/launch/picking_demo_launch.py +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='demo_picking', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py index d7b419711eb..638550c44e4 100644 --- a/nav2_simple_commander/launch/recoveries_example_launch.py +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='demo_recoveries', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py index c04de67d451..a958a75f734 100644 --- a/nav2_simple_commander/launch/security_demo_launch.py +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='demo_security', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py index 343017c0e47..f164035e3b9 100644 --- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -17,7 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -38,24 +42,26 @@ def generate_launch_description(): # Declare the launch arguments declare_use_rviz_cmd = DeclareLaunchArgument( - 'use_rviz', - default_value='True', - description='Whether to start RVIZ') + 'use_rviz', default_value='True', description='Whether to start RVIZ' + ) declare_simulator_cmd = DeclareLaunchArgument( - 'headless', - default_value='False', - description='Whether to execute gzclient)') + 'headless', default_value='False', description='Whether to execute gzclient)' + ) # start the simulation start_gazebo_server_cmd = ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) start_gazebo_client_cmd = ExecuteProcess( condition=IfCondition(PythonExpression(['not ', headless])), cmd=['gzclient'], - cwd=[warehouse_dir], output='screen') + cwd=[warehouse_dir], + output='screen', + ) urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') start_robot_state_publisher_cmd = Node( @@ -63,28 +69,33 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - arguments=[urdf]) + arguments=[urdf], + ) # start the visualization rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), condition=IfCondition(use_rviz), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) # start navigation bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={'map': map_yaml_file}.items(), + ) # start the demo autonomy task demo_cmd = Node( package='nav2_simple_commander', executable='example_waypoint_follower', emulate_tty=True, - output='screen') + output='screen', + ) ld = LaunchDescription() ld.add_action(declare_use_rviz_cmd) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 429e11b68f8..5f6d04769a0 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -41,7 +41,8 @@ def main(): [3.661, -4.121], [5.431, -4.121], [3.661, -5.850], - [5.431, -5.800]] + [5.431, -5.800], + ] # Set our demo's initial pose initial_pose = PoseStamped() @@ -76,8 +77,12 @@ def main(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Executing current waypoint: ' + - str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points))) + print( + 'Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + + '/' + + str(len(inspection_points)) + ) result = navigator.getResult() if result == TaskResult.SUCCEEDED: diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 6a7a177acb1..48c0551c2cd 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -25,14 +25,16 @@ 'shelf_A': [-3.829, -7.604], 'shelf_B': [-3.791, -3.287], 'shelf_C': [-3.791, 1.254], - 'shelf_D': [-3.24, 5.861]} + 'shelf_D': [-3.24, 5.861], +} # Shipping destination for picked products shipping_destinations = { 'recycling': [-0.205, 7.403], 'pallet_jack7': [-0.073, -8.497], 'conveyer_432': [6.217, 2.153], - 'frieght_bay_3': [-6.349, 9.147]} + 'frieght_bay_3': [-6.349, 9.147], +} """ Basic item picking demo. In this demonstration, the expectation @@ -45,7 +47,7 @@ def main(): # Recieved virtual request for picking item at Shelf A and bring to # worker at the pallet jack 7 for shipping. This request would - # contain the shelf ID ("shelf_A") and shipping destination ("pallet_jack7") + # contain the shelf ID ('shelf_A') and shipping destination ('pallet_jack7') #################### request_item_location = 'shelf_C' request_destination = 'pallet_jack7' @@ -86,26 +88,43 @@ def main(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated time of arrival at ' + request_item_location + - ' for worker: ' + '{0:.0f}'.format( - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) - + ' seconds.') + print( + 'Estimated time of arrival at ' + + request_item_location + + ' for worker: ' + + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + / 1e9 + ) + + ' seconds.' + ) result = navigator.getResult() if result == TaskResult.SUCCEEDED: - print('Got product from ' + request_item_location + - '! Bringing product to shipping destination (' + request_destination + ')...') + print( + 'Got product from ' + + request_item_location + + '! Bringing product to shipping destination (' + + request_destination + + ')...' + ) shipping_destination = PoseStamped() shipping_destination.header.frame_id = 'map' shipping_destination.header.stamp = navigator.get_clock().now().to_msg() - shipping_destination.pose.position.x = shipping_destinations[request_destination][0] - shipping_destination.pose.position.y = shipping_destinations[request_destination][1] + shipping_destination.pose.position.x = shipping_destinations[ + request_destination + ][0] + shipping_destination.pose.position.y = shipping_destinations[ + request_destination + ][1] shipping_destination.pose.orientation.z = 1.0 shipping_destination.pose.orientation.w = 0.0 navigator.goToPose(shipping_destination) elif result == TaskResult.CANCELED: - print(f'Task at {request_item_location} was canceled. Returning to staging point...') + print( + f'Task at {request_item_location} was canceled. Returning to staging point...' + ) initial_pose.header.stamp = navigator.get_clock().now().to_msg() navigator.goToPose(initial_pose) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index f727cc8d722..24cc05feff2 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -43,7 +43,8 @@ def main(): [-3.665, -9.427], [-3.665, -4.303], [-3.665, 2.330], - [-3.665, 9.283]] + [-3.665, 9.283], + ] # Set our demo's initial pose initial_pose = PoseStamped() @@ -79,12 +80,19 @@ def main(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated time to complete current route: ' + '{0:.0f}'.format( - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) - + ' seconds.') + print( + 'Estimated time to complete current route: ' + + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + / 1e9 + ) + + ' seconds.' + ) # Some failure mode, must stop since the robot is clearly stuck - if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0): + if Duration.from_msg(feedback.navigation_time) > Duration( + seconds=180.0 + ): print('Navigation has exceeded timeout of 180s, canceling request.') navigator.cancelTask() diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py index 963dacf9428..104083f34a7 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -68,10 +68,12 @@ def main(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated distance remaining to goal position: ' + - '{0:.3f}'.format(feedback.distance_to_goal) + - '\nCurrent speed of the robot: ' + - '{0:.3f}'.format(feedback.speed)) + print( + 'Estimated distance remaining to goal position: ' + + '{0:.3f}'.format(feedback.distance_to_goal) + + '\nCurrent speed of the robot: ' + + '{0:.3f}'.format(feedback.speed) + ) # Do something depending on the return code result = navigator.getResult() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py index 58c47c75e1d..66023ec11a5 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -100,9 +100,14 @@ def main(): i = i + 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated time of arrival: ' + '{0:.0f}'.format( - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) - + ' seconds.') + print( + 'Estimated time of arrival: ' + + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + / 1e9 + ) + + ' seconds.' + ) # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py index 58dee813c9b..d7fb850d12b 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -79,9 +79,14 @@ def main(): i = i + 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Estimated time of arrival: ' + '{0:.0f}'.format( - Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) - + ' seconds.') + print( + 'Estimated time of arrival: ' + + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + / 1e9 + ) + + ' seconds.' + ) # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py index 9d02341a669..30444658518 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -101,8 +101,12 @@ def main(): i = i + 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: - print('Executing current waypoint: ' + - str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses))) + print( + 'Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + + '/' + + str(len(goal_poses)) + ) now = navigator.get_clock().now() # Some navigation timeout to demo cancellation diff --git a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py index 2117e21a55b..5cf2f81794a 100644 --- a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py +++ b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py @@ -34,7 +34,7 @@ FREE_SPACE = 0 -class FootprintCollisionChecker(): +class FootprintCollisionChecker: """ FootprintCollisionChecker. @@ -65,8 +65,7 @@ def footprintCost(self, footprint: Polygon): x1 = 0.0 y1 = 0.0 - x0, y0 = self.worldToMapValidated( - footprint.points[0].x, footprint.points[0].y) + x0, y0 = self.worldToMapValidated(footprint.points[0].x, footprint.points[0].y) if x0 is None or y0 is None: return LETHAL_OBSTACLE @@ -76,13 +75,13 @@ def footprintCost(self, footprint: Polygon): for i in range(len(footprint.points) - 1): x1, y1 = self.worldToMapValidated( - footprint.points[i + 1].x, footprint.points[i + 1].y) + footprint.points[i + 1].x, footprint.points[i + 1].y + ) if x1 is None or y1 is None: return LETHAL_OBSTACLE - footprint_cost = max( - float(self.lineCost(x0, x1, y0, y1)), footprint_cost) + footprint_cost = max(float(self.lineCost(x0, x1, y0, y1)), footprint_cost) x0 = x1 y0 = y1 @@ -115,7 +114,8 @@ def lineCost(self, x0, x1, y0, y1, step_size=0.5): while line_iterator.isValid(): point_cost = self.pointCost( - int(line_iterator.getX()), int(line_iterator.getY())) + int(line_iterator.getX()), int(line_iterator.getY()) + ) if point_cost == LETHAL_OBSTACLE: return point_cost @@ -146,7 +146,8 @@ def worldToMapValidated(self, wx: float, wy: float): """ if self.costmap_ is None: raise ValueError( - 'Costmap not specified, use setCostmap to specify the costmap first') + 'Costmap not specified, use setCostmap to specify the costmap first' + ) return self.costmap_.worldToMapValidated(wx, wy) def pointCost(self, x: int, y: int): @@ -165,7 +166,8 @@ def pointCost(self, x: int, y: int): """ if self.costmap_ is None: raise ValueError( - 'Costmap not specified, use setCostmap to specify the costmap first') + 'Costmap not specified, use setCostmap to specify the costmap first' + ) return self.costmap_.getCostXY(x, y) def setCostmap(self, costmap: PyCostmap2D): @@ -207,12 +209,12 @@ def footprintCostAtPose(self, x: float, y: float, theta: float, footprint: Polyg for i in range(len(footprint.points)): new_pt = Point32() - new_pt.x = x + \ - (footprint.points[i].x * cos_th - - footprint.points[i].y * sin_th) - new_pt.y = y + \ - (footprint.points[i].x * sin_th + - footprint.points[i].y * cos_th) + new_pt.x = x + ( + footprint.points[i].x * cos_th - footprint.points[i].y * sin_th + ) + new_pt.y = y + ( + footprint.points[i].x * sin_th + footprint.points[i].y * cos_th + ) oriented_footprint.points.append(new_pt) return self.footprintCost(oriented_footprint) diff --git a/nav2_simple_commander/nav2_simple_commander/line_iterator.py b/nav2_simple_commander/nav2_simple_commander/line_iterator.py index 7fe50a11ec5..9f6c092570a 100644 --- a/nav2_simple_commander/nav2_simple_commander/line_iterator.py +++ b/nav2_simple_commander/nav2_simple_commander/line_iterator.py @@ -24,7 +24,7 @@ from cmath import sqrt -class LineIterator(): +class LineIterator: """ LineIterator. @@ -77,18 +77,19 @@ def __init__(self, x0, y0, x1, y1, step_size=1.0): if x1 != x0 and y1 != y0: self.valid_ = True - self.m_ = (y1-y0)/(x1-x0) - self.b_ = y1 - (self.m_*x1) + self.m_ = (y1 - y0) / (x1 - x0) + self.b_ = y1 - (self.m_ * x1) elif x1 == x0 and y1 != y0: self.valid_ = True elif y1 == y1 and x1 != x0: self.valid_ = True - self.m_ = (y1-y0)/(x1-x0) - self.b_ = y1 - (self.m_*x1) + self.m_ = (y1 - y0) / (x1 - x0) + self.b_ = y1 - (self.m_ * x1) else: self.valid_ = False raise ValueError( - 'Line has zero length (All 4 points have same coordinates)') + 'Line has zero length (All 4 points have same coordinates)' + ) def isValid(self): """Check if line is valid.""" @@ -98,29 +99,33 @@ def advance(self): """Advance to the next point in the line.""" if self.x1_ > self.x0_: if self.x_ < self.x1_: - self.x_ = round(self.clamp( - self.x_ + self.step_size_, self.x0_, self.x1_), 5) + self.x_ = round( + self.clamp(self.x_ + self.step_size_, self.x0_, self.x1_), 5 + ) self.y_ = round(self.m_ * self.x_ + self.b_, 5) else: self.valid_ = False elif self.x1_ < self.x0_: if self.x_ > self.x1_: - self.x_ = round(self.clamp( - self.x_ - self.step_size_, self.x1_, self.x0_), 5) + self.x_ = round( + self.clamp(self.x_ - self.step_size_, self.x1_, self.x0_), 5 + ) self.y_ = round(self.m_ * self.x_ + self.b_, 5) else: self.valid_ = False else: if self.y1_ > self.y0_: if self.y_ < self.y1_: - self.y_ = round(self.clamp( - self.y_ + self.step_size_, self.y0_, self.y1_), 5) + self.y_ = round( + self.clamp(self.y_ + self.step_size_, self.y0_, self.y1_), 5 + ) else: self.valid_ = False elif self.y1_ < self.y0_: if self.y_ > self.y1_: - self.y_ = round(self.clamp( - self.y_ - self.step_size_, self.y1_, self.y0_), 5) + self.y_ = round( + self.clamp(self.y_ - self.step_size_, self.y1_, self.y0_), 5 + ) else: self.valid_ = False else: diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 76a6c3d79be..a8ded664cd5 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -25,8 +25,13 @@ from lifecycle_msgs.srv import GetState from nav2_msgs.action import AssistedTeleop, BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import FollowPath, FollowWaypoints, FollowGPSWaypoints, \ - NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import ( + FollowGPSWaypoints, + FollowPath, + FollowWaypoints, + NavigateThroughPoses, + NavigateToPose, +) from nav2_msgs.action import SmoothPath from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes @@ -46,7 +51,6 @@ class TaskResult(Enum): class BasicNavigator(Node): - def __init__(self, node_name='basic_navigator', namespace=''): super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() @@ -57,42 +61,58 @@ def __init__(self, node_name='basic_navigator', namespace=''): self.status = None amcl_pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) self.initial_pose_received = False - self.nav_through_poses_client = ActionClient(self, - NavigateThroughPoses, - 'navigate_through_poses') + self.nav_through_poses_client = ActionClient( + self, NavigateThroughPoses, 'navigate_through_poses' + ) self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') - self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') - self.follow_gps_waypoints_client = ActionClient(self, FollowGPSWaypoints, - 'follow_gps_waypoints') + self.follow_waypoints_client = ActionClient( + self, FollowWaypoints, 'follow_waypoints' + ) + self.follow_gps_waypoints_client = ActionClient( + self, FollowGPSWaypoints, 'follow_gps_waypoints' + ) self.follow_path_client = ActionClient(self, FollowPath, 'follow_path') - self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, - 'compute_path_to_pose') - self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses, - 'compute_path_through_poses') + self.compute_path_to_pose_client = ActionClient( + self, ComputePathToPose, 'compute_path_to_pose' + ) + self.compute_path_through_poses_client = ActionClient( + self, ComputePathThroughPoses, 'compute_path_through_poses' + ) self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') - self.assisted_teleop_client = ActionClient(self, AssistedTeleop, 'assisted_teleop') - self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', - self._amclPoseCallback, - amcl_pose_qos) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', - 10) + self.assisted_teleop_client = ActionClient( + self, AssistedTeleop, 'assisted_teleop' + ) + self.localization_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, + 'amcl_pose', + self._amclPoseCallback, + amcl_pose_qos, + ) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) self.change_maps_srv = self.create_client(LoadMap, 'map_server/load_map') self.clear_costmap_global_srv = self.create_client( - ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap') + ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap' + ) self.clear_costmap_local_srv = self.create_client( - ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap') - self.get_costmap_global_srv = self.create_client(GetCostmap, 'global_costmap/get_costmap') - self.get_costmap_local_srv = self.create_client(GetCostmap, 'local_costmap/get_costmap') + ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap' + ) + self.get_costmap_global_srv = self.create_client( + GetCostmap, 'global_costmap/get_costmap' + ) + self.get_costmap_local_srv = self.create_client( + GetCostmap, 'local_costmap/get_costmap' + ) def destroyNode(self): self.destroy_node() @@ -126,8 +146,9 @@ def goThroughPoses(self, poses, behavior_tree=''): goal_msg.behavior_tree = behavior_tree self.info(f'Navigating with {len(goal_msg.poses)} goals....') - send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.nav_through_poses_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -148,16 +169,27 @@ def goToPose(self, pose, behavior_tree=''): goal_msg.pose = pose goal_msg.behavior_tree = behavior_tree - self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' + - str(pose.pose.position.y) + '...') - send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg, - self._feedbackCallback) + self.info( + 'Navigating to goal: ' + + str(pose.pose.position.x) + + ' ' + + str(pose.pose.position.y) + + '...' + ) + send_goal_future = self.nav_to_pose_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() if not self.goal_handle.accepted: - self.error('Goal to ' + str(pose.pose.position.x) + ' ' + - str(pose.pose.position.y) + ' was rejected!') + self.error( + 'Goal to ' + + str(pose.pose.position.x) + + ' ' + + str(pose.pose.position.y) + + ' was rejected!' + ) return False self.result_future = self.goal_handle.get_result_async() @@ -173,8 +205,9 @@ def followWaypoints(self, poses): goal_msg.poses = poses self.info(f'Following {len(goal_msg.poses)} goals....') - send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.follow_waypoints_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -195,13 +228,16 @@ def followGpsWaypoints(self, gps_poses): goal_msg.gps_poses = gps_poses self.info(f'Following {len(goal_msg.gps_poses)} gps goals....') - send_goal_future = self.follow_gps_waypoints_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.follow_gps_waypoints_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() if not self.goal_handle.accepted: - self.error(f'Following {len(gps_poses)} gps waypoints request was rejected!') + self.error( + f'Following {len(gps_poses)} gps waypoints request was rejected!' + ) return False self.result_future = self.goal_handle.get_result_async() @@ -216,7 +252,9 @@ def spin(self, spin_dist=1.57, time_allowance=10): goal_msg.time_allowance = Duration(sec=time_allowance) self.info(f'Spinning to angle {goal_msg.target_yaw}....') - send_goal_future = self.spin_client.send_goal_async(goal_msg, self._feedbackCallback) + send_goal_future = self.spin_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -237,7 +275,9 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): goal_msg.time_allowance = Duration(sec=time_allowance) self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....') - send_goal_future = self.backup_client.send_goal_async(goal_msg, self._feedbackCallback) + send_goal_future = self.backup_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -256,8 +296,9 @@ def assistedTeleop(self, time_allowance=30): goal_msg.time_allowance = Duration(sec=time_allowance) self.info("Running 'assisted_teleop'....") - send_goal_future = \ - self.assisted_teleop_client.send_goal_async(goal_msg, self._feedbackCallback) + send_goal_future = self.assisted_teleop_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -280,8 +321,9 @@ def followPath(self, path, controller_id='', goal_checker_id=''): goal_msg.goal_checker_id = goal_checker_id self.info('Executing path...') - send_goal_future = self.follow_path_client.send_goal_async(goal_msg, - self._feedbackCallback) + send_goal_future = self.follow_path_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -335,7 +377,7 @@ def getResult(self): def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" - if localizer != "robot_localization": # non-lifecycle node + if localizer != 'robot_localization': # non-lifecycle node self._waitForNodeToActivate(localizer) if localizer == 'amcl': self._waitForInitialPose() @@ -394,8 +436,12 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) Internal implementation to get the full result, not just the path. """ self.debug("Waiting for 'ComputePathThroughPoses' action server") - while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): - self.info("'ComputePathThroughPoses' action server not available, waiting...") + while not self.compute_path_through_poses_client.wait_for_server( + timeout_sec=1.0 + ): + self.info( + "'ComputePathThroughPoses' action server not available, waiting..." + ) goal_msg = ComputePathThroughPoses.Goal() goal_msg.start = start @@ -404,7 +450,9 @@ def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False) goal_msg.use_start = use_start self.info('Getting path...') - send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) + send_goal_future = self.compute_path_through_poses_client.send_goal_async( + goal_msg + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() @@ -431,7 +479,9 @@ def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): else: return rtn.path - def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + def _smoothPathImpl( + self, path, smoother_id='', max_duration=2.0, check_for_collision=False + ): """ Send a `SmoothPath` action request. @@ -462,10 +512,11 @@ def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_coll return self.result_future.result().result - def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + def smoothPath( + self, path, smoother_id='', max_duration=2.0, check_for_collision=False + ): """Send a `SmoothPath` action request.""" - rtn = self._smoothPathImpl( - path, smoother_id, max_duration, check_for_collision) + rtn = self._smoothPathImpl(path, smoother_id, max_duration, check_for_collision) if self.status != GoalStatus.STATUS_SUCCEEDED: self.warn(f'Getting path failed with status code: {self.status}') diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index c7b12a3fcfa..4b49f89a450 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -11,8 +11,7 @@ version='1.0.0', packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name), glob('launch/*')), ], @@ -25,15 +24,15 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main', - 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', - 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', - 'example_follow_path = nav2_simple_commander.example_follow_path:main', - 'demo_picking = nav2_simple_commander.demo_picking:main', - 'demo_inspection = nav2_simple_commander.demo_inspection:main', - 'demo_security = nav2_simple_commander.demo_security:main', - 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', - 'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main', + 'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main', + 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', + 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', + 'example_follow_path = nav2_simple_commander.example_follow_path:main', + 'demo_picking = nav2_simple_commander.demo_picking:main', + 'demo_inspection = nav2_simple_commander.demo_inspection:main', + 'demo_security = nav2_simple_commander.demo_security:main', + 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', + 'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main', ], }, ) diff --git a/nav2_simple_commander/test/test_flake8.py b/nav2_simple_commander/test/test_flake8.py index 27ee1078ff0..26030113cce 100644 --- a/nav2_simple_commander/test/test_flake8.py +++ b/nav2_simple_commander/test/test_flake8.py @@ -20,6 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len( + errors + ) + '\n'.join(errors) diff --git a/nav2_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py index 8ffc4b653b9..070a7bc6e4b 100644 --- a/nav2_simple_commander/test/test_footprint_collision_checker.py +++ b/nav2_simple_commander/test/test_footprint_collision_checker.py @@ -23,7 +23,6 @@ class TestFootprintCollisionChecker(unittest.TestCase): - def test_no_costmap(self): # Test if a type error raised when costmap is not specified yet fcc_ = FootprintCollisionChecker() diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py index 87c348ea702..1360c183e7d 100644 --- a/nav2_simple_commander/test/test_line_iterator.py +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -19,7 +19,6 @@ class TestLineIterator(unittest.TestCase): - def test_type_error(self): # Test if a type error raised when passing invalid arguements types self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') @@ -58,7 +57,7 @@ def test_straight_line(self): i = 0 while lt.isValid(): self.assertEqual(lt.getX(), lt.getX0() + i) - self.assertEqual(lt.getY(), lt.getY0() + (i*2)) + self.assertEqual(lt.getY(), lt.getY0() + (i * 2)) lt.advance() i += 1 @@ -67,7 +66,7 @@ def test_straight_line(self): i = 0 while lt.isValid(): self.assertEqual(lt.getX(), lt.getX0() + i) - self.assertEqual(lt.getY(), lt.getY0() + (-i*2)) + self.assertEqual(lt.getY(), lt.getY0() + (-i * 2)) lt.advance() i += 1 diff --git a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h index 5d3812a3a35..4d3977937bf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h +++ b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h @@ -111,7 +111,7 @@ static Counts& counts() { # error Unsupported bitness #endif -// endianess +// endianness #ifdef _MSC_VER # define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() 1 # define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() 0 @@ -2132,7 +2132,7 @@ class Table return maxElements * MaxLoadFactor100 / 100; } - // we might be a bit inprecise, but since maxElements is quite large that doesn't matter + // we might be a bit imprecise, but since maxElements is quite large that doesn't matter return (maxElements / 100) * MaxLoadFactor100; } diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py index e70c235e83d..d19ef987ece 100644 --- a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py +++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py @@ -40,24 +40,28 @@ def handle_arg_parsing(): An object containing all parsed arguments """ - parser = argparse.ArgumentParser(description='Generate motionprimitives ' - "for Nav2's State " - 'Lattice Planner') - parser.add_argument('--config', - type=Path, - default='./config.json', - help='The config file containing the ' - 'parameters to be used') - parser.add_argument('--output', - type=Path, - default='./output.json', - help='The output file containing the ' - 'trajectory data') - parser.add_argument('--visualizations', - type=Path, - default='./visualizations', - help='The output folder where the ' - 'visualizations of the trajectories will be saved') + parser = argparse.ArgumentParser( + description="Generate motionprimitives for Nav2's State Lattice Planner" + ) + parser.add_argument( + '--config', + type=Path, + default='./config.json', + help='The config file containing the ' 'parameters to be used', + ) + parser.add_argument( + '--output', + type=Path, + default='./output.json', + help='The output file containing the ' 'trajectory data', + ) + parser.add_argument( + '--visualizations', + type=Path, + default='./visualizations', + help='The output folder where the ' + 'visualizations of the trajectories will be saved', + ) return parser.parse_args() @@ -130,14 +134,18 @@ def create_header(config: dict, minimal_set_trajectories: dict) -> dict: header_dict['lattice_metadata'][key] = value heading_angles = create_heading_angle_list(minimal_set_trajectories) - adjusted_heading_angles = [angle + 2*np.pi if angle < 0 else angle for angle in heading_angles] + adjusted_heading_angles = [ + angle + 2 * np.pi if angle < 0 else angle for angle in heading_angles + ] header_dict['lattice_metadata']['heading_angles'] = adjusted_heading_angles return header_dict -def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dict) -> None: +def write_to_json( + output_path: Path, minimal_set_trajectories: dict, config: dict +) -> None: """ Write the minimal spanning set to an output file. @@ -156,32 +164,29 @@ def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dic trajectory_start_angles = list(minimal_set_trajectories.keys()) heading_angle_list = create_heading_angle_list(minimal_set_trajectories) - heading_lookup = {angle: idx for idx, angle in - enumerate(heading_angle_list)} + heading_lookup = {angle: idx for idx, angle in enumerate(heading_angle_list)} idx = 0 - for start_angle in sorted(trajectory_start_angles, - key=lambda x: (x < 0, x)): + for start_angle in sorted(trajectory_start_angles, key=lambda x: (x < 0, x)): for trajectory in sorted( - minimal_set_trajectories[start_angle], - key=lambda x: x.parameters.end_angle + minimal_set_trajectories[start_angle], key=lambda x: x.parameters.end_angle ): traj_info = {} traj_info['trajectory_id'] = idx - traj_info['start_angle_index'] = heading_lookup[trajectory.parameters.start_angle] - traj_info['end_angle_index'] = heading_lookup[trajectory.parameters.end_angle] + traj_info['start_angle_index'] = heading_lookup[ + trajectory.parameters.start_angle + ] + traj_info['end_angle_index'] = heading_lookup[ + trajectory.parameters.end_angle + ] traj_info['left_turn'] = bool(trajectory.parameters.left_turn) - traj_info['trajectory_radius'] = \ - trajectory.parameters.turning_radius + traj_info['trajectory_radius'] = trajectory.parameters.turning_radius traj_info['trajectory_length'] = round( trajectory.parameters.total_length, 5 ) - traj_info['arc_length'] = round( - trajectory.parameters.arc_length, - 5 - ) + traj_info['arc_length'] = round(trajectory.parameters.arc_length, 5) traj_info['straight_length'] = round( trajectory.parameters.start_straight_length + trajectory.parameters.end_straight_length, @@ -198,7 +203,9 @@ def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dic json.dump(output_dict, output_file, indent='\t') -def save_visualizations(visualizations_folder: Path, minimal_set_trajectories: dict) -> None: +def save_visualizations( + visualizations_folder: Path, minimal_set_trajectories: dict +) -> None: """ Draw the visualizations for every trajectory and save it as an image. diff --git a/nav2_smac_planner/lattice_primitives/helper.py b/nav2_smac_planner/lattice_primitives/helper.py index 24d3cf91402..e29c7877389 100644 --- a/nav2_smac_planner/lattice_primitives/helper.py +++ b/nav2_smac_planner/lattice_primitives/helper.py @@ -28,11 +28,11 @@ def normalize_angle(angle): The normalized angle in the range [0,2pi) """ - while angle >= 2*np.pi: - angle -= 2*np.pi + while angle >= 2 * np.pi: + angle -= 2 * np.pi while angle < 0: - angle += 2*np.pi + angle += 2 * np.pi return angle @@ -123,5 +123,4 @@ def get_rotation_matrix(angle): A 2x2 matrix representing a 2D rotation by angle """ - return np.array([[np.cos(angle), -np.sin(angle)], - [np.sin(angle), np.cos(angle)]]) + return np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index 3bcf735de80..185cc0f839f 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -57,8 +57,7 @@ def __init__(self, config: dict): self.turning_radius = config['turning_radius'] self.stopping_threshold = config['stopping_threshold'] self.num_of_headings = config['num_of_headings'] - self.headings = \ - self._get_heading_discretization(config['num_of_headings']) + self.headings = self._get_heading_discretization(config['num_of_headings']) self.motion_model = self.MotionModel[config['motion_model'].upper()] @@ -119,7 +118,7 @@ def _get_heading_discretization(self, number_of_headings: int) -> list: A list of headings in radians """ - max_val = int((((number_of_headings + 4) / 4) - 1) / 2) + max_val = int(number_of_headings / 8) outer_edge_x = [] outer_edge_y = [] @@ -567,8 +566,7 @@ def _handle_motion_model(self, spanning_set: dict) -> dict: return omni_spanning_set else: - print('No handling implemented for Motion Model: ' + - f'{self.motion_model}') + print('No handling implemented for Motion Model: ' + f'{self.motion_model}') raise NotImplementedError def _add_in_place_turns(self, spanning_set: dict) -> dict: diff --git a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py index 01476decd0e..30a7a24eb2a 100644 --- a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py @@ -28,11 +28,13 @@ class TestLatticeGenerator(unittest.TestCase): """Contains the unit tests for the TrajectoryGenerator.""" def setUp(self) -> None: - config = {'motion_model': MOTION_MODEL, - 'turning_radius': TURNING_RADIUS, - 'grid_resolution': GRID_RESOLUTION, - 'stopping_threshold': STOPPING_THRESHOLD, - 'num_of_headings': NUM_OF_HEADINGS} + config = { + 'motion_model': MOTION_MODEL, + 'turning_radius': TURNING_RADIUS, + 'grid_resolution': GRID_RESOLUTION, + 'stopping_threshold': STOPPING_THRESHOLD, + 'num_of_headings': NUM_OF_HEADINGS, + } lattice_gen = LatticeGenerator(config) @@ -83,7 +85,7 @@ def test_output_angles_in_correct_range(self): for x, y, angle in output: self.assertGreaterEqual(angle, 0) - self.assertLessEqual(angle, 2*np.pi) + self.assertLessEqual(angle, 2 * np.pi) if __name__ == '__main__': diff --git a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py index 0a994cac490..ea3f51930fa 100644 --- a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py @@ -32,7 +32,8 @@ def test_generate_trajectory_only_arc(self): # Quadrant 1 end_point = np.array([1, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -40,7 +41,8 @@ def test_generate_trajectory_only_arc(self): # Quadrant 2 end_point = np.array([-1, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -48,7 +50,8 @@ def test_generate_trajectory_only_arc(self): # Quadrant 3 end_point = np.array([-1, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -56,7 +59,8 @@ def test_generate_trajectory_only_arc(self): # Quadrant 4 end_point = np.array([1, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -65,7 +69,8 @@ def test_generate_trajectory_only_line(self): # Quadrant 1 end_point = np.array([1, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE) + end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -73,7 +78,8 @@ def test_generate_trajectory_only_line(self): # Quadrant 2 end_point = np.array([-1, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE) + end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -81,7 +87,8 @@ def test_generate_trajectory_only_line(self): # Quadrant 3 end_point = np.array([-1, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE) + end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -89,7 +96,8 @@ def test_generate_trajectory_only_line(self): # Quadrant 4 end_point = np.array([1, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE) + end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -98,7 +106,8 @@ def test_generate_trajectory_line_to_arc(self): # Quadrant 1 end_point = np.array([2, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -106,7 +115,8 @@ def test_generate_trajectory_line_to_arc(self): # Quadrant 2 end_point = np.array([-2, 1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -114,7 +124,8 @@ def test_generate_trajectory_line_to_arc(self): # Quadrant 3 end_point = np.array([-2, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -122,7 +133,8 @@ def test_generate_trajectory_line_to_arc(self): # Quadrant 1 end_point = np.array([2, -1]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -131,7 +143,8 @@ def test_generate_trajectory_line_to_end(self): # Quadrant 1 end_point = np.array([1, 2]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -139,7 +152,8 @@ def test_generate_trajectory_line_to_end(self): # Quadrant 2 end_point = np.array([-1, 2]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -147,7 +161,8 @@ def test_generate_trajectory_line_to_end(self): # Quadrant 3 end_point = np.array([-1, -2]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -155,37 +170,42 @@ def test_generate_trajectory_line_to_end(self): # Quadrant 4 end_point = np.array([1, -2]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) def test_generate_trajectory_radius_too_small(self): # Quadrant 1 - end_point = np.array([.9, .9]) + end_point = np.array([0.9, 0.9]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(trajectory, None) # Quadrant 2 - end_point = np.array([-.9, -.9]) + end_point = np.array([-0.9, -0.9]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(trajectory, None) # Quadrant 3 - end_point = np.array([-.9, -.9]) + end_point = np.array([-0.9, -0.9]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(trajectory, None) # Quadrant 4 - end_point = np.array([.9, -.9]) + end_point = np.array([0.9, -0.9]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE + ) self.assertEqual(trajectory, None) @@ -193,7 +213,8 @@ def test_generate_trajectory_parallel_lines_coincident(self): # Quadrant 1 end_point = np.array([5, 0]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -201,7 +222,8 @@ def test_generate_trajectory_parallel_lines_coincident(self): # Quadrant 2 end_point = np.array([-5, 0]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE + ) self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -210,14 +232,16 @@ def test_generate_trajectory_parallel_lines_not_coincident(self): # Quadrant 1 end_point = np.array([0, 3]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE) + end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE + ) self.assertEqual(trajectory, None) # Quadrant 2 end_point = np.array([0, 3]) trajectory = self.trajectory_generator.generate_trajectory( - end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE) + end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE + ) self.assertEqual(trajectory, None) diff --git a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py index ef1ce0f4727..9187d0b1e46 100644 --- a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py +++ b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py @@ -32,8 +32,14 @@ def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'): """Plot arrow.""" - plt.arrow(x, y, length * np.cos(yaw), length * - np.sin(yaw), width=0.05*length, length_includes_head=True) + plt.arrow( + x, + y, + length * np.cos(yaw), + length * np.sin(yaw), + width=0.05 * length, + length_includes_head=True, + ) plt.plot(x, y) plt.plot(0, 0) @@ -51,21 +57,38 @@ def read_trajectories_data(file_path): trajectory_data = read_trajectories_data(trajectory_file_path) -min_x = min([min([pose[0] for pose in primitive['poses']]) - for primitive in trajectory_data['primitives']]) -max_x = max([max([pose[0] for pose in primitive['poses']]) - for primitive in trajectory_data['primitives']]) - -min_y = min([min([pose[1] for pose in primitive['poses']]) - for primitive in trajectory_data['primitives']]) -max_y = max([max([pose[1] for pose in primitive['poses']]) - for primitive in trajectory_data['primitives']]) +min_x = min( + [ + min([pose[0] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives'] + ] +) +max_x = max( + [ + max([pose[0] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives'] + ] +) + +min_y = min( + [ + min([pose[1] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives'] + ] +) +max_y = max( + [ + max([pose[1] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives'] + ] +) heading_angles = trajectory_data['lattice_metadata']['heading_angles'] for primitive in trajectory_data['primitives']: - arrow_length = (primitive['arc_length'] + - primitive['straight_length']) / len(primitive['poses']) + arrow_length = (primitive['arc_length'] + primitive['straight_length']) / len( + primitive['poses'] + ) if arrow_length == 0: arrow_length = max_x / len(primitive['poses']) @@ -73,7 +96,7 @@ def read_trajectories_data(file_path): xs = np.array([pose[0] for pose in primitive['poses']]) ys = np.array([pose[1] for pose in primitive['poses']]) - lengths = np.sqrt((xs[1:] - xs[:-1])**2 + (ys[1:] - ys[:-1])**2) + lengths = np.sqrt((xs[1:] - xs[:-1]) ** 2 + (ys[1:] - ys[:-1]) ** 2) print('Distances between points: ', lengths) for x, y, yaw in primitive['poses']: @@ -85,14 +108,13 @@ def read_trajectories_data(file_path): left_x, right_x = plt.xlim() left_y, right_y = plt.ylim() - plt.xlim(1.2*min_x, 1.2*max_x) - plt.ylim(1.2*min_y, 1.2*max_y) + plt.xlim(1.2 * min_x, 1.2 * max_x) + plt.ylim(1.2 * min_y, 1.2 * max_y) start_angle = np.rad2deg(heading_angles[primitive['start_angle_index']]) end_angle = np.rad2deg(heading_angles[primitive['end_angle_index']]) plt.title(f"Trajectory ID: {primitive['trajectory_id']}") - plt.figtext( - 0.7, 0.9, f'Start: {start_angle}\nEnd: {end_angle}') + plt.figtext(0.7, 0.9, f'Start: {start_angle}\nEnd: {end_angle}') plt.show() diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py index 520ded328da..0dcb6edc07e 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -73,8 +73,7 @@ def end_straight_length(self): @property def total_length(self): """Total length of trajectory.""" - return self.arc_length + self.start_straight_length + \ - self.end_straight_length + return self.arc_length + self.start_straight_length + self.end_straight_length @staticmethod def no_arc(end_point, start_angle, end_angle): diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py index f2f51baf01e..bd47d055cdf 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -243,6 +243,7 @@ def _get_intersection_point( The intersection point of line 1 and 2 """ + def line1(x): return m1 * x + c1 diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index e3863fa75ef..c852a96dd32 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -320,7 +320,7 @@ float NodeLattice::getTraversalCost(const NodePtr & child) float NodeLattice::getHeuristicCost( const Coordinates & node_coords, const Coordinates & goal_coords, - const nav2_costmap_2d::Costmap2D * costmap) + const nav2_costmap_2d::Costmap2D * /*costmap*/) { // get obstacle heuristic value const float obstacle_heuristic = getObstacleHeuristic( @@ -415,7 +415,7 @@ float NodeLattice::getDistanceHeuristic( void NodeLattice::precomputeDistanceHeuristic( const float & lookup_table_dim, - const MotionModel & motion_model, + const MotionModel & /*motion_model*/, const unsigned int & dim_3_size, const SearchInfo & search_info) { diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 6545d3e724f..92d567a0573 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -142,7 +142,7 @@ bool SmootherServer::loadSmootherPlugins() node, smoother_ids_[i], tf_, costmap_sub_, footprint_sub_); smoothers_.insert({smoother_ids_[i], smoother}); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & ex) { RCLCPP_FATAL( get_logger(), "Failed to create smoother. Exception: %s", ex.what()); diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp index 89df4c3a4fa..524ba4f9fca 100644 --- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -206,16 +206,12 @@ TEST(SmootherTest, test_sg_smoother_noisey_path) EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time)); length = 0; - double non_refined_length = 0; for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { length += std::hypot( noisey_path_refined.poses[i + 1].pose.position.x - noisey_path_refined.poses[i].pose.position.x, noisey_path_refined.poses[i + 1].pose.position.y - noisey_path_refined.poses[i].pose.position.y); - non_refined_length += std::hypot( - noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x, - noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y); } EXPECT_LT(length, base_length); diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 08e475de8e1..67472eb46bf 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -38,8 +38,7 @@ using namespace std::chrono_literals; class DummySmoother : public nav2_core::Smoother { public: - DummySmoother() - : initialized_(false) {} + DummySmoother() {} ~DummySmoother() {} @@ -81,7 +80,6 @@ class DummySmoother : public nav2_core::Smoother } private: - bool initialized_; std::string command_; std::chrono::system_clock::time_point start_time_; }; diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index 57f2b656252..b262d36efbd 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -89,9 +102,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_assisted_teleop_behavior_node', - output='screen') + cmd=[testExecutable], name='test_assisted_teleop_behavior_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index 6091ffb40c3..6f4a6c3c16e 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -89,9 +102,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_backup_behavior_node', - output='screen') + cmd=[testExecutable], name='test_backup_behavior_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py index 49959ec86df..8f23ce67b26 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -91,7 +104,8 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], name='test_drive_on_heading_behavior_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 27b1f5e800b..706767b9ced 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -142,7 +142,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( auto goal_msg = Spin::Goal(); goal_msg.target_yaw = target_yaw; - // Intialize fake costmap + // Initialize fake costmap if (make_fake_costmap_) { sendFakeCostmap(target_yaw); sendFakeOdom(0.0); @@ -160,7 +160,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( fabs(tf2::getYaw(initial_pose.pose.orientation))); RCLCPP_INFO(node_->get_logger(), "Before sending goal"); - // Intialize fake costmap + // Initialize fake costmap if (make_fake_costmap_) { sendFakeCostmap(target_yaw); sendFakeOdom(0.0); diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py index 1b0ace8f1bb..22b8327cd25 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py @@ -35,7 +35,9 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') autostart = LaunchConfiguration('autostart') params_file = LaunchConfiguration('params_file') - default_nav_through_poses_bt_xml = LaunchConfiguration('default_nav_through_poses_bt_xml') + default_nav_through_poses_bt_xml = LaunchConfiguration( + 'default_nav_through_poses_bt_xml' + ) default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml') map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') @@ -45,13 +47,15 @@ def generate_launch_description(): 'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml, 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, 'autostart': autostart, - 'map_subscribe_transient_local': map_subscribe_transient_local} + 'map_subscribe_transient_local': map_subscribe_transient_local, + } configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ) lifecycle_nodes = ['behavior_server'] @@ -61,83 +65,94 @@ def generate_launch_description(): # https://github.com/ros/robot_state_publisher/pull/30 # TODO(orduno) Substitute with `PushNodeRemapping` # https://github.com/ros2/launch_ros/issues/56 - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - DeclareLaunchArgument( - 'namespace', default_value='', - description='Top-level namespace'), - - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack'), - - DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use'), - - DeclareLaunchArgument( - 'default_nav_through_poses_bt_xml', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_through_poses_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use'), - - DeclareLaunchArgument( - 'default_nav_to_pose_bt_xml', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_to_pose_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use'), - - DeclareLaunchArgument( - 'map_subscribe_transient_local', default_value='false', - description='Whether to set the map subscriber QoS to transient local'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - Node( - package='nav2_behaviors', - executable='behavior_server', - name='behavior_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]), - ]) + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ), + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true', + ), + DeclareLaunchArgument( + 'autostart', + default_value='true', + description='Automatically startup the nav2 stack', + ), + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use', + ), + DeclareLaunchArgument( + 'default_nav_through_poses_bt_xml', + default_value=os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + 'navigate_through_poses_w_replanning_and_recovery.xml', + ), + description='Full path to the behavior tree xml file to use', + ), + DeclareLaunchArgument( + 'default_nav_to_pose_bt_xml', + default_value=os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + 'navigate_to_pose_w_replanning_and_recovery.xml', + ), + description='Full path to the behavior tree xml file to use', + ), + DeclareLaunchArgument( + 'map_subscribe_transient_local', + default_value='false', + description='Whether to set the map subscriber QoS to transient local', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + parameters=[configured_params], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[ + {'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}, + ], + ), + ] + ) def main(argv=sys.argv[1:]): @@ -146,9 +161,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_spin_behavior_fake_node', - output='screen') + cmd=[testExecutable], name='test_spin_behavior_fake_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index 154f9ed5331..4552f51768d 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -89,9 +102,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_spin_behavior_node', - output='screen') + cmd=[testExecutable], name='test_spin_behavior_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index ee89e416de7..f4c4e5bc168 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -20,7 +20,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -32,55 +36,64 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, - root_key='', - param_rewrites='', - convert_types=True) - - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + source_file=params_file, root_key='', param_rewrites='', convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): @@ -89,9 +102,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_wait_behavior_node', - output='screen') + cmd=[testExecutable], name='test_wait_behavior_node', output='screen' + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 61c5392137a..dcace8aeb6d 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -36,9 +40,11 @@ def generate_launch_description(): filter_mask_file = os.getenv('TEST_MASK') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') script_dir = os.path.dirname(os.path.realpath(__file__)) @@ -48,96 +54,119 @@ def generate_launch_description(): param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, - 'map_server.ros__parameters.yaml_filename': map_yaml_file} + 'map_server.ros__parameters.yaml_filename': map_yaml_file, + } configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) context = LaunchContext() new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_filters', - output='screen', - parameters=[{ - 'node_names': - [ - 'filter_mask_server', 'costmap_filter_info_server' - ] - }, - {'autostart': True}]), - - # Nodes required for Costmap Filters configuration - Node( - package='nav2_map_server', - executable='map_server', - name='filter_mask_server', - output='screen', - parameters=[new_yaml]), - - Node( - package='nav2_map_server', - executable='costmap_filter_info_server', - name='costmap_filter_info_server', - output='screen', - parameters=[new_yaml]), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - - Node( - package='nav2_costmap_2d', - executable='nav2_costmap_2d_cloud', - name='costmap_2d_cloud', - output='screen', - remappings=[('voxel_grid', 'local_costmap/voxel_grid')]), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_filters', + output='screen', + parameters=[ + { + 'node_names': [ + 'filter_mask_server', + 'costmap_filter_info_server', + ] + }, + {'autostart': True}, + ], + ), + # Nodes required for Costmap Filters configuration + Node( + package='nav2_map_server', + executable='map_server', + name='filter_mask_server', + output='screen', + parameters=[new_yaml], + ), + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + parameters=[new_yaml], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + Node( + package='nav2_costmap_2d', + executable='nav2_costmap_2d_cloud', + name='costmap_2d_cloud', + output='screen', + remappings=[('voxel_grid', 'local_costmap/voxel_grid')], + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), - '-t', 'keepout', '-r', '-2.0', '-0.5', '0.0', '-0.5'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-t', + 'keepout', + '-r', + '-2.0', + '-0.5', + '0.0', + '-0.5', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 971fcdd5e0d..95d6fb412f0 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -36,9 +40,11 @@ def generate_launch_description(): filter_mask_file = os.getenv('TEST_MASK') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.getenv('PARAMS_FILE') @@ -47,88 +53,111 @@ def generate_launch_description(): param_substitutions = { 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, - 'map_server.ros__parameters.yaml_filename': map_yaml_file} + 'map_server.ros__parameters.yaml_filename': map_yaml_file, + } configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) context = LaunchContext() new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_filters', - output='screen', - parameters=[{ - 'node_names': - [ - 'filter_mask_server', 'costmap_filter_info_server' - ] - }, - {'autostart': True}]), - - # Nodes required for Costmap Filters configuration - Node( - package='nav2_map_server', - executable='map_server', - name='filter_mask_server', - output='screen', - parameters=[new_yaml]), - - Node( - package='nav2_map_server', - executable='costmap_filter_info_server', - name='costmap_filter_info_server', - output='screen', - parameters=[new_yaml]), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_filters', + output='screen', + parameters=[ + { + 'node_names': [ + 'filter_mask_server', + 'costmap_filter_info_server', + ] + }, + {'autostart': True}, + ], + ), + # Nodes required for Costmap Filters configuration + Node( + package='nav2_map_server', + executable='map_server', + name='filter_mask_server', + output='screen', + parameters=[new_yaml], + ), + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + parameters=[new_yaml], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), - '-t', 'speed', '-r', '-2.0', '-0.5', '0.0', '-0.5'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-t', + 'speed', + '-r', + '-2.0', + '-0.5', + '0.0', + '-0.5', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index 0a611162545..da8ab82a20b 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -46,12 +46,8 @@ class TestType(Enum): SPEED = 1 -class FilterMask(): - - def __init__( - self, - filter_mask: OccupancyGrid - ): +class FilterMask: + def __init__(self, filter_mask: OccupancyGrid): self.filter_mask = filter_mask # Converts world coordinates into filter mask map coordinate. @@ -82,38 +78,45 @@ def getValue(self, mx, my): class NavTester(Node): - def __init__( self, test_type: TestType, initial_pose: Pose, goal_pose: Pose, - namespace: str = '' + namespace: str = '', ): super().__init__(node_name='nav2_tester', namespace=namespace) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) transient_local_qos = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.KEEP_LAST, - depth=1) + depth=1, + ) volatile_qos = QoSProfile( durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, - transient_local_qos) - self.clearing_ep_sub = self.create_subscription(PointCloud2, - 'local_costmap/clearing_endpoints', - self.clearingEndpointsCallback, - transient_local_qos) + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, + 'amcl_pose', + self.poseCallback, + transient_local_qos, + ) + self.clearing_ep_sub = self.create_subscription( + PointCloud2, + 'local_costmap/clearing_endpoints', + self.clearingEndpointsCallback, + transient_local_qos, + ) self.test_type = test_type self.filter_test_result = True self.clearing_endpoints_received = False @@ -122,38 +125,37 @@ def __init__( self.cost_cloud_received = False if self.test_type == TestType.KEEPOUT: - self.plan_sub = self.create_subscription(Path, 'plan', - self.planCallback, volatile_qos) - self.voxel_marked_sub = self.create_subscription(PointCloud2, - 'voxel_marked_cloud', - self.voxelMarkedCallback, - 1) - self.voxel_unknown_sub = self.create_subscription(PointCloud2, - 'voxel_unknown_cloud', - self.voxelUnknownCallback, - 1) - self.cost_cloud_sub = self.create_subscription(PointCloud2, - 'cost_cloud', - self.dwbCostCloudCallback, - 1) + self.plan_sub = self.create_subscription( + Path, 'plan', self.planCallback, volatile_qos + ) + self.voxel_marked_sub = self.create_subscription( + PointCloud2, 'voxel_marked_cloud', self.voxelMarkedCallback, 1 + ) + self.voxel_unknown_sub = self.create_subscription( + PointCloud2, 'voxel_unknown_cloud', self.voxelUnknownCallback, 1 + ) + self.cost_cloud_sub = self.create_subscription( + PointCloud2, 'cost_cloud', self.dwbCostCloudCallback, 1 + ) elif self.test_type == TestType.SPEED: self.speed_it = 0 # Expected chain of speed limits self.limits = [50.0, 0.0] - # Permissive array: all received speed limits must match to "limits" from above + # Permissive array: all received speed limits must match to 'limits' from above self.limit_passed = [False, False] - self.plan_sub = self.create_subscription(SpeedLimit, 'speed_limit', - self.speedLimitCallback, volatile_qos) + self.plan_sub = self.create_subscription( + SpeedLimit, 'speed_limit', self.speedLimitCallback, volatile_qos + ) self.mask_received = False - self.mask_sub = self.create_subscription(OccupancyGrid, 'filter_mask', - self.maskCallback, transient_local_qos) + self.mask_sub = self.create_subscription( + OccupancyGrid, 'filter_mask', self.maskCallback, transient_local_qos + ) self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose - self.action_client = ActionClient( - self, NavigateToPose, 'navigate_to_pose') + self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') def info_msg(self, msg: str): self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') @@ -186,8 +188,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateToPose' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg( - "'NavigateToPose' action server not available, waiting...") + self.info_msg("'NavigateToPose' action server not available, waiting...") self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() @@ -235,12 +236,12 @@ def checkKeepout(self, x, y): return True # Checks that currently received speed_limit is equal to the it-th item - # of expected speed "limits" array. - # If so, sets it-th item of permissive array "limit_passed" to be true. + # of expected speed 'limits' array. + # If so, sets it-th item of permissive array 'limit_passed' to be true. # Otherwise it will be remained to be false. # Also verifies that speed limit messages received no more than N-times - # (where N - is the length of "limits" array), - # otherwise sets overall "filter_test_result" to be false. + # (where N - is the length of 'limits' array), + # otherwise sets overall 'filter_test_result' to be false. def checkSpeed(self, it, speed_limit): if it >= len(self.limits): self.error_msg('Got excess speed limit') @@ -249,15 +250,21 @@ def checkSpeed(self, it, speed_limit): if speed_limit == self.limits[it]: self.limit_passed[it] = True else: - self.error_msg('Incorrect speed limit received: ' + str(speed_limit) + - ', but should be: ' + str(self.limits[it])) + self.error_msg( + 'Incorrect speed limit received: ' + + str(speed_limit) + + ', but should be: ' + + str(self.limits[it]) + ) def poseCallback(self, msg): self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose self.initial_pose_received = True if self.test_type == TestType.KEEPOUT: - if not self.checkKeepout(msg.pose.pose.position.x, msg.pose.pose.position.y): + if not self.checkKeepout( + msg.pose.pose.position.x, msg.pose.pose.position.y + ): self.error_msg('Robot goes into keepout zone') def planCallback(self, msg): @@ -307,16 +314,21 @@ def wait_for_filter_mask(self, timeout): def wait_for_pointcloud_subscribers(self, timeout): start_time = time.time() - while not self.voxel_unknown_received or not self.voxel_marked_received \ - or not self.clearing_endpoints_received: + while ( + not self.voxel_unknown_received + or not self.voxel_marked_received + or not self.clearing_endpoints_received + ): self.info_msg( 'Waiting for voxel_marked_cloud/voxel_unknown_cloud/\ - clearing_endpoints msg to be received ...') + clearing_endpoints msg to be received ...' + ) rclpy.spin_once(self, timeout_sec=1) if (time.time() - start_time) > timeout: self.error_msg( 'Time out to waiting for voxel_marked_cloud/voxel_unknown_cloud/\ - clearing_endpoints msgs') + clearing_endpoints msgs' + ) return False return True @@ -351,7 +363,7 @@ def wait_for_node_active(self, node_name: str): self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' - while (state != 'active'): + while state != 'active': self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -359,8 +371,9 @@ def wait_for_node_active(self, node_name: str): state = future.result().current_state.label self.info_msg(f'Result of get_state: {state}') else: - self.error_msg('Exception while calling service: %r' % - future.exception()) + self.error_msg( + 'Exception while calling service: %r' % future.exception() + ) time.sleep(5) def shutdown(self): @@ -368,8 +381,7 @@ def shutdown(self): self.action_client.destroy() transition_service = 'lifecycle_manager_navigation/manage_nodes' - mgr_client = self.create_client( - ManageLifecycleNodes, transition_service) + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): self.info_msg(f'{transition_service} service not available, waiting...') @@ -380,13 +392,11 @@ def shutdown(self): self.info_msg('Shutting down navigation lifecycle manager...') rclpy.spin_until_future_complete(self, future) future.result() - self.info_msg( - 'Shutting down navigation lifecycle manager complete.') + self.info_msg('Shutting down navigation lifecycle manager complete.') except Exception as e: # noqa: B902 self.error_msg(f'Service call failed {e!r}') transition_service = 'lifecycle_manager_localization/manage_nodes' - mgr_client = self.create_client( - ManageLifecycleNodes, transition_service) + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): self.info_msg(f'{transition_service} service not available, waiting...') @@ -397,8 +407,7 @@ def shutdown(self): self.info_msg('Shutting down localization lifecycle manager...') rclpy.spin_until_future_complete(self, future) future.result() - self.info_msg( - 'Shutting down localization lifecycle manager complete') + self.info_msg('Shutting down localization lifecycle manager complete') except Exception as e: # noqa: B902 self.error_msg(f'Service call failed {e!r}') @@ -419,10 +428,10 @@ def test_RobotMovesToGoal(robot_tester): # Tests that all received speed limits are correct: -# If overall "filter_test_result" is true -# checks that all items in "limit_passed" permissive array are also true. +# If overall 'filter_test_result' is true +# checks that all items in 'limit_passed' permissive array are also true. # In other words, it verifies that all speed limits are received -# exactly (by count and values) as expected by "limits" array. +# exactly (by count and values) as expected by 'limits' array. def test_SpeedLimitsAllCorrect(robot_tester): if not robot_tester.filter_test_result: return False @@ -436,21 +445,21 @@ def test_SpeedLimitsAllCorrect(robot_tester): def run_all_tests(robot_tester): # set transforms to use_sim_time result = True - if (result): + if result: robot_tester.wait_for_node_active('amcl') robot_tester.wait_for_initial_pose() robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.wait_for_filter_mask(10) - if (result): + if result: result = robot_tester.runNavigateAction() if robot_tester.test_type == TestType.KEEPOUT: result = result and robot_tester.wait_for_pointcloud_subscribers(10) - if (result): + if result: result = test_RobotMovesToGoal(robot_tester) - if (result): + if result: if robot_tester.test_type == TestType.KEEPOUT: result = robot_tester.filter_test_result result = result and robot_tester.cost_cloud_received @@ -459,7 +468,7 @@ def run_all_tests(robot_tester): # Add more tests here if desired - if (result): + if result: robot_tester.info_msg('Test PASSED') else: robot_tester.error_msg('Test FAILED') @@ -491,23 +500,44 @@ def get_tester(args): tester = NavTester( test_type, initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester, robot going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y + '.') + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + '.' + ) return tester def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments parser = argparse.ArgumentParser( - description='System-level costmap filters tester node') - parser.add_argument('-t', '--type', type=str, action='store', dest='type', - help='Type of costmap filter being tested.') + description='System-level costmap filters tester node' + ) + parser.add_argument( + '-t', + '--type', + type=str, + action='store', + dest='type', + help='Type of costmap filter being tested.', + ) group = parser.add_mutually_exclusive_group(required=True) - group.add_argument('-r', '--robot', action='append', nargs=4, - metavar=('init_x', 'init_y', 'final_x', 'final_y'), - help='The robot starting and final positions.') + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) args, unknown = parser.parse_known_args() diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index 7a46f5fd812..f023bbb27c1 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -17,7 +17,12 @@ import time from geometry_msgs.msg import PoseStamped -from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose, FollowPath, SmoothPath +from nav2_msgs.action import ( + ComputePathThroughPoses, + ComputePathToPose, + FollowPath, + SmoothPath, +) from nav2_simple_commander.robot_navigator import BasicNavigator from nav_msgs.msg import Path import rclpy @@ -60,7 +65,7 @@ def main(argv=sys.argv[1:]): 'invalid_path': FollowPath.Result().INVALID_PATH, 'patience_exceeded': FollowPath.Result().PATIENCE_EXCEEDED, 'failed_to_make_progress': FollowPath.Result().FAILED_TO_MAKE_PROGRESS, - 'no_valid_control': FollowPath.Result().NO_VALID_CONTROL + 'no_valid_control': FollowPath.Result().NO_VALID_CONTROL, } for controller, error_code in follow_path.items(): @@ -70,8 +75,9 @@ def main(argv=sys.argv[1:]): while not navigator.isTaskComplete(): time.sleep(0.5) - assert navigator.result_future.result().result.error_code == error_code, \ - 'Follow path error code does not match' + assert ( + navigator.result_future.result().result.error_code == error_code + ), 'Follow path error code does not match' else: assert False, 'Follow path was rejected' @@ -96,11 +102,14 @@ def main(argv=sys.argv[1:]): 'start_occupied': ComputePathToPose.Result().START_OCCUPIED, 'goal_occupied': ComputePathToPose.Result().GOAL_OCCUPIED, 'timeout': ComputePathToPose.Result().TIMEOUT, - 'no_valid_path': ComputePathToPose.Result().NO_VALID_PATH} + 'no_valid_path': ComputePathToPose.Result().NO_VALID_PATH, + } for planner, error_code in compute_path_to_pose.items(): result = navigator._getPathImpl(initial_pose, goal_pose, planner) - assert result.error_code == error_code, 'Compute path to pose error does not match' + assert ( + result.error_code == error_code + ), 'Compute path to pose error does not match' # Check compute path through error codes goal_pose1 = goal_pose @@ -116,11 +125,14 @@ def main(argv=sys.argv[1:]): 'goal_occupied': ComputePathThroughPoses.Result().GOAL_OCCUPIED, 'timeout': ComputePathThroughPoses.Result().TIMEOUT, 'no_valid_path': ComputePathThroughPoses.Result().NO_VALID_PATH, - 'no_viapoints_given': ComputePathThroughPoses.Result().NO_VIAPOINTS_GIVEN} + 'no_viapoints_given': ComputePathThroughPoses.Result().NO_VIAPOINTS_GIVEN, + } for planner, error_code in compute_path_through_poses.items(): result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) - assert result.error_code == error_code, 'Compute path through pose error does not match' + assert ( + result.error_code == error_code + ), 'Compute path through pose error does not match' # Check compute path to pose error codes pose = PoseStamped() @@ -146,7 +158,7 @@ def main(argv=sys.argv[1:]): 'timeout': SmoothPath.Result().TIMEOUT, 'smoothed_path_in_collision': SmoothPath.Result().SMOOTHED_PATH_IN_COLLISION, 'failed_to_smooth_path': SmoothPath.Result().FAILED_TO_SMOOTH_PATH, - 'invalid_path': SmoothPath.Result().INVALID_PATH + 'invalid_path': SmoothPath.Result().INVALID_PATH, } for smoother, error_code in smoother.items(): diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index d70c56ac8e0..98a33d38d63 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -27,12 +27,9 @@ def generate_launch_description(): params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - lifecycle_nodes = ['controller_server', - 'planner_server', - 'smoother_server'] + lifecycle_nodes = ['controller_server', 'planner_server', 'smoother_server'] load_nodes = GroupAction( actions=[ @@ -40,19 +37,22 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + ), Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link']), + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'], + ), Node( package='nav2_controller', executable='controller_server', output='screen', respawn_delay=2.0, parameters=[params_file], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')], + ), Node( package='nav2_planner', executable='planner_server', @@ -60,7 +60,8 @@ def generate_launch_description(): output='screen', respawn_delay=2.0, parameters=[params_file], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_smoother', executable='smoother_server', @@ -68,15 +69,17 @@ def generate_launch_description(): output='screen', respawn_delay=2.0, parameters=[params_file], - remappings=remappings), + remappings=remappings, + ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen', - parameters=[{'autostart': True}, - {'node_names': lifecycle_nodes}]) - ]) + parameters=[{'autostart': True}, {'node_names': lifecycle_nodes}], + ), + ] + ) ld = LaunchDescription() ld.add_action(load_nodes) @@ -90,7 +93,8 @@ def main(argv=sys.argv[1:]): test_error_codes_action = ExecuteProcess( cmd=[os.path.join(os.getenv('TEST_DIR'), 'test_error_codes.py')], name='test_error_codes', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test_error_codes_action) diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py index 852436615b4..8cc64ec69e1 100644 --- a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py @@ -11,52 +11,53 @@ # See the License for the specific language governing permissions and # limitations under the License. -from launch import LaunchDescription -import launch_ros.actions import os + +from launch import LaunchDescription import launch.actions +import launch_ros.actions def generate_launch_description(): launch_dir = os.path.dirname(os.path.realpath(__file__)) - params_file = os.path.join(launch_dir, "dual_ekf_navsat_params.yaml") - os.environ["FILE_PATH"] = str(launch_dir) + params_file = os.path.join(launch_dir, 'dual_ekf_navsat_params.yaml') + os.environ['FILE_PATH'] = str(launch_dir) return LaunchDescription( [ launch.actions.DeclareLaunchArgument( - "output_final_position", default_value="false" + 'output_final_position', default_value='false' ), launch.actions.DeclareLaunchArgument( - "output_location", default_value="~/dual_ekf_navsat_example_debug.txt" + 'output_location', default_value='~/dual_ekf_navsat_example_debug.txt' ), launch_ros.actions.Node( - package="robot_localization", - executable="ekf_node", - name="ekf_filter_node_odom", - output="screen", - parameters=[params_file, {"use_sim_time": True}], - remappings=[("odometry/filtered", "odometry/local")], + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_odom', + output='screen', + parameters=[params_file, {'use_sim_time': True}], + remappings=[('odometry/filtered', 'odometry/local')], ), launch_ros.actions.Node( - package="robot_localization", - executable="ekf_node", - name="ekf_filter_node_map", - output="screen", - parameters=[params_file, {"use_sim_time": True}], - remappings=[("odometry/filtered", "odometry/global")], + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_map', + output='screen', + parameters=[params_file, {'use_sim_time': True}], + remappings=[('odometry/filtered', 'odometry/global')], ), launch_ros.actions.Node( - package="robot_localization", - executable="navsat_transform_node", - name="navsat_transform", - output="screen", - parameters=[params_file, {"use_sim_time": True}], + package='robot_localization', + executable='navsat_transform_node', + name='navsat_transform', + output='screen', + parameters=[params_file, {'use_sim_time': True}], remappings=[ - ("imu/data", "imu/data"), - ("gps/fix", "gps/fix"), - ("gps/filtered", "gps/filtered"), - ("odometry/gps", "odometry/gps"), - ("odometry/filtered", "odometry/global"), + ('imu/data', 'imu/data'), + ('gps/fix', 'gps/fix'), + ('gps/filtered', 'gps/filtered'), + ('odometry/gps', 'odometry/gps'), + ('odometry/filtered', 'odometry/global'), ], ), ] diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index cd90fdb6208..db949b7756e 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -33,79 +33,79 @@ def generate_launch_description(): - world = os.getenv("TEST_WORLD") + world = os.getenv('TEST_WORLD') launch_dir = os.path.dirname(os.path.realpath(__file__)) - params_file = os.path.join(launch_dir, "nav2_no_map_params.yaml") - bringup_dir = get_package_share_directory("nav2_bringup") + params_file = os.path.join(launch_dir, 'nav2_no_map_params.yaml') + bringup_dir = get_package_share_directory('nav2_bringup') configured_params = RewrittenYaml( - source_file=params_file, root_key="", param_rewrites="", convert_types=True + source_file=params_file, root_key='', param_rewrites='', convert_types=True ) return LaunchDescription( [ - SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), - SetEnvironmentVariable("RCUTILS_LOGGING_USE_STDOUT", "1"), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( cmd=[ - "gzserver", - "-s", - "libgazebo_ros_init.so", - "--minimal_comms", + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', world, ], - output="screen", + output='screen', ), # TODO(orduno) Launch the robot state publisher instead # using a local copy of TB3 urdf file Node( - package="tf2_ros", - executable="static_transform_publisher", - output="screen", - arguments=["0", "0", "0", "0", "0", "0", "base_footprint", "base_link"], + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], ), Node( - package="tf2_ros", - executable="static_transform_publisher", - output="screen", - arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_scan"], + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], ), Node( - package="tf2_ros", - executable="static_transform_publisher", - output="screen", + package='tf2_ros', + executable='static_transform_publisher', + output='screen', arguments=[ - "-0.32", - "0", - "0.068", - "0", - "0", - "0", - "base_link", - "imu_link", + '-0.32', + '0', + '0.068', + '0', + '0', + '0', + 'base_link', + 'imu_link', ], ), Node( - package="tf2_ros", - executable="static_transform_publisher", - output="screen", - arguments=["0", "0", "0", "0", "0", "0", "base_link", "gps_link"], + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'gps_link'], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, "launch", "navigation_launch.py") + os.path.join(bringup_dir, 'launch', 'navigation_launch.py') ), launch_arguments={ - "use_sim_time": "True", - "params_file": configured_params, - "autostart": "True", + 'use_sim_time': 'True', + 'params_file': configured_params, + 'autostart': 'True', }.items(), ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(launch_dir, "dual_ekf_navsat.launch.py") + os.path.join(launch_dir, 'dual_ekf_navsat.launch.py') ), ), ] @@ -116,9 +116,9 @@ def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv("TEST_DIR"), "tester.py")], - name="tester_node", - output="screen", + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester.py')], + name='tester_node', + output='screen', ) lts = LaunchTestService() @@ -128,5 +128,5 @@ def main(argv=sys.argv[1:]): return lts.run(ls) -if __name__ == "__main__": +if __name__ == '__main__': sys.exit(main()) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index aab88336682..5e765981ab2 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -21,25 +21,27 @@ from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters -from rclpy.parameter import Parameter import rclpy + from rclpy.action import ActionClient from rclpy.node import Node +from rclpy.parameter import Parameter class GpsWaypointFollowerTest(Node): def __init__(self): - super().__init__(node_name="nav2_gps_waypoint_tester", namespace="") + super().__init__(node_name='nav2_gps_waypoint_tester', namespace='') self.waypoints = None self.action_client = ActionClient( - self, FollowGPSWaypoints, "follow_gps_waypoints" + self, FollowGPSWaypoints, 'follow_gps_waypoints' ) self.goal_handle = None self.action_result = None - self.param_cli = self.create_client(SetParameters, - '/waypoint_follower/set_parameters') + self.param_cli = self.create_client( + SetParameters, '/waypoint_follower/set_parameters' + ) def setWaypoints(self, waypoints): self.waypoints = [] @@ -52,29 +54,30 @@ def setWaypoints(self, waypoints): def run(self, block, cancel): # if not self.waypoints: - # rclpy.error_msg("Did not set valid waypoints before running test!") + # rclpy.error_msg('Did not set valid waypoints before running test!') # return False while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg( - "'follow_gps_waypoints' action server not available, waiting...") + "'follow_gps_waypoints' action server not available, waiting..." + ) action_request = FollowGPSWaypoints.Goal() action_request.gps_poses = self.waypoints - self.info_msg("Sending goal request...") + self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(action_request) try: rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() except Exception as e: # noqa: B902 - self.error_msg(f"Service call failed {e!r}") + self.error_msg(f'Service call failed {e!r}') if not self.goal_handle.accepted: - self.error_msg("Goal rejected") + self.error_msg('Goal rejected') return False - self.info_msg("Goal accepted") + self.info_msg('Goal accepted') if not block: return True @@ -90,40 +93,39 @@ def run(self, block, cancel): result = get_result_future.result().result self.action_result = result except Exception as e: # noqa: B902 - self.error_msg(f"Service call failed {e!r}") + self.error_msg(f'Service call failed {e!r}') if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg(f"Goal failed with status code: {status}") + self.info_msg(f'Goal failed with status code: {status}') return False if len(result.missed_waypoints) > 0: self.info_msg( - "Goal failed to process all waypoints," - " missed {0} wps.".format(len(result.missed_waypoints)) + 'Goal failed to process all waypoints,' + ' missed {0} wps.'.format(len(result.missed_waypoints)) ) return False - self.info_msg("Goal succeeded!") + self.info_msg('Goal succeeded!') return True def setStopFailureParam(self, value): req = SetParameters.Request() - req.parameters = [Parameter('stop_on_failure', - Parameter.Type.BOOL, value).to_parameter_msg()] + req.parameters = [ + Parameter('stop_on_failure', Parameter.Type.BOOL, value).to_parameter_msg() + ] future = self.param_cli.call_async(req) rclpy.spin_until_future_complete(self, future) def shutdown(self): - self.info_msg("Shutting down") + self.info_msg('Shutting down') self.action_client.destroy() - self.info_msg("Destroyed follow_gps_waypoints action client") + self.info_msg('Destroyed follow_gps_waypoints action client') - transition_service = "lifecycle_manager_navigation/manage_nodes" - mgr_client = self.create_client( - ManageLifecycleNodes, transition_service) + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg( - f"{transition_service} service not available, waiting...") + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -132,9 +134,9 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() except Exception as e: # noqa: B902 - self.error_msg(f"{transition_service} service call failed {e!r}") + self.error_msg(f'{transition_service} service call failed {e!r}') - self.info_msg(f"{transition_service} finished") + self.info_msg(f'{transition_service} finished') def cancel_goal(self): cancel_future = self.goal_handle.cancel_goal_async() @@ -183,8 +185,10 @@ def main(argv=sys.argv[1:]): result = test.run(True, False) assert not result result = not result - assert test.action_result.missed_waypoints[0].error_code == \ - ComputePathToPose.Result().GOAL_OUTSIDE_MAP + assert ( + test.action_result.missed_waypoints[0].error_code + == ComputePathToPose.Result().GOAL_OUTSIDE_MAP + ) # stop on failure test with bogous waypoint test.setStopFailureParam(True) @@ -208,15 +212,15 @@ def main(argv=sys.argv[1:]): result = not result test.shutdown() - test.info_msg("Done Shutting Down.") + test.info_msg('Done Shutting Down.') if not result: - test.info_msg("Exiting failed") + test.info_msg('Exiting failed') exit(1) else: - test.info_msg("Exiting passed") + test.info_msg('Exiting passed') exit(0) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index 70c30e0842a..d638e02c4e8 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -32,40 +32,50 @@ def main(argv=sys.argv[1:]): launch_gazebo = launch.actions.ExecuteProcess( cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world], - output='screen') + output='screen', + ) link_footprint = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']) + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ) footprint_scan = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']) + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ) run_map_server = launch_ros.actions.Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', - parameters=[{'yaml_filename': mapFile}]) + parameters=[{'yaml_filename': mapFile}], + ) run_amcl = launch_ros.actions.Node( - package='nav2_amcl', - executable='amcl', - output='screen') + package='nav2_amcl', executable='amcl', output='screen' + ) run_lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager', output='screen', - parameters=[{'node_names': ['map_server', 'amcl']}, {'autostart': True}]) - ld = LaunchDescription([launch_gazebo, link_footprint, footprint_scan, - run_map_server, run_amcl, run_lifecycle_manager]) + parameters=[{'node_names': ['map_server', 'amcl']}, {'autostart': True}], + ) + ld = LaunchDescription( + [ + launch_gazebo, + link_footprint, + footprint_scan, + run_map_server, + run_amcl, + run_lifecycle_manager, + ] + ) test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_localization_node', - output='screen' + cmd=[testExecutable], name='test_localization_node', output='screen' ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index 61355a82c46..ed150c70408 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -52,9 +52,6 @@ ament_add_gtest(test_planner_plugins ) ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies}) -target_link_libraries(test_planner_plugins - stdc++fs -) ament_add_gtest(test_planner_is_path_valid test_planner_is_path_valid.cpp diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index b849e899087..7a5f8d4b7f9 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -62,6 +62,9 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer void setCostmap(nav2_util::Costmap * costmap) { + std::unique_lock lock( + *(costmap_ros_->getCostmap()->getMutex())); + nav2_msgs::msg::CostmapMetaData prop; nav2_msgs::msg::Costmap cm = costmap->get_costmap(prop); prop = cm.metadata; diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py index 4aa3dfa55af..09bd3d34eee 100755 --- a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py +++ b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py @@ -32,7 +32,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable, '--ros-args -p use_sim_time:=True'], name='test_planner_costmaps_node', - output='screen' + output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/planning/test_planner_random_launch.py b/nav2_system_tests/src/planning/test_planner_random_launch.py index 6d6563616cc..a7747e513ab 100755 --- a/nav2_system_tests/src/planning/test_planner_random_launch.py +++ b/nav2_system_tests/src/planning/test_planner_random_launch.py @@ -32,7 +32,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable, '--ros-args -p use_sim_time:=True'], name='test_planner_random_node', - output='screen' + output='screen', ) lts = LaunchTestService() diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index de3b823c6de..5e587ad8fa9 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -37,29 +37,28 @@ class NavTester(Node): - - def __init__( - self, - initial_pose: Pose, - goal_pose: Pose, - namespace: str = '' - ): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, pose_qos) + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose - self.action_client = ActionClient(self, NavigateThroughPoses, 'navigate_through_poses') + self.action_client = ActionClient( + self, NavigateThroughPoses, 'navigate_through_poses' + ) def info_msg(self, msg: str): self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') @@ -88,12 +87,16 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateThroughPoses' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'NavigateThroughPoses' action server not available, waiting...") + self.info_msg( + "'NavigateThroughPoses' action server not available, waiting..." + ) self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateThroughPoses.Goal() - goal_msg.poses = [self.getStampedPoseMsg(self.goal_pose), - self.getStampedPoseMsg(self.goal_pose)] + goal_msg.poses = [ + self.getStampedPoseMsg(self.goal_pose), + self.getStampedPoseMsg(self.goal_pose), + ] self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(goal_msg) @@ -122,7 +125,9 @@ def runFakeNavigateAction(self): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateThroughPoses' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'NavigateThroughPoses' action server not available, waiting...") + self.info_msg( + "'NavigateThroughPoses' action server not available, waiting..." + ) goal_msg = NavigateThroughPoses.Goal() @@ -153,7 +158,9 @@ def runNavigatePreemptionAction(self, block): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateThroughPoses' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'NavigateThroughPoses' action server not available, waiting...") + self.info_msg( + "'NavigateThroughPoses' action server not available, waiting..." + ) goal_msg = NavigateThroughPoses.Goal() goal_msg.poses = [self.getStampedPoseMsg(self.initial_pose)] @@ -198,7 +205,7 @@ def wait_for_node_active(self, node_name: str): self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' - while (state != 'active'): + while state != 'active': self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -206,7 +213,9 @@ def wait_for_node_active(self, node_name: str): state = future.result().current_state.label self.info_msg(f'Result of get_state: {state}') else: - self.error_msg(f'Exception while calling service: {future.exception()!r}') + self.error_msg( + f'Exception while calling service: {future.exception()!r}' + ) time.sleep(5) def shutdown(self): @@ -256,7 +265,7 @@ def wait_for_initial_pose(self): def run_all_tests(robot_tester): # set transforms to use_sim_time result = True - if (result): + if result: robot_tester.wait_for_node_active('amcl') robot_tester.wait_for_initial_pose() robot_tester.wait_for_node_active('bt_navigator') @@ -269,7 +278,7 @@ def run_all_tests(robot_tester): # Add more tests here if desired - if (result): + if result: robot_tester.info_msg('Test PASSED') else: robot_tester.error_msg('Test FAILED') @@ -295,10 +304,19 @@ def get_testers(args): init_x, init_y, final_x, final_y = args.robot[0] tester = NavTester( initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester, robot going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y + '.') + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + '.' + ) testers.append(tester) return testers @@ -307,9 +325,14 @@ def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments parser = argparse.ArgumentParser(description='System-level navigation tester node') group = parser.add_mutually_exclusive_group(required=True) - group.add_argument('-r', '--robot', action='append', nargs=4, - metavar=('init_x', 'init_y', 'final_x', 'final_y'), - help='The robot starting and final positions.') + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) args, unknown = parser.parse_known_args() diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 8fd1bc60342..cbb2ac20e6b 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -38,7 +38,6 @@ class NavTester(Node): - def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index ea7e00db93b..92f9a912dfd 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -20,8 +20,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription, LaunchService -from launch.actions import (ExecuteProcess, GroupAction, - IncludeLaunchDescription, SetEnvironmentVariable) +from launch.actions import ( + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import TextSubstitution from launch_ros.actions import Node, PushROSNamespace @@ -35,26 +39,39 @@ def generate_launch_description(): urdf = os.getenv('TEST_URDF') sdf = os.getenv('TEST_SDF') - bt_xml_file = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_xml_file = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') - robot1_params_file = os.path.join(bringup_dir, # noqa: F841 - 'params/nav2_multirobot_params_1.yaml') - robot2_params_file = os.path.join(bringup_dir, # noqa: F841 - 'params/nav2_multirobot_params_2.yaml') + robot1_params_file = os.path.join( # noqa: F841 + bringup_dir, 'params/nav2_multirobot_params_1.yaml' + ) + robot2_params_file = os.path.join( # noqa: F841 + bringup_dir, 'params/nav2_multirobot_params_2.yaml' + ) # Names and poses of the robots robots = [ {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01}, - {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}] + {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}, + ] # Launch Gazebo server for simulation start_gazebo_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', '--minimal_comms', world], - output='screen') + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', + '--minimal_comms', + world, + ], + output='screen', + ) # Define commands for spawing the robots into Gazebo spawn_robots_cmds = [] @@ -65,13 +82,21 @@ def generate_launch_description(): executable='spawn_entity.py', output='screen', arguments=[ - '-entity', TextSubstitution(text=robot['name']), - '-robot_namespace', TextSubstitution(text=robot['name']), - '-file', TextSubstitution(text=sdf), - '-x', TextSubstitution(text=str(robot['x_pose'])), - '-y', TextSubstitution(text=str(robot['y_pose'])), - '-z', TextSubstitution(text=str(robot['z_pose']))] - )) + '-entity', + TextSubstitution(text=robot['name']), + '-robot_namespace', + TextSubstitution(text=robot['name']), + '-file', + TextSubstitution(text=sdf), + '-x', + TextSubstitution(text=str(robot['x_pose'])), + '-y', + TextSubstitution(text=str(robot['y_pose'])), + '-z', + TextSubstitution(text=str(robot['z_pose'])), + ], + ) + ) with open(urdf, 'r') as infp: robot_description = infp.read() @@ -85,35 +110,48 @@ def generate_launch_description(): executable='robot_state_publisher', namespace=TextSubstitution(text=robot['name']), output='screen', - parameters=[{'use_sim_time': True, 'robot_description': robot_description}], - remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')])) + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + ) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: params_file = eval(f"{robot['name']}_params_file") - group = GroupAction([ - # Instances use the robot's name for namespace - PushROSNamespace(robot['name']), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={ - 'namespace': robot['name'], - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': params_file, - 'bt_xml_file': bt_xml_file, - 'autostart': 'True', - 'use_composition': 'False', - 'use_remappings': 'True'}.items()) - ]) + group = GroupAction( + [ + # Instances use the robot's name for namespace + PushROSNamespace(robot['name']), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': robot['name'], + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'bt_xml_file': bt_xml_file, + 'autostart': 'True', + 'use_composition': 'False', + 'use_remappings': 'True', + }.items(), + ), + ] + ) nav_instances_cmds.append(group) ld = LaunchDescription() - ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),) - ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),) + ld.add_action( + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + ) + ld.add_action( + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + ) ld.add_action(start_gazebo_cmd) for spawn_robot in spawn_robots_cmds: ld.add_action(spawn_robot) @@ -130,12 +168,26 @@ def main(argv=sys.argv[1:]): # TODO(orduno) remove duplicated definition of robots on `generate_launch_description` test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), - '-rs', 'robot1', '0.0', '0.5', '1.0', '0.5', - '-rs', 'robot2', '0.0', '-0.5', '1.0', '-0.5', - '-e', 'True'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-rs', + 'robot1', + '0.0', + '0.5', + '1.0', + '0.5', + '-rs', + 'robot2', + '0.0', + '-0.5', + '1.0', + '-0.5', + '-e', + 'True', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 3678b242ac8..5b0dd2625c0 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -35,9 +39,11 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') @@ -47,69 +53,90 @@ def generate_launch_description(): context = LaunchContext() param_substitutions = {} - if (os.getenv('ASTAR') == 'True'): + if os.getenv('ASTAR') == 'True': param_substitutions.update({'use_astar': 'True'}) param_substitutions.update( - {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} + ) param_substitutions.update( - {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')} + ) configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), - '-r', '-2.0', '-0.5', '0.0', '2.0', - '-e', 'True'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', + '-2.0', + '-0.5', + '0.0', + '2.0', + '-e', + 'True', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 2c10898a226..d09cc549195 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -35,9 +39,11 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') @@ -47,70 +53,91 @@ def generate_launch_description(): context = LaunchContext() param_substitutions = {} - if (os.getenv('ASTAR') == 'True'): + if os.getenv('ASTAR') == 'True': param_substitutions.update({'use_astar': 'True'}) param_substitutions.update( - {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} + ) param_substitutions.update( - {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')} + ) configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True', - 'use_composition': 'False'}.items()), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + 'use_composition': 'False', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), - '-r', '-200000.0', '-200000.0', '0.0', '2.0', - '-e', 'False'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', + '-200000.0', + '-200000.0', + '0.0', + '2.0', + '-e', + 'False', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 5bc8022766f..01a7d50bc96 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -22,7 +22,11 @@ from launch import LaunchDescription from launch import LaunchService -from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -35,70 +39,91 @@ def generate_launch_description(): map_yaml_file = os.getenv('TEST_MAP') world = os.getenv('TEST_WORLD') - bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML')) + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML'), + ) bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') # Replace the `use_astar` setting on the params file param_substitutions = { - 'planner_server.ros__parameters.GridBased.use_astar': 'False'} + 'planner_server.ros__parameters.GridBased.use_astar': 'False' + } configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, - convert_types=True) + convert_types=True, + ) context = LaunchContext() new_yaml = configured_params.perform(context) - return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - - # Launch gazebo server for simulation - ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '--minimal_comms', world], - output='screen'), - - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), - - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False', - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': new_yaml, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True'}.items()), - ]) + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + # Launch gazebo server for simulation + ExecuteProcess( + cmd=[ + 'gzserver', + '-s', + 'libgazebo_ros_init.so', + '--minimal_comms', + world, + ], + output='screen', + ), + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), - '-r', '-2.0', '-0.5', '100.0', '100.0'], + cmd=[ + os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-r', + '-2.0', + '-0.5', + '100.0', + '100.0', + ], name='tester_node', - output='screen') + output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index e4b3a9baeb6..765535c6e5b 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -37,26 +37,23 @@ class NavTester(Node): - - def __init__( - self, - initial_pose: Pose, - goal_pose: Pose, - namespace: str = '' - ): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, pose_qos) + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose @@ -158,7 +155,7 @@ def wait_for_node_active(self, node_name: str): self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' - while (state != 'active'): + while state != 'active': self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -166,7 +163,9 @@ def wait_for_node_active(self, node_name: str): state = future.result().current_state.label self.info_msg(f'Result of get_state: {state}') else: - self.error_msg(f'Exception while calling service: {future.exception()!r}') + self.error_msg( + f'Exception while calling service: {future.exception()!r}' + ) time.sleep(5) def shutdown(self): @@ -216,7 +215,7 @@ def wait_for_initial_pose(self): def run_all_tests(robot_tester): # set transforms to use_sim_time result = True - if (result): + if result: robot_tester.wait_for_node_active('amcl') robot_tester.wait_for_initial_pose() robot_tester.wait_for_node_active('bt_navigator') @@ -224,7 +223,7 @@ def run_all_tests(robot_tester): # Add more tests here if desired - if (result): + if result: robot_tester.info_msg('Test PASSED') else: robot_tester.error_msg('Test FAILED') @@ -252,10 +251,19 @@ def get_testers(args): init_x, init_y, final_x, final_y = args.robot[0] tester = NavTester( initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester, robot going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y + '.') + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + '.' + ) testers.append(tester) return testers @@ -266,13 +274,23 @@ def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments parser = argparse.ArgumentParser(description='System-level navigation tester node') group = parser.add_mutually_exclusive_group(required=True) - group.add_argument('-r', '--robot', action='append', nargs=4, - metavar=('init_x', 'init_y', 'final_x', 'final_y'), - help='The robot starting and final positions.') - group.add_argument('-rs', '--robots', action='append', nargs=5, - metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), - help="The robot's namespace and starting and final positions. " + - 'Repeating the argument for multiple robots is supported.') + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) + group.add_argument( + '-rs', + '--robots', + action='append', + nargs=5, + metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), + help="The robot's namespace and starting and final positions. " + + 'Repeating the argument for multiple robots is supported.', + ) args, unknown = parser.parse_known_args() diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index 28b20bd4c34..458779411b5 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -25,56 +25,70 @@ def generate_launch_description(): # Configuration parameters for the launch - launch_dir = os.path.join( - get_package_share_directory('nav2_bringup'), 'launch') + launch_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') map_yaml_file = os.path.join( - get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml') + get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml' + ) # Specify the actions start_tf_cmd_1 = Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']) + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + ) start_tf_cmd_2 = Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint']) + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint'], + ) start_tf_cmd_3 = Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']) + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + ) start_tf_cmd_4 = Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']) + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ) nav2_bringup = launch.actions.IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'bringup_launch.py')), - launch_arguments={'map': map_yaml_file, - 'use_sim_time': 'True', - 'use_composition': 'False', - 'autostart': 'False'}.items()) + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'use_composition': 'False', + 'autostart': 'False', + }.items(), + ) start_test = launch.actions.ExecuteProcess( cmd=[ os.path.join( get_package_prefix('nav2_system_tests'), - 'lib/nav2_system_tests/test_updown')], - cwd=[launch_dir], output='screen') + 'lib/nav2_system_tests/test_updown', + ) + ], + cwd=[launch_dir], + output='screen', + ) test_exit_event_handler = launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=start_test, - on_exit=launch.actions.EmitEvent(event=launch.events.Shutdown(reason='Done!')))) + on_exit=launch.actions.EmitEvent( + event=launch.events.Shutdown(reason='Done!') + ), + ) + ) # Compose the launch description diff --git a/nav2_system_tests/src/updown/updownresults.py b/nav2_system_tests/src/updown/updownresults.py index 604c3a25324..6025094be1d 100755 --- a/nav2_system_tests/src/updown/updownresults.py +++ b/nav2_system_tests/src/updown/updownresults.py @@ -63,7 +63,7 @@ def main(): shutdown_successful = True print('Number of tests: ', test_count) - print('Number of successes: ', test_count-fail_count) + print('Number of successes: ', test_count - fail_count) print('Number of successful bringups', successful_bringup_count) print('Number of successful shutdowns', successful_shutdown_count) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 30dfff574b0..7eb46457065 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -31,27 +31,30 @@ class WaypointFollowerTest(Node): - def __init__(self): super().__init__(node_name='nav2_waypoint_tester', namespace='') self.waypoints = None self.action_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') - self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, - 'initialpose', 10) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) self.initial_pose_received = False self.goal_handle = None self.action_result = None pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, pose_qos) - self.param_cli = self.create_client(SetParameters, - '/waypoint_follower/set_parameters') + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) + self.param_cli = self.create_client( + SetParameters, '/waypoint_follower/set_parameters' + ) def setInitialPose(self, pose): self.init_pose = PoseWithCovarianceStamped() @@ -120,8 +123,10 @@ def run(self, block, cancel): self.info_msg(f'Goal failed with status code: {status}') return False if len(self.action_result.missed_waypoints) > 0: - self.info_msg('Goal failed to process all waypoints,' - ' missed {0} wps.'.format(len(self.action_result.missed_waypoints))) + self.info_msg( + 'Goal failed to process all waypoints,' + ' missed {0} wps.'.format(len(self.action_result.missed_waypoints)) + ) return False self.info_msg('Goal succeeded!') @@ -132,8 +137,9 @@ def publishInitialPose(self): def setStopFailureParam(self, value): req = SetParameters.Request() - req.parameters = [Parameter('stop_on_failure', - Parameter.Type.BOOL, value).to_parameter_msg()] + req.parameters = [ + Parameter('stop_on_failure', Parameter.Type.BOOL, value).to_parameter_msg() + ] future = self.param_cli.call_async(req) rclpy.spin_until_future_complete(self, future) @@ -230,8 +236,10 @@ def main(argv=sys.argv[1:]): result = test.run(True, False) assert not result result = not result - assert test.action_result.missed_waypoints[0].error_code == \ - ComputePathToPose.Result().GOAL_OUTSIDE_MAP + assert ( + test.action_result.missed_waypoints[0].error_code + == ComputePathToPose.Result().GOAL_OUTSIDE_MAP + ) # stop on failure test with bogous waypoint test.setStopFailureParam(True) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index c509afb065c..f93eaa44d73 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -91,6 +91,8 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); + std::unique_lock lock(*(planner_->costmap_->getMutex())); + // Corner case of start and goal beeing on the same cell unsigned int mx_start, my_start, mx_goal, my_goal; if (!planner_->costmap_->worldToMap( diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 884b7b366ed..67cc87243d6 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -132,7 +132,7 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal) Iter lowest_it = begin; for (Iter it = ++begin; it != end; ++it) { auto comp = getCompareVal(*it); - if (comp < lowest) { + if (comp <= lowest) { lowest = comp; lowest_it = it; } diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 62201624a6d..d0bb65cf41e 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -142,11 +142,11 @@ std::string get_plugin_type_param( if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) { RCLCPP_FATAL( node->get_logger(), "Can not get 'plugin' param value for %s", plugin_name.c_str()); - exit(-1); + throw std::runtime_error("No 'plugin' param for param ns!"); } } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str()); - exit(-1); + throw std::runtime_error("No 'plugin' param for param ns!"); } return plugin_type; diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 26c64b1c7dc..3af965d4c68 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -548,6 +548,5 @@ int main(int argc, char ** argv) ::testing::InitGoogleTest(&argc, argv); auto result = RUN_ALL_TESTS(); rclcpp::shutdown(); - rclcpp::Rate(1).sleep(); return result; } diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index 9d6d6ba133b..ff85d2bd415 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -92,7 +92,7 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) auto node = std::make_shared("test_node"); node->declare_parameter("Foo.plugin", "bar"); ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); - ASSERT_EXIT(get_plugin_type_param(node, "Waldo"), ::testing::ExitedWithCode(255), ".*"); + EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error); } TEST(TestParamCopying, TestParamCopying) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index bc101fd7328..1c86659aae4 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -83,6 +83,14 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) throw std::runtime_error( "Negative values set of acceleration! These should be positive to speed up!"); } + if (min_velocities_[i] > 0.0) { + throw std::runtime_error( + "Positive values set of min_velocities! These should be negative!"); + } + if (max_velocities_[i] < 0.0) { + throw std::runtime_error( + "Negative values set of max_velocities! These should be positive!"); + } if (min_velocities_[i] > max_velocities_[i]) { throw std::runtime_error( "Min velocities are higher than max velocities!"); @@ -361,9 +369,29 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } if (name == "max_velocity") { - max_velocities_ = parameter.as_double_array(); + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] < 0.0) { + RCLCPP_WARN( + get_logger(), + "Negative values set of max_velocity! These should be positive!"); + result.successful = false; + } + } + if (result.successful) { + max_velocities_ = parameter.as_double_array(); + } } else if (name == "min_velocity") { - min_velocities_ = parameter.as_double_array(); + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] > 0.0) { + RCLCPP_WARN( + get_logger(), + "Positive values set of min_velocity! These should be negative!"); + result.successful = false; + } + } + if (result.successful) { + min_velocities_ = parameter.as_double_array(); + } } else if (name == "max_accel") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] < 0.0) { @@ -373,7 +401,9 @@ VelocitySmoother::dynamicParametersCallback(std::vector param result.successful = false; } } - max_accels_ = parameter.as_double_array(); + if (result.successful) { + max_accels_ = parameter.as_double_array(); + } } else if (name == "max_decel") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] > 0.0) { @@ -383,7 +413,9 @@ VelocitySmoother::dynamicParametersCallback(std::vector param result.successful = false; } } - max_decels_ = parameter.as_double_array(); + if (result.successful) { + max_decels_ = parameter.as_double_array(); + } } else if (name == "deadband_velocity") { deadband_velocities_ = parameter.as_double_array(); } diff --git a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp index baff10d6840..07c391fff59 100644 --- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp +++ b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp @@ -112,7 +112,7 @@ class VoxelGrid unsigned int marked_bits = *col >> 16; - // make sure the number of bits in each is below our thesholds + // make sure the number of bits in each is below our thresholds return !bitsBelowThreshold(marked_bits, marked_threshold); } @@ -146,7 +146,7 @@ class VoxelGrid unsigned int unknown_bits = uint16_t(*col >> 16) ^ uint16_t(*col); unsigned int marked_bits = *col >> 16; - // make sure the number of bits in each is below our thesholds + // make sure the number of bits in each is below our thresholds if (bitsBelowThreshold(unknown_bits, 1) && bitsBelowThreshold(marked_bits, 1)) { costmap[index] = 0; } @@ -392,7 +392,7 @@ class VoxelGrid unsigned int unknown_bits = uint16_t(*col >> 16) ^ uint16_t(*col); unsigned int marked_bits = *col >> 16; - // make sure the number of bits in each is below our thesholds + // make sure the number of bits in each is below our thresholds if (bitsBelowThreshold(marked_bits, marked_clear_threshold_)) { if (bitsBelowThreshold(unknown_bits, unknown_clear_threshold_)) { costmap_[offset] = free_cost_; diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 1d5405878bf..0ea7eda8eba 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -27,8 +27,6 @@ find_package(geographic_msgs REQUIRED) nav2_package() -link_libraries(stdc++fs) - include_directories( include ) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index 49b32e4aa4d..9c46fdf1f86 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -22,7 +22,7 @@ #define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM -#include +#include #include #include #include @@ -97,7 +97,7 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor // to ensure safety when accessing global var curr_frame_ std::mutex global_mutex_; // the taken photos will be saved under this directory - std::experimental::filesystem::path save_dir_; + std::filesystem::path save_dir_; // .png ? .jpg ? or some other well known format std::string image_format_; // the topic to subscribe in order capture a frame @@ -108,7 +108,7 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor sensor_msgs::msg::Image::SharedPtr curr_frame_msg_; // global logger rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; - // ros susbcriber to get camera image + // ros subscriber to get camera image rclcpp::Subscription::SharedPtr camera_image_subscriber_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp index 8a6cb74b96a..296f8f41de0 100644 --- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp @@ -61,14 +61,14 @@ void PhotoAtWaypoint::initialize( // get inputted save directory and make sure it exists, if not log and create it save_dir_ = save_dir_as_string; try { - if (!std::experimental::filesystem::exists(save_dir_)) { + if (!std::filesystem::exists(save_dir_)) { RCLCPP_WARN( logger_, "Provided save directory for photo at waypoint plugin does not exist," "provided directory is: %s, the directory will be created automatically.", save_dir_.c_str() ); - if (!std::experimental::filesystem::create_directory(save_dir_)) { + if (!std::filesystem::create_directory(save_dir_)) { RCLCPP_ERROR( logger_, "Failed to create directory!: %s required by photo at waypoint plugin, " @@ -110,10 +110,10 @@ bool PhotoAtWaypoint::processAtWaypoint( } try { // construct the full path to image filename - std::experimental::filesystem::path file_name = std::to_string( + std::filesystem::path file_name = std::to_string( curr_waypoint_index) + "_" + std::to_string(curr_pose.header.stamp.sec) + "." + image_format_; - std::experimental::filesystem::path full_path_image_path = save_dir_ / file_name; + std::filesystem::path full_path_image_path = save_dir_ / file_name; // save the taken photo at this waypoint to given directory std::lock_guard guard(global_mutex_); diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index f462a771a31..2bd895598ba 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -120,10 +120,10 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_logger(), "Created waypoint_task_executor : %s of type %s", waypoint_task_executor_id_.c_str(), waypoint_task_executor_type_.c_str()); waypoint_task_executor_->initialize(node, waypoint_task_executor_id_); - } catch (const pluginlib::PluginlibException & ex) { + } catch (const std::exception & e) { RCLCPP_FATAL( get_logger(), - "Failed to create waypoint_task_executor. Exception: %s", ex.what()); + "Failed to create waypoint_task_executor. Exception: %s", e.what()); } return nav2_util::CallbackReturn::SUCCESS; diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp index da0a0e99443..87d1b2b0c27 100644 --- a/nav2_waypoint_follower/test/test_task_executors.cpp +++ b/nav2_waypoint_follower/test/test_task_executors.cpp @@ -71,7 +71,7 @@ TEST(WaypointFollowerTest, InputAtWaypoint) auto pub = node->create_publisher("input_at_waypoint/input", 1); pub->on_activate(); auto publish_message = - [&, this]() -> void + [&]() -> void { rclcpp::Rate(5).sleep(); auto msg = std::make_unique(); @@ -117,7 +117,7 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) std::unique_lock lck(mtx, std::defer_lock); bool data_published = false; auto publish_message = - [&, this]() -> void + [&]() -> void { rclcpp::Rate(5).sleep(); auto msg = std::make_unique(); diff --git a/tools/bt2img.py b/tools/bt2img.py index 31480f19be3..b3c2b34eaf5 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -18,64 +18,66 @@ import argparse import xml.etree.ElementTree + import graphviz # pip3 install graphviz control_nodes = [ - "Fallback", - "Parallel", - "ReactiveFallback", - "ReactiveSequence", - "Sequence", - "SequenceStar", - "BlackboardCheckInt", - "BlackboardCheckDouble", - "BlackboardCheckString", - "ForceFailure", - "ForceSuccess", - "Inverter", - "Repeat", - "Subtree", - "Timeout", - "RecoveryNode", - "PipelineSequence", - "RoundRobin", - "Control", - ] + 'Fallback', + 'Parallel', + 'ReactiveFallback', + 'ReactiveSequence', + 'Sequence', + 'SequenceStar', + 'BlackboardCheckInt', + 'BlackboardCheckDouble', + 'BlackboardCheckString', + 'ForceFailure', + 'ForceSuccess', + 'Inverter', + 'Repeat', + 'Subtree', + 'Timeout', + 'RecoveryNode', + 'PipelineSequence', + 'RoundRobin', + 'Control', +] action_nodes = [ - "AlwaysFailure", - "AlwaysSuccess", - "SetBlackboard", - "ComputePathToPose", - "FollowPath", - "BackUp", - "Spin", - "Wait", - "ClearEntireCostmap", - "ReinitializeGlobalLocalization", - "Action", - ] + 'AlwaysFailure', + 'AlwaysSuccess', + 'SetBlackboard', + 'ComputePathToPose', + 'FollowPath', + 'BackUp', + 'Spin', + 'Wait', + 'ClearEntireCostmap', + 'ReinitializeGlobalLocalization', + 'Action', +] condition_nodes = [ - "IsStuck", - "GoalReached", - "initialPoseReceived", - "GoalUpdated", - "DistanceTraveled", - "TimeExpired", - "TransformAvailable", - "Condition", - ] + 'IsStuck', + 'GoalReached', + 'initialPoseReceived', + 'GoalUpdated', + 'DistanceTraveled', + 'TimeExpired', + 'TransformAvailable', + 'Condition', +] decorator_nodes = [ - "Decorator", - "RateController", - "DistanceController", - "SpeedController", + 'Decorator', + 'RateController', + 'DistanceController', + 'SpeedController', ] subtree_nodes = [ - "SubTree", + 'SubTree', ] global xml_tree + def main(): global xml_tree args = parse_command_line() @@ -93,27 +95,43 @@ def main(): args.save_dot.write(dot.source) dot.render(args.image_out, view=args.display) + def parse_command_line(): - parser = argparse.ArgumentParser(description='Convert a behavior tree XML file to an image') - parser.add_argument('--behavior_tree', required=True, - help='the behavior tree XML file to convert to an image') - parser.add_argument('--image_out', required=True, - help='The name of the output image file. Leave off the .png extension') - parser.add_argument('--display', action="store_true", - help='If specified, opens the image in the default viewer') - parser.add_argument('--save_dot', type=argparse.FileType('w'), - help='Saves the intermediate dot source to the specified file') - parser.add_argument('--legend', - help='Generate a legend image as well') + parser = argparse.ArgumentParser( + description='Convert a behavior tree XML file to an image' + ) + parser.add_argument( + '--behavior_tree', + required=True, + help='the behavior tree XML file to convert to an image', + ) + parser.add_argument( + '--image_out', + required=True, + help='The name of the output image file. Leave off the .png extension', + ) + parser.add_argument( + '--display', + action='store_true', + help='If specified, opens the image in the default viewer', + ) + parser.add_argument( + '--save_dot', + type=argparse.FileType('w'), + help='Saves the intermediate dot source to the specified file', + ) + parser.add_argument('--legend', help='Generate a legend image as well') return parser.parse_args() + def find_root_tree_name(xml_tree): return xml_tree.getroot().get('main_tree_to_execute') + def find_behavior_tree(xml_tree, tree_name): trees = xml_tree.findall('BehaviorTree') if len(trees) == 0: - raise RuntimeError("No behavior trees were found in the XML file") + raise RuntimeError('No behavior trees were found in the XML file') for tree in trees: if tree_name == tree.get('ID'): @@ -121,6 +139,7 @@ def find_behavior_tree(xml_tree, tree_name): raise RuntimeError(f'No behavior tree for name {tree_name} found in the XML file') + # Generate a dot description of the root of the behavior tree. def convert2dot(behavior_tree): dot = graphviz.Digraph() @@ -130,65 +149,85 @@ def convert2dot(behavior_tree): convert_subtree(dot, root, parent_dot_name) return dot + # Recursive function. We add the children to the dot file, and then recursively # call this function on the children. Nodes are given an ID that is the hash # of the node to ensure each is unique. def convert_subtree(dot, parent_node, parent_dot_name): - if parent_node.tag == "SubTree": + if parent_node.tag == 'SubTree': add_sub_tree(dot, parent_dot_name, parent_node) else: add_nodes(dot, parent_dot_name, parent_node) + def add_sub_tree(dot, parent_dot_name, parent_node): root_tree_name = parent_node.get('ID') dot.node(parent_dot_name, root_tree_name, shape='box') behavior_tree = find_behavior_tree(xml_tree, root_tree_name) convert_subtree(dot, behavior_tree, parent_dot_name) + def add_nodes(dot, parent_dot_name, parent_node): for node in list(parent_node): label = make_label(node) - dot.node(str(hash(node)), label, color=node_color(node.tag), style='filled', shape='box') + dot.node( + str(hash(node)), + label, + color=node_color(node.tag), + style='filled', + shape='box', + ) dot_name = str(hash(node)) dot.edge(parent_dot_name, dot_name) convert_subtree(dot, node, dot_name) + # The node label contains the: # type, the name if provided, and the parameters. def make_label(node): - label = '< ' - label += '' + label = "<
' + node.tag + '
" + label += "" name = node.get('name') if name: - label += '' + label += "" for (param_name, value) in node.items(): - label += '' + label += ( + "" + ) label += '
' + node.tag + '
' + name + '
' + name + '
' + param_name + '=' + value + '
' + param_name + '=' + value + '
>' return label -def node_color(type): - if type in control_nodes: - return "chartreuse4" - if type in action_nodes: - return "cornflowerblue" - if type in condition_nodes: - return "yellow2" - if type in decorator_nodes: - return "darkorange1" - if type in subtree_nodes: - return "darkorchid1" - #else it's unknown - return "grey" + +def node_color(node_type): + if node_type in control_nodes: + return 'chartreuse4' + if node_type in action_nodes: + return 'cornflowerblue' + if node_type in condition_nodes: + return 'yellow2' + if node_type in decorator_nodes: + return 'darkorange1' + if node_type in subtree_nodes: + return 'darkorchid1' + # else it's unknown + return 'grey' + # creates a legend which can be provided with the other images. def make_legend(): legend = graphviz.Digraph(graph_attr={'rankdir': 'LR'}) legend.attr(label='Legend') - legend.node('Unknown', shape='box', style='filled', color="grey") - legend.node('Action', 'Action Node', shape='box', style='filled', color="cornflowerblue") - legend.node('Condition', 'Condition Node', shape='box', style='filled', color="yellow2") - legend.node('Control', 'Control Node', shape='box', style='filled', color="chartreuse4") + legend.node('Unknown', shape='box', style='filled', color='grey') + legend.node( + 'Action', 'Action Node', shape='box', style='filled', color='cornflowerblue' + ) + legend.node( + 'Condition', 'Condition Node', shape='box', style='filled', color='yellow2' + ) + legend.node( + 'Control', 'Control Node', shape='box', style='filled', color='chartreuse4' + ) return legend diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 975416fcbd5..0ae0f883efe 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -13,22 +13,17 @@ # See the License for the specific language governing permissions and # limitations under the License. -from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -import rclpy -from ament_index_python.packages import get_package_share_directory - +import glob import math import os import pickle -import glob +from random import randint, seed, uniform import time -import numpy as np - -from random import seed -from random import randint -from random import uniform +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import numpy as np +import rclpy from transforms3d.euler import euler2quat @@ -39,7 +34,7 @@ def getPlannerResults(navigator, initial_pose, goal_pose, planners): if path is not None and path.error_code == 0: results.append(path) else: - print(planner, "planner failed to produce the path") + print(planner, 'planner failed to produce the path') return results return results @@ -49,14 +44,14 @@ def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): start.header.frame_id = 'map' start.header.stamp = time_stamp while True: - row = randint(side_buffer, costmap.shape[0]-side_buffer) - col = randint(side_buffer, costmap.shape[1]-side_buffer) + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) if costmap[row, col] < max_cost: - start.pose.position.x = col*res - start.pose.position.y = row*res + start.pose.position.x = col * res + start.pose.position.y = row * res - yaw = uniform(0, 1) * 2*math.pi + yaw = uniform(0, 1) * 2 * math.pi quad = euler2quat(0.0, 0.0, yaw) start.pose.orientation.w = quad[0] start.pose.orientation.x = quad[1] @@ -71,13 +66,13 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): goal.header.frame_id = 'map' goal.header.stamp = time_stamp while True: - row = randint(side_buffer, costmap.shape[0]-side_buffer) - col = randint(side_buffer, costmap.shape[1]-side_buffer) + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) start_x = start.pose.position.x start_y = start.pose.position.y - goal_x = col*res - goal_y = row*res + goal_x = col * res + goal_y = row * res x_diff = goal_x - start_x y_diff = goal_y - start_y dist = math.sqrt(x_diff ** 2 + y_diff ** 2) @@ -86,7 +81,7 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): goal.pose.position.x = goal_x goal.pose.position.y = goal_y - yaw = uniform(0, 1) * 2*math.pi + yaw = uniform(0, 1) * 2 * math.pi quad = euler2quat(0.0, 0.0, yaw) goal.pose.orientation.w = quad[0] goal.pose.orientation.x = quad[1] @@ -111,7 +106,7 @@ def main(): costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] + planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() @@ -122,19 +117,19 @@ def main(): res = costmap_msg.metadata.resolution i = 0 while len(results) != random_pairs: - print("Cycle: ", i, "out of: ", random_pairs) + print('Cycle: ', i, 'out of: ', random_pairs) start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) - print("Start", start) - print("Goal", goal) + print('Start', start) + print('Goal', goal) result = getPlannerResults(navigator, start, goal, planners) if len(result) == len(planners): results.append(result) i = i + 1 else: - print("One of the planners was invalid") + print('One of the planners was invalid') - print("Write Results...") + print('Write Results...') with open(os.getcwd() + '/results.pickle', 'wb+') as f: pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) @@ -143,7 +138,7 @@ def main(): with open(os.getcwd() + '/planners.pickle', 'wb+') as f: pickle.dump(planners, f, pickle.HIGHEST_PROTOCOL) - print("Write Complete") + print('Write Complete') exit(0) diff --git a/tools/planner_benchmarking/planning_benchmark_bringup.py b/tools/planner_benchmarking/planning_benchmark_bringup.py index d6589e89968..506ba544183 100644 --- a/tools/planner_benchmarking/planning_benchmark_bringup.py +++ b/tools/planner_benchmarking/planning_benchmark_bringup.py @@ -14,60 +14,69 @@ import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): nav2_bringup_dir = get_package_share_directory('nav2_bringup') - config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') + config = os.path.join( + get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml' + ) map_file = os.path.join(nav2_bringup_dir, 'maps', 'turtlebot3_world.yaml') lifecycle_nodes = ['map_server', 'planner_server'] - return LaunchDescription([ - Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - output='screen', - parameters=[{'use_sim_time': True}, - {'yaml_filename': map_file}, - {'topic_name': "map"}]), - - Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - parameters=[config]), - - Node( - package = 'tf2_ros', - executable = 'static_transform_publisher', - output = 'screen', - arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]), - - Node( - package = 'tf2_ros', - executable = 'static_transform_publisher', - output = 'screen', - arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - parameters=[{'use_sim_time': True}, - {'autostart': True}, - {'node_names': lifecycle_nodes}]), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) - - ]) + return LaunchDescription( + [ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': 'map'}, + ], + ), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[config], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'map'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'odom'], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}, + ], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ), + ] + ) diff --git a/tools/planner_benchmarking/process_data.py b/tools/planner_benchmarking/process_data.py index 50be77cc488..299997774db 100644 --- a/tools/planner_benchmarking/process_data.py +++ b/tools/planner_benchmarking/process_data.py @@ -13,15 +13,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -import numpy as np import math - import os -from ament_index_python.packages import get_package_share_directory import pickle -import seaborn as sns import matplotlib.pylab as plt +import numpy as np +import seaborn as sns from tabulate import tabulate @@ -37,7 +35,7 @@ def getTimes(results): times = [] for result in results: for time in result: - times.append(time.planning_time.nanosec/1e09 + time.planning_time.sec) + times.append(time.planning_time.nanosec / 1e09 + time.planning_time.sec) return times @@ -47,8 +45,8 @@ def getMapCoordsFromPaths(paths, resolution): x = [] y = [] for pose in path.poses: - x.append(pose.pose.position.x/resolution) - y.append(pose.pose.position.y/resolution) + x.append(pose.pose.position.x / resolution) + y.append(pose.pose.position.y / resolution) coords.append(x) coords.append(y) return coords @@ -61,7 +59,9 @@ def getPathLength(path): for i in range(1, len(path.poses)): x_curr = path.poses[i].pose.position.x y_curr = path.poses[i].pose.position.y - path_length = path_length + math.sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) + path_length = path_length + math.sqrt( + (x_curr - x_prev) ** 2 + (y_curr - y_prev) ** 2 + ) x_prev = x_curr y_prev = y_curr return path_length @@ -76,7 +76,7 @@ def plotResults(costmap, paths): plt.figure(3) ax = sns.heatmap(data, cmap='Greys', cbar=False) for i in range(0, len(coords), 2): - ax.plot(coords[i], coords[i+1], linewidth=0.7) + ax.plot(coords[i], coords[i + 1], linewidth=0.7) plt.axis('off') ax.set_aspect('equal', 'box') plt.show() @@ -95,8 +95,8 @@ def averagePathCost(paths, costmap, num_of_planners): for i in range(0, len(coords), 2): costs = [] for j in range(len(coords[i])): - costs.append(data[math.floor(coords[i+1][j])][math.floor(coords[i][j])]) - average_path_costs[k % num_of_planners].append(sum(costs)/len(costs)) + costs.append(data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])]) + average_path_costs[k % num_of_planners].append(sum(costs) / len(costs)) k += 1 return average_path_costs @@ -115,7 +115,7 @@ def maxPathCost(paths, costmap, num_of_planners): for i in range(0, len(coords), 2): max_cost = 0 for j in range(len(coords[i])): - cost = data[math.floor(coords[i+1][j])][math.floor(coords[i][j])] + cost = data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])] if max_cost < cost: max_cost = cost max_path_costs[k % num_of_planners].append(max_cost) @@ -126,7 +126,7 @@ def maxPathCost(paths, costmap, num_of_planners): def main(): - print("Read data") + print('Read data') with open(os.getcwd() + '/results.pickle', 'rb') as f: results = pickle.load(f) @@ -144,12 +144,12 @@ def main(): path_lengths = np.asarray(path_lengths) total_paths = len(paths) - path_lengths.resize((int(total_paths/len(planners)), len(planners))) + path_lengths.resize((int(total_paths / len(planners)), len(planners))) path_lengths = path_lengths.transpose() times = getTimes(results) times = np.asarray(times) - times.resize((int(total_paths/len(planners)), len(planners))) + times.resize((int(total_paths / len(planners)), len(planners))) times = np.transpose(times) # Costs @@ -157,12 +157,26 @@ def main(): max_path_costs = np.asarray(maxPathCost(paths, costmap, len(planners))) # Generate table - planner_table = [['Planner', 'Average path length (m)', 'Average Time (s)', - 'Average cost', 'Max cost']] + planner_table = [ + [ + 'Planner', + 'Average path length (m)', + 'Average Time (s)', + 'Average cost', + 'Max cost', + ] + ] for i in range(0, len(planners)): - planner_table.append([planners[i], np.average(path_lengths[i]), np.average(times[i]), - np.average(average_path_costs[i]), np.average(max_path_costs[i])]) + planner_table.append( + [ + planners[i], + np.average(path_lengths[i]), + np.average(times[i]), + np.average(average_path_costs[i]), + np.average(max_path_costs[i]), + ] + ) # Visualize results print(tabulate(planner_table)) diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py index fac2b19a959..fa01be9754f 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -14,31 +14,29 @@ # See the License for the specific language governing permissions and # limitations under the License. -from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -import rclpy - import math import os import pickle -import numpy as np - -from random import seed -from random import randint -from random import uniform +from random import randint, seed, uniform +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import numpy as np +import rclpy from transforms3d.euler import euler2quat # Note: Map origin is assumed to be (0,0) + def getPlannerResults(navigator, initial_pose, goal_pose, planner): result = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True) if result is None or result.error_code != 0: - print(planner, "planner failed to produce the path") + print(planner, 'planner failed to produce the path') return None return result + def getSmootherResults(navigator, path, smoothers): smoothed_results = [] for smoother in smoothers: @@ -46,23 +44,24 @@ def getSmootherResults(navigator, path, smoothers): if smoothed_result is not None: smoothed_results.append(smoothed_result) else: - print(smoother, "failed to smooth the path") + print(smoother, 'failed to smooth the path') return None return smoothed_results + def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): start = PoseStamped() start.header.frame_id = 'map' start.header.stamp = time_stamp while True: - row = randint(side_buffer, costmap.shape[0]-side_buffer) - col = randint(side_buffer, costmap.shape[1]-side_buffer) + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) if costmap[row, col] < max_cost: - start.pose.position.x = col*res - start.pose.position.y = row*res + start.pose.position.x = col * res + start.pose.position.y = row * res - yaw = uniform(0, 1) * 2*math.pi + yaw = uniform(0, 1) * 2 * math.pi quad = euler2quat(0.0, 0.0, yaw) start.pose.orientation.w = quad[0] start.pose.orientation.x = quad[1] @@ -71,18 +70,19 @@ def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): break return start + def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): goal = PoseStamped() goal.header.frame_id = 'map' goal.header.stamp = time_stamp while True: - row = randint(side_buffer, costmap.shape[0]-side_buffer) - col = randint(side_buffer, costmap.shape[1]-side_buffer) + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) start_x = start.pose.position.x start_y = start.pose.position.y - goal_x = col*res - goal_y = row*res + goal_x = col * res + goal_y = row * res x_diff = goal_x - start_x y_diff = goal_y - start_y dist = math.sqrt(x_diff ** 2 + y_diff ** 2) @@ -91,7 +91,7 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): goal.pose.position.x = goal_x goal.pose.position.y = goal_y - yaw = uniform(0, 1) * 2*math.pi + yaw = uniform(0, 1) * 2 * math.pi quad = euler2quat(0.0, 0.0, yaw) goal.pose.orientation.w = quad[0] goal.pose.orientation.x = quad[1] @@ -100,13 +100,14 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): break return goal + def main(): rclpy.init() navigator = BasicNavigator() # Wait for planner and smoother to fully activate - print("Waiting for planner and smoother servers to activate") + print('Waiting for planner and smoother servers to activate') navigator.waitUntilNav2Active('smoother_server', 'planner_server') # Get the costmap for start/goal validation @@ -126,20 +127,20 @@ def main(): i = 0 res = costmap_msg.metadata.resolution while i < random_pairs: - print("Cycle: ", i, "out of: ", random_pairs) + print('Cycle: ', i, 'out of: ', random_pairs) start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) - print("Start", start) - print("Goal", goal) + print('Start', start) + print('Goal', goal) result = getPlannerResults(navigator, start, goal, planner) if result is not None: smoothed_results = getSmootherResults(navigator, result.path, smoothers) if smoothed_results is not None: - results.append(result) - results.append(smoothed_results) - i += 1 + results.append(result) + results.append(smoothed_results) + i += 1 - print("Write Results...") + print('Write Results...') benchmark_dir = os.getcwd() with open(os.path.join(benchmark_dir, 'results.pickle'), 'wb') as f: pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) @@ -150,7 +151,7 @@ def main(): smoothers.insert(0, planner) with open(os.path.join(benchmark_dir, 'methods.pickle'), 'wb') as f: pickle.dump(smoothers, f, pickle.HIGHEST_PROTOCOL) - print("Write Complete") + print('Write Complete') exit(0) diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py index 43392833bda..4a422bc0037 100644 --- a/tools/smoother_benchmarking/process_data.py +++ b/tools/smoother_benchmarking/process_data.py @@ -15,14 +15,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -import numpy as np import math - import os import pickle -import seaborn as sns import matplotlib.pylab as plt +import numpy as np +import seaborn as sns from tabulate import tabulate @@ -44,11 +43,16 @@ def getTimes(results): for i in range(len(results)): if (i % 2) == 0: # Append non-smoothed time - times.append(results[i].planning_time.nanosec/1e09 + results[i].planning_time.sec) + times.append( + results[i].planning_time.nanosec / 1e09 + results[i].planning_time.sec + ) else: # Append smoothed times array for result in results[i]: - times.append(result.smoothing_duration.nanosec/1e09 + result.smoothing_duration.sec) + times.append( + result.smoothing_duration.nanosec / 1e09 + + result.smoothing_duration.sec + ) return times @@ -58,8 +62,8 @@ def getMapCoordsFromPaths(paths, resolution): x = [] y = [] for pose in path.poses: - x.append(pose.pose.position.x/resolution) - y.append(pose.pose.position.y/resolution) + x.append(pose.pose.position.x / resolution) + y.append(pose.pose.position.y / resolution) coords.append(x) coords.append(y) return coords @@ -72,11 +76,14 @@ def getPathLength(path): for i in range(1, len(path.poses)): x_curr = path.poses[i].pose.position.x y_curr = path.poses[i].pose.position.y - path_length = path_length + math.sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) + path_length = path_length + math.sqrt( + (x_curr - x_prev) ** 2 + (y_curr - y_prev) ** 2 + ) x_prev = x_curr y_prev = y_curr return path_length + # Path smoothness calculations def getSmoothness(pt_prev, pt, pt_next): d1 = pt - pt_prev @@ -84,6 +91,7 @@ def getSmoothness(pt_prev, pt, pt_next): delta = d2 - d1 return np.linalg.norm(delta) + def getPathSmoothnesses(paths): smoothnesses = [] pm0 = np.array([0.0, 0.0]) @@ -94,14 +102,15 @@ def getPathSmoothnesses(paths): for i in range(2, len(path.poses)): pm0[0] = path.poses[i].pose.position.x pm0[1] = path.poses[i].pose.position.y - pm1[0] = path.poses[i-1].pose.position.x - pm1[1] = path.poses[i-1].pose.position.y - pm2[0] = path.poses[i-2].pose.position.x - pm2[1] = path.poses[i-2].pose.position.y + pm1[0] = path.poses[i - 1].pose.position.x + pm1[1] = path.poses[i - 1].pose.position.y + pm2[0] = path.poses[i - 2].pose.position.x + pm2[1] = path.poses[i - 2].pose.position.y smoothness += getSmoothness(pm2, pm1, pm0) smoothnesses.append(smoothness) return smoothnesses + # Curvature calculations def arcCenter(pt_prev, pt, pt_next): cusp_thresh = -0.7 @@ -132,9 +141,12 @@ def arcCenter(pt_prev, pt, pt_next): n2 = (-d2[1], d2[0]) det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0] det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0] - center = np.array([(det1 * n2[0] - det2 * n1[0]) / det, (det1 * n2[1] - det2 * n1[1]) / det]) + center = np.array( + [(det1 * n2[0] - det2 * n1[0]) / det, (det1 * n2[1] - det2 * n1[1]) / det] + ) return center + def getPathCurvatures(paths): curvatures = [] pm0 = np.array([0.0, 0.0]) @@ -145,17 +157,18 @@ def getPathCurvatures(paths): for i in range(2, len(path.poses)): pm0[0] = path.poses[i].pose.position.x pm0[1] = path.poses[i].pose.position.y - pm1[0] = path.poses[i-1].pose.position.x - pm1[1] = path.poses[i-1].pose.position.y - pm2[0] = path.poses[i-2].pose.position.x - pm2[1] = path.poses[i-2].pose.position.y + pm1[0] = path.poses[i - 1].pose.position.x + pm1[1] = path.poses[i - 1].pose.position.y + pm2[0] = path.poses[i - 2].pose.position.x + pm2[1] = path.poses[i - 2].pose.position.y center = arcCenter(pm2, pm1, pm0) if center[0] != float('inf'): - turning_rad = np.linalg.norm(pm1 - center); - radiuses.append(turning_rad) + turning_rad = np.linalg.norm(pm1 - center) + radiuses.append(turning_rad) curvatures.append(np.average(radiuses)) return curvatures + def plotResults(costmap, paths): coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) data = np.asarray(costmap.data) @@ -165,7 +178,7 @@ def plotResults(costmap, paths): plt.figure(3) ax = sns.heatmap(data, cmap='Greys', cbar=False) for i in range(0, len(coords), 2): - ax.plot(coords[i], coords[i+1], linewidth=0.7) + ax.plot(coords[i], coords[i + 1], linewidth=0.7) plt.axis('off') ax.set_aspect('equal', 'box') plt.show() @@ -184,8 +197,8 @@ def averagePathCost(paths, costmap, num_of_planners): for i in range(0, len(coords), 2): costs = [] for j in range(len(coords[i])): - costs.append(data[math.floor(coords[i+1][j])][math.floor(coords[i][j])]) - average_path_costs[k % num_of_planners].append(sum(costs)/len(costs)) + costs.append(data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])]) + average_path_costs[k % num_of_planners].append(sum(costs) / len(costs)) k += 1 return average_path_costs @@ -204,7 +217,7 @@ def maxPathCost(paths, costmap, num_of_planners): for i in range(0, len(coords), 2): max_cost = 0 for j in range(len(coords[i])): - cost = data[math.floor(coords[i+1][j])][math.floor(coords[i][j])] + cost = data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])] if max_cost < cost: max_cost = cost max_path_costs[k % num_of_planners].append(max_cost) @@ -216,7 +229,7 @@ def maxPathCost(paths, costmap, num_of_planners): def main(): # Read the data benchmark_dir = os.getcwd() - print("Read data") + print('Read data') with open(os.path.join(benchmark_dir, 'results.pickle'), 'rb') as f: results = pickle.load(f) @@ -239,14 +252,14 @@ def main(): total_paths = len(paths) # [planner, smoothers] path lenghth in a row - path_lengths.resize((int(total_paths/methods_num), methods_num)) + path_lengths.resize((int(total_paths / methods_num), methods_num)) # [planner, smoothers] path length in a column path_lengths = path_lengths.transpose() # Times times = getTimes(results) times = np.asarray(times) - times.resize((int(total_paths/methods_num), methods_num)) + times.resize((int(total_paths / methods_num), methods_num)) times = np.transpose(times) # Costs @@ -256,40 +269,52 @@ def main(): # Smoothness smoothnesses = getPathSmoothnesses(paths) smoothnesses = np.asarray(smoothnesses) - smoothnesses.resize((int(total_paths/methods_num), methods_num)) + smoothnesses.resize((int(total_paths / methods_num), methods_num)) smoothnesses = np.transpose(smoothnesses) # Curvatures curvatures = getPathCurvatures(paths) curvatures = np.asarray(curvatures) - curvatures.resize((int(total_paths/methods_num), methods_num)) + curvatures.resize((int(total_paths / methods_num), methods_num)) curvatures = np.transpose(curvatures) # Generate table - planner_table = [['Planner', - 'Time (s)', - 'Path length (m)', - 'Average cost', - 'Max cost', - 'Path smoothness (x100)', - 'Average turning rad (m)']] + planner_table = [ + [ + 'Planner', + 'Time (s)', + 'Path length (m)', + 'Average cost', + 'Max cost', + 'Path smoothness (x100)', + 'Average turning rad (m)', + ] + ] # for path planner - planner_table.append([planner, - np.average(times[0]), - np.average(path_lengths[0]), - np.average(average_path_costs[0]), - np.average(max_path_costs[0]), - np.average(smoothnesses[0]) * 100, - np.average(curvatures[0])]) + planner_table.append( + [ + planner, + np.average(times[0]), + np.average(path_lengths[0]), + np.average(average_path_costs[0]), + np.average(max_path_costs[0]), + np.average(smoothnesses[0]) * 100, + np.average(curvatures[0]), + ] + ) # for path smoothers for i in range(1, methods_num): - planner_table.append([smoothers[i-1], - np.average(times[i]), - np.average(path_lengths[i]), - np.average(average_path_costs[i]), - np.average(max_path_costs[i]), - np.average(smoothnesses[i]) * 100, - np.average(curvatures[i])]) + planner_table.append( + [ + smoothers[i - 1], + np.average(times[i]), + np.average(path_lengths[i]), + np.average(average_path_costs[i]), + np.average(max_path_costs[i]), + np.average(smoothnesses[i]) * 100, + np.average(curvatures[i]), + ] + ) # Visualize results print(tabulate(planner_table)) diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py index 9b0ed744ded..4d60e17e64a 100644 --- a/tools/smoother_benchmarking/smoother_benchmark_bringup.py +++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py @@ -14,73 +14,87 @@ import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): nav2_bringup_dir = get_package_share_directory('nav2_bringup') benchmark_dir = os.getcwd() metrics_py = os.path.join(benchmark_dir, 'metrics.py') - config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') + config = os.path.join( + get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml' + ) map_file = os.path.join(benchmark_dir, 'maps', 'smoothers_world.yaml') lifecycle_nodes = ['map_server', 'planner_server', 'smoother_server'] static_transform_one = Node( - package = 'tf2_ros', - executable = 'static_transform_publisher', - output = 'screen', - arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]) + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'map'], + ) static_transform_two = Node( - package = 'tf2_ros', - executable = 'static_transform_publisher', - output = 'screen', - arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]) + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'odom'], + ) start_map_server_cmd = Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', - parameters=[{'use_sim_time': True}, - {'yaml_filename': map_file}, - {'topic_name': "map"}]) + parameters=[ + {'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': 'map'}, + ], + ) start_planner_server_cmd = Node( package='nav2_planner', executable='planner_server', name='planner_server', output='screen', - parameters=[config]) + parameters=[config], + ) start_smoother_server_cmd = Node( package='nav2_smoother', executable='smoother_server', name='smoother_server', output='screen', - parameters=[config]) + parameters=[config], + ) start_lifecycle_manager_cmd = Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager', output='screen', - parameters=[{'use_sim_time': True}, - {'autostart': True}, - {'node_names': lifecycle_nodes}]) + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}, + ], + ) rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), - launch_arguments={'namespace': '', - 'use_namespace': 'False'}.items()) + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ) metrics_cmd = ExecuteProcess( - cmd=['python3', '-u', metrics_py], - cwd=[benchmark_dir], output='screen') + cmd=['python3', '-u', metrics_py], cwd=[benchmark_dir], output='screen' + ) ld = LaunchDescription() ld.add_action(static_transform_one)