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 5e316835ae9..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 @@ -268,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; } } 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..026fdb4cbc8 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 @@ -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; 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/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/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 7bfc003cf1c..a5ee96af5e1 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( 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_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 89d3e136cea..ad4c7e091ed 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -25,7 +25,7 @@ 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(): diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index fc1499a0f27..c508e1d285f 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -14,6 +14,7 @@ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 7080f580edb..fde5b6bda72 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -19,7 +19,6 @@ navigation2 nav2_common slam_toolbox - turtlebot3_gazebo ament_lint_common ament_lint_auto 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..615ce5ef64b 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 diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 51cafe71dd0..b902cc4ea83 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/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 30d58d53bc1..865a3c023b9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -69,8 +69,9 @@ 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; @@ -80,6 +81,18 @@ class Source */ 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. 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/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 6a5aca7dfec..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(); @@ -301,15 +307,51 @@ 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_) { if (source->getEnabled()) { - source->getData(curr_time, collision_points); + 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()) { diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index a1395a7b70e..481cafbb654 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,54 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) // Points array collected from different data sources in a robot base frame std::vector 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()) { - source->getData(curr_time, collision_points); + 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; + } } } - // 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; + 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()) { @@ -414,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 @@ -511,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(), diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index f23f5e0bfea..fb3fca201d0 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -65,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; @@ -88,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 @@ -99,7 +99,7 @@ void PointCloud::getData( data_->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } @@ -118,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/range.cpp b/nav2_collision_monitor/src/range.cpp index 56c0a4750fc..45540a9c4d0 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -65,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 @@ -84,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; @@ -97,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 @@ -108,7 +108,7 @@ void Range::getData( data_->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } @@ -142,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 603e2f04e69..45f1e62d701 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -64,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; @@ -87,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 @@ -98,7 +98,7 @@ void Scan::getData( data_->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { - return; + return false; } } @@ -118,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 8dcf0c85e2d..a27876f05b3 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -69,6 +69,12 @@ void Source::getCommonParameters(std::string & source_topic) 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( @@ -78,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. " @@ -95,6 +101,16 @@ 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) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index eab3e177548..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" @@ -49,6 +50,7 @@ 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}; @@ -144,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 @@ -156,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() @@ -172,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() @@ -179,6 +192,7 @@ Tester::~Tester() scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + collision_points_marker_sub_.reset(); cd_.reset(); } @@ -196,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( @@ -678,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 e66e33e5cb4..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"}; @@ -164,10 +166,12 @@ class Tester : public ::testing::Test 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_; @@ -190,6 +194,10 @@ class Tester : public ::testing::Test 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 @@ -217,6 +225,9 @@ 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( @@ -235,6 +246,7 @@ Tester::~Tester() cmd_vel_out_sub_.reset(); action_state_sub_.reset(); + collision_points_marker_sub_.reset(); cm_.reset(); } @@ -554,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(); @@ -623,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; @@ -633,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(); @@ -877,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)); @@ -921,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(); @@ -1170,6 +1283,35 @@ TEST_F(Tester, testSourcesNotSet) 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_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index eb32a73ab4e..0386c06fc7d 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -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(); 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/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 581db0065c5..c35933c9083 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -129,6 +129,7 @@ void Costmap2DROS::init() declare_parameter("rolling_window", rclcpp::ParameterValue(false)); declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + declare_parameter("initial_transform_timeout", rclcpp::ParameterValue(60.0)); declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); @@ -265,13 +266,6 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - footprint_pub_->on_activate(); - costmap_publisher_->on_activate(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub->on_activate(); - } - // First, make sure that the transform between the robot base frame // and the global frame is available @@ -279,6 +273,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Checking transform"); rclcpp::Rate r(2); + const auto initial_transform_timeout = rclcpp::Duration::from_seconds( + initial_transform_timeout_); + const auto initial_transform_timeout_point = now() + initial_transform_timeout; while (rclcpp::ok() && !tf_buffer_->canTransform( global_frame_, robot_base_frame_, tf2::TimePointZero, &tf_error)) @@ -288,12 +285,29 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) " to become available, tf error: %s", robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); + // Check timeout + if (now() > initial_transform_timeout_point) { + RCLCPP_ERROR( + get_logger(), "Failed to activate %s because transform from %s to %s did not become available before timeout", + get_name(), robot_base_frame_.c_str(), global_frame_.c_str()); + + return nav2_util::CallbackReturn::FAILURE; + } + // The error string will accumulate and errors will typically be the same, so the last // will do for the warning above. Reset the string here to avoid accumulation tf_error.clear(); r.sleep(); } + // Activate publishers + footprint_pub_->on_activate(); + costmap_publisher_->on_activate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_activate(); + } + // Create a thread to handle updating the map stopped_ = true; // to active plugins stop_updates_ = false; @@ -386,6 +400,7 @@ Costmap2DROS::getParameters() get_parameter("rolling_window", rolling_window_); get_parameter("track_unknown_space", track_unknown_space_); get_parameter("transform_tolerance", transform_tolerance_); + get_parameter("initial_transform_timeout", initial_transform_timeout_); get_parameter("update_frequency", map_update_frequency_); get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); diff --git a/nav2_costmap_2d/test/module_tests.cpp b/nav2_costmap_2d/test/module_tests.cpp index 989d81fe721..7a5fe811e41 100644 --- a/nav2_costmap_2d/test/module_tests.cpp +++ b/nav2_costmap_2d/test/module_tests.cpp @@ -1123,7 +1123,7 @@ TEST(costmap, testTrickyPropagation) { // Add a dynamic obstacle pcl::PointCloud c2; c2.points.resize(3); - // Dynamic obstacle that raytaces. + // Dynamic obstacle that raytraces. c2.points[0].x = 7.0; c2.points[0].y = 8.0; c2.points[0].z = 1.0; @@ -1167,7 +1167,7 @@ TEST(costmap, testTrickyPropagation) { pcl::PointCloud c; c.points.resize(1); - // Dynamic obstacle that raytaces the one at (3.0, 4.0). + // Dynamic obstacle that raytraces the one at (3.0, 4.0). c.points[0].x = 4.0; c.points[0].y = 5.0; c.points[0].z = 1.0; diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 19c2edb56fe..b9480fdffab 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -55,3 +55,8 @@ ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_p target_link_libraries(denoise_layer_test nav2_costmap_2d_core layers ) + +ament_add_gtest(lifecycle_test lifecycle_test.cpp) +target_link_libraries(lifecycle_test + nav2_costmap_2d_core +) \ No newline at end of file diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index 31bdfc025bc..fa7fed4e107 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -703,7 +703,7 @@ void TestNode::reset() TEST_F(TestNode, testBinaryState) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("map", 10.0)); @@ -718,7 +718,7 @@ TEST_F(TestNode, testBinaryState) TEST_F(TestNode, testBinaryStateScaled) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 100.0, -1.0); ASSERT_TRUE(createBinaryFilter("map", 35.0)); @@ -733,7 +733,7 @@ TEST_F(TestNode, testBinaryStateScaled) TEST_F(TestNode, testInvertedBinaryState) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); setDefaultState(true); @@ -749,7 +749,7 @@ TEST_F(TestNode, testInvertedBinaryState) TEST_F(TestNode, testOutOfBounds) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("map", 10.0)); @@ -764,7 +764,7 @@ TEST_F(TestNode, testOutOfBounds) TEST_F(TestNode, testInfoRePublish) { - // Initilize test system + // Initialize test system createMaps("map"); // Publish Info with incorrect dummy mask topic publishMaps(nav2_costmap_2d::BINARY_FILTER, "dummy_topic", 0.0, 1.0); @@ -805,7 +805,7 @@ TEST_F(TestNode, testMaskRePublish) TEST_F(TestNode, testIncorrectFilterType) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(INCORRECT_TYPE, MASK_TOPIC, 0.0, 1.0); ASSERT_FALSE(createBinaryFilter("map", 10.0)); @@ -817,7 +817,7 @@ TEST_F(TestNode, testIncorrectFilterType) TEST_F(TestNode, testDifferentFrame) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("odom", 10.0)); @@ -833,7 +833,7 @@ TEST_F(TestNode, testDifferentFrame) TEST_F(TestNode, testIncorrectFrame) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("odom", 10.0)); @@ -849,7 +849,7 @@ TEST_F(TestNode, testIncorrectFrame) TEST_F(TestNode, testResetState) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); ASSERT_TRUE(createBinaryFilter("map", 10.0)); diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index 304b423f5c0..3f236dd4d49 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -341,7 +341,7 @@ void TestNode::reset() TEST_F(TestNode, testFreeMasterLethalKeepout) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); publishMaps(); createKeepoutFilter("map"); @@ -356,7 +356,7 @@ TEST_F(TestNode, testFreeMasterLethalKeepout) TEST_F(TestNode, testUnknownMasterNonLethalKeepout) { - // Initilize test system + // Initialize test system createMaps( nav2_costmap_2d::NO_INFORMATION, (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE) / 2, @@ -376,7 +376,7 @@ TEST_F(TestNode, testUnknownMasterNonLethalKeepout) TEST_F(TestNode, testFreeKeepout) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_FREE, "map"); publishMaps(); createKeepoutFilter("map"); @@ -395,7 +395,7 @@ TEST_F(TestNode, testFreeKeepout) TEST_F(TestNode, testUnknownKeepout) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_UNKNOWN, "map"); publishMaps(); createKeepoutFilter("map"); @@ -414,7 +414,7 @@ TEST_F(TestNode, testUnknownKeepout) TEST_F(TestNode, testInfoRePublish) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); publishMaps(); createKeepoutFilter("map"); @@ -433,7 +433,7 @@ TEST_F(TestNode, testInfoRePublish) TEST_F(TestNode, testMaskRePublish) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); publishMaps(); createKeepoutFilter("map"); @@ -451,7 +451,7 @@ TEST_F(TestNode, testMaskRePublish) TEST_F(TestNode, testDifferentFrames) { - // Initilize test system + // Initialize test system createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); publishMaps(); createKeepoutFilter("odom"); diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp new file mode 100644 index 00000000000..8a657d81ad1 --- /dev/null +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -0,0 +1,54 @@ +// 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 + +#include "gtest/gtest.h" + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "rclcpp/rclcpp.hpp" +#include "lifecycle_msgs/msg/state.hpp" + + +TEST(LifecylceTest, CheckInitialTfTimeout) { + rclcpp::init(0, nullptr); + + auto costmap = std::make_shared("test_costmap"); + costmap->set_parameter({"initial_transform_timeout", 0.0}); + + std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; + + { + const auto state_after_configure = costmap->configure(); + ASSERT_EQ(state_after_configure.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // Without providing the transform from global to robot base the activation should fail + // and the costmap should transition into the inactive state. + const auto state_after_activate = costmap->activate(); + ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + + // Set a dummy transform from global to robot base + geometry_msgs::msg::TransformStamped transform_global_to_robot{}; + transform_global_to_robot.header.frame_id = costmap->getGlobalFrameID(); + transform_global_to_robot.child_frame_id = costmap->getBaseFrameID(); + costmap->getTfBuffer()->setTransform(transform_global_to_robot, "test", true); + // Now the costmap should successful transition into the active state + { + const auto state_after_activate = costmap->activate(); + ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + rclcpp::shutdown(); + if (spin_thread.joinable()) { + spin_thread.join(); + } +} diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index d419cade363..2e1d2b4e8ca 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -633,7 +633,7 @@ void TestNode::reset() TEST_F(TestNode, testPercentSpeedLimit) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); EXPECT_TRUE(createSpeedFilter("map")); @@ -648,7 +648,7 @@ TEST_F(TestNode, testPercentSpeedLimit) TEST_F(TestNode, testIncorrectPercentSpeedLimit) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0); EXPECT_TRUE(createSpeedFilter("map")); @@ -663,7 +663,7 @@ TEST_F(TestNode, testIncorrectPercentSpeedLimit) TEST_F(TestNode, testAbsoluteSpeedLimit) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); EXPECT_TRUE(createSpeedFilter("map")); @@ -678,7 +678,7 @@ TEST_F(TestNode, testAbsoluteSpeedLimit) TEST_F(TestNode, testIncorrectAbsoluteSpeedLimit) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0); EXPECT_TRUE(createSpeedFilter("map")); @@ -693,7 +693,7 @@ TEST_F(TestNode, testIncorrectAbsoluteSpeedLimit) TEST_F(TestNode, testOutOfBounds) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); EXPECT_TRUE(createSpeedFilter("map")); @@ -708,7 +708,7 @@ TEST_F(TestNode, testOutOfBounds) TEST_F(TestNode, testInfoRePublish) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); EXPECT_TRUE(createSpeedFilter("map")); @@ -728,7 +728,7 @@ TEST_F(TestNode, testInfoRePublish) TEST_F(TestNode, testMaskRePublish) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); EXPECT_TRUE(createSpeedFilter("map")); @@ -747,7 +747,7 @@ TEST_F(TestNode, testMaskRePublish) TEST_F(TestNode, testIncorrectFilterType) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(INCORRECT_TYPE, 1.23, 4.5); EXPECT_FALSE(createSpeedFilter("map")); @@ -759,7 +759,7 @@ TEST_F(TestNode, testIncorrectFilterType) TEST_F(TestNode, testDifferentFrame) { - // Initilize test system + // Initialize test system createMaps("map"); publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); EXPECT_TRUE(createSpeedFilter("odom")); diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 5f0c5359dbe..1501e4321a1 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -6,12 +6,19 @@ add_definitions(-DXTENSOR_USE_XSIMD) set(XTENSOR_USE_TBB 0) set(XTENSOR_USE_OPENMP 0) +set(XTENSOR_USE_XSIMD 1) +# set(XTENSOR_DEFAULT_LAYOUT column_major) # row_major, column_major +# set(XTENSOR_DEFAULT_TRAVERSAL row_major) # row_major, column_major find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) find_package(xsimd REQUIRED) +include_directories( + include +) + set(dependencies_pkgs rclcpp nav2_common @@ -75,6 +82,7 @@ add_library(mppi_critics SHARED src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp + src/critics/path_align_legacy_critic.cpp src/critics/path_follow_critic.cpp src/critics/path_angle_critic.cpp src/critics/prefer_forward_critic.cpp @@ -86,7 +94,7 @@ set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) - target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS} ${OpenMP_INCLUDE_DIRS}) + target_include_directories(${lib} PUBLIC ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd) ament_target_dependencies(${lib} ${dependencies_pkgs}) endforeach() diff --git a/nav2_mppi_controller/LICENSE.md b/nav2_mppi_controller/LICENSE.md index da24c0064fc..57a5719427a 100644 --- a/nav2_mppi_controller/LICENSE.md +++ b/nav2_mppi_controller/LICENSE.md @@ -2,6 +2,7 @@ MIT License Copyright (c) 2021-2022 Fast Sense Studio Copyright (c) 2022-2023 Samsung Research America +Copyright (c) 2023 Open Navigation LLC Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 684e428fa20..13c314205bd 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -29,6 +29,7 @@ This process is then repeated a number of times and returns a converged solution - Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors - High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide - Easily extensible to support modern research variants of MPPI +- Comes pre-tuned for good out-of-the-box behavior ## Configuration @@ -52,6 +53,8 @@ This process is then repeated a number of times and returns a converged solution | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | + | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | + #### Trajectory Visualizer | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -116,6 +119,8 @@ This process is then repeated a number of times and returns a converged solution | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | +Note: There is a "Legacy" version of this critic also available with the same parameters of an old formulation pre-October 2023. + #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -254,6 +259,8 @@ The most common parameters you might want to start off changing are the velocity If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. +By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. + ### Prediction Horizon, Costmap Sizing, and Offsets As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 669944f2fe7..790568e7967 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -17,6 +17,10 @@ mppi critic for aligning to path + + 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/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/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..90fa92a57a7 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -152,7 +152,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..4b4d93abf3f 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,62 @@ 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; + } } 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/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 89f6d0d667e..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; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 779cdfe1bbf..84edc6e4df3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -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/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 6f84b05382d..78beab89fea 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_regulated_pure_pursuit_controller) find_package(ament_cmake REQUIRED) +find_package(nav2_amcl REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) @@ -26,6 +27,7 @@ set(dependencies nav2_costmap_2d pluginlib nav_msgs + nav2_amcl nav2_util nav2_core tf2 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..16f001ad1c5 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 @@ -57,6 +57,7 @@ struct Parameters bool allow_reversing; double max_robot_pose_search_dist; bool use_interpolation; + bool interpolate_curvature_after_goal; bool use_collision_detection; double transform_tolerance; }; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7b28ca9720e..64afdeda33a 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -134,7 +134,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @return Whether should rotate to path heading */ bool shouldRotateToPath( - const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path, + double & x_vel_sign); /** * @brief Whether robot should rotate to final goal orientation @@ -187,7 +188,9 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param path Current global path * @return Lookahead point */ - geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + geometry_msgs::msg::PoseStamped getLookAheadPoint( + const double &, const nav_msgs::msg::Path &, + bool interpolate_after_goal = false); /** * @brief checks for the cusp position @@ -210,6 +213,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; + std::shared_ptr> + curvature_carrot_pub_; std::shared_ptr> carrot_arc_pub_; std::unique_ptr path_handler_; std::unique_ptr param_handler_; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 5b62ce5aeaa..c56147a9693 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -10,6 +10,7 @@ ament_cmake + nav2_amcl nav2_common nav2_core nav2_util diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 92187376632..69b7d4eb144 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -93,6 +93,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".use_interpolation", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".interpolate_curvature_after_goal", + rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); @@ -165,6 +168,15 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".use_interpolation", params_.use_interpolation); + node->get_parameter( + plugin_name_ + ".interpolate_curvature_after_goal", + params_.interpolate_curvature_after_goal); + if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) { + RCLCPP_WARN( + logger_, "For interpolate_curvature_after_goal to be set to true, " + "use_fixed_curvature_lookahead should be true, it is currently set to false"); + params_.interpolate_curvature_after_goal = false; + } node->get_parameter( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); @@ -176,16 +188,6 @@ ParameterHandler::ParameterHandler( params_.use_cost_regulated_linear_velocity_scaling = false; } - /** Possible to drive in reverse direction if and only if - "use_rotate_to_heading" parameter is set to false **/ - - if (params_.use_rotate_to_heading && params_.allow_reversing) { - RCLCPP_WARN( - logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. By default setting use_rotate_to_heading true"); - params_.allow_reversing = false; - } - dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind( &ParameterHandler::dynamicParametersCallback, @@ -256,12 +258,6 @@ ParameterHandler::dynamicParametersCallback( } else if (name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); } else if (name == plugin_name_ + ".use_rotate_to_heading") { - if (parameter.as_bool() && params_.allow_reversing) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } params_.use_rotate_to_heading = parameter.as_bool(); } else if (name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index 90b7c4a3e0c..cedd7b0bbc1 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -73,6 +73,16 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( return euclidean_distance(robot_pose, ps); }); + // Make sure we always have at least 2 points on the transformed plan and than we don't prune + // the global plan below 2 points in order to have always enough point to interpolate the + // end of path direction + if (global_plan_.poses.begin() != closest_pose_upper_bound && + transformation_begin == std::prev(closest_pose_upper_bound)) + { + RCLCPP_WARN_STREAM(logger_, "transformation_begin == closest_pose_upper_bound "); + transformation_begin = std::prev(std::prev(closest_pose_upper_bound)); + } + // We'll discard points on the plan that are outside the local costmap const double max_costmap_extent = getCostmapMaxExtent(); auto transformation_end = std::find_if( 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..f04da098c43 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 @@ -20,6 +20,7 @@ #include #include +#include "nav2_amcl/angleutils.hpp" #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" @@ -73,6 +74,8 @@ void RegulatedPurePursuitController::configure( global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); + curvature_carrot_pub_ = node->create_publisher( + "curvature_lookahead_point", 1); } void RegulatedPurePursuitController::cleanup() @@ -84,6 +87,7 @@ void RegulatedPurePursuitController::cleanup() plugin_name_.c_str()); global_path_pub_.reset(); carrot_pub_.reset(); + curvature_carrot_pub_.reset(); } void RegulatedPurePursuitController::activate() @@ -95,6 +99,7 @@ void RegulatedPurePursuitController::activate() plugin_name_.c_str()); global_path_pub_->on_activate(); carrot_pub_->on_activate(); + curvature_carrot_pub_->on_activate(); } void RegulatedPurePursuitController::deactivate() @@ -106,6 +111,7 @@ void RegulatedPurePursuitController::deactivate() plugin_name_.c_str()); global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); + curvature_carrot_pub_->on_deactivate(); } std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( @@ -157,6 +163,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; @@ -187,6 +196,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Get the particular point on the path at the lookahead distance auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + auto rotate_to_path_carrot_pose = carrot_pose; carrot_pub_->publish(createCarrotMsg(carrot_pose)); double linear_vel, angular_vel; @@ -197,33 +207,39 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity if (params_->use_fixed_curvature_lookahead) { auto curvature_lookahead_pose = getLookAheadPoint( params_->curvature_lookahead_dist, - transformed_plan); + transformed_plan, params_->interpolate_curvature_after_goal); + rotate_to_path_carrot_pose = curvature_lookahead_pose; regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position); + curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose)); } // Setting the velocity direction - double sign = 1.0; + double x_vel_sign = 1.0; if (params_->allow_reversing) { - sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; + x_vel_sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; } linear_vel = params_->desired_linear_vel; // Make sure we're in compliance with basic constraints + // For shouldRotateToPath, using x_vel_sign in order to support allow_reversing + // and rotate_to_path_carrot_pose for the direction carrot pose: + // - equal to "normal" carrot_pose when curvature_lookahead_pose = false + // - otherwise equal to curvature_lookahead_pose (which can be interpolated after goal) double angle_to_heading; if (shouldRotateToGoalHeading(carrot_pose)) { double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); - } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + } else if (shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)) { rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); } else { applyConstraints( regulation_curvature, speed, collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, - linear_vel, sign); + linear_vel, x_vel_sign); // Apply curvature to angular velocity after constraining linear velocity - angular_vel = linear_vel * lookahead_curvature; + angular_vel = linear_vel * regulation_curvature; } // Collision checking on this velocity heading @@ -243,10 +259,15 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } bool RegulatedPurePursuitController::shouldRotateToPath( - const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path, + double & x_vel_sign) { // Whether we should rotate robot to rough path heading angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + // In case we are reversing + if (x_vel_sign < 0) { + angle_to_path = nav2_amcl::angleutils::normalize(angle_to_path + M_PI); + } return params_->use_rotate_to_heading && fabs(angle_to_path) > params_->rotate_to_heading_min_angle; } @@ -311,7 +332,8 @@ geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersect geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( const double & lookahead_dist, - const nav_msgs::msg::Path & transformed_plan) + const nav_msgs::msg::Path & transformed_plan, + bool interpolate_after_goal) { // Find the first pose which is at a distance greater than the lookahead distance auto goal_pose_it = std::find_if( @@ -321,7 +343,32 @@ 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()); + if (interpolate_after_goal) { + auto last_pose_it = std::prev(transformed_plan.poses.end()); + auto prev_last_pose_it = std::prev(last_pose_it); + + double end_path_orientation = atan2( + last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y, + last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x); + + auto current_robot_pose_it = transformed_plan.poses.begin(); + + double distance_after_last_pose = lookahead_dist - + std::hypot( + last_pose_it->pose.position.x - current_robot_pose_it->pose.position.x, + last_pose_it->pose.position.y - current_robot_pose_it->pose.position.y); + + geometry_msgs::msg::PoseStamped interpolated_point; + interpolated_point.header = last_pose_it->header; + interpolated_point.pose.position.x = last_pose_it->pose.position.x + + cos(end_path_orientation) * distance_after_last_pose; + interpolated_point.pose.position.y = last_pose_it->pose.position.y + + sin(end_path_orientation) * distance_after_last_pose; + + return interpolated_point; + } else { + goal_pose_it = std::prev(transformed_plan.poses.end()); + } } else if (params_->use_interpolation && 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) 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..1fff630654f 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -77,7 +77,8 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure bool shouldRotateToPathWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) { - return shouldRotateToPath(carrot_pose, angle_to_path); + double x_vel_sign = 1.0; + return shouldRotateToPath(carrot_pose, angle_to_path, x_vel_sign); } bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) 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_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_system_tests/COLCON_IGNORE b/nav2_system_tests/COLCON_IGNORE new file mode 100644 index 00000000000..e69de29bb2d 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/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_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_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/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index 7c56b85e334..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 @@ -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