diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 8f45cd318f..30955681de 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -256,6 +256,10 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME} ) +install(DIRECTORY test/utils/ + DESTINATION include/${PROJECT_NAME}/nav2_behavior_tree/test/utils +) + install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME}) install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME}) 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 ce4d96504b..c6caeeb5b2 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 @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -159,28 +160,30 @@ class CollisionMonitor : public nav2_util::LifecycleNode /** * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key and + * array of source's 2D obstacle points as value * @param velocity Desired robot velocity * @param robot_action Output processed robot action * @return True if returned action is caused by current polygon, otherwise false */ bool processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const; /** * @brief Processes APPROACH action type * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key and + * array of source's 2D obstacle points as value * @param velocity Desired robot velocity * @param robot_action Output processed robot action * @return True if returned action is caused by current polygon, otherwise false */ bool processApproach( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index fdc9e07b7a..6bb3b5dd01 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" @@ -126,6 +127,12 @@ class Polygon */ virtual void getPolygon(std::vector & poly) const; + /** + * @brief Obtains the name of the observation sources for current polygon. + * @return Names of the observation sources + */ + std::vector getSourcesNames() const; + /** * @brief Returns true if polygon points were set. * Otherwise, prints a warning and returns false. @@ -145,16 +152,28 @@ class Polygon */ virtual int getPointsInside(const std::vector & points) const; + /** + * @brief Gets number of points inside given polygon + * @param sources_collision_points_map Map containing source name as key, + * and input array of source's points to be checked as value + * @return Number of points inside polygon, + * for sources in map that are associated with current polygon. + * If there are no points, returns zero value. + */ + virtual int getPointsInside( + const std::unordered_map> & sources_collision_points_map) const; + /** * @brief Obtains estimated (simulated) time before a collision. * Applicable for APPROACH model. - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key, + * and input array of source's 2D obstacle points as value * @param velocity Simulated robot velocity * @return Estimated time before a collision. If there is no collision, * return value will be negative. */ double getCollisionTime( - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const; /** @@ -269,6 +288,8 @@ class Polygon rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; + /// @brief Name of the observation sources to check for polygon + std::vector sources_names_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 4d8da1e241..d8e03e19ed 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -168,10 +168,6 @@ bool CollisionDetector::getParameters() const bool base_shift_correction = get_parameter("base_shift_correction").as_bool(); - if (!configurePolygons(base_frame_id, transform_tolerance)) { - return false; - } - if (!configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -179,6 +175,10 @@ bool CollisionDetector::getParameters() return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 766ed713cd..c0bd15cb63 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -270,10 +270,6 @@ bool CollisionMonitor::getParameters( stop_pub_timeout_ = rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double()); - if (!configurePolygons(base_frame_id, transform_tolerance)) { - return false; - } - if ( !configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -281,6 +277,10 @@ bool CollisionMonitor::getParameters( return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } @@ -412,17 +412,21 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } // Points array collected from different data sources in a robot base frame - std::vector collision_points; + std::unordered_map> sources_collision_points_map; // 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 + // Fill collision points array from different data sources + auto marker_array = std::make_unique(); for (std::shared_ptr source : sources_) { + auto iter = sources_collision_points_map.insert( + {source->getSourceName(), std::vector()}); + if (source->getEnabled()) { - if (!source->getData(curr_time, collision_points) && + if (!source->getData(curr_time, iter.first->second) && source->getSourceTimeout().seconds() != 0.0) { action_polygon = nullptr; @@ -434,33 +438,35 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: break; } } + + if (collision_points_marker_pub_->get_subscription_count() > 0) { + // visualize collision points with markers + 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_" + source->getSourceName(); + 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); + marker.frame_locked = true; + + for (const auto & point : iter.first->second) { + 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); + } } 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); - marker.frame_locked = true; - - 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)); } @@ -479,12 +485,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: const ActionType at = polygon->getActionType(); if (at == STOP || at == SLOWDOWN || at == LIMIT) { // Process STOP/SLOWDOWN for the selected polygon - if (processStopSlowdownLimit(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processStopSlowdownLimit( + polygon, sources_collision_points_map, cmd_vel_in, robot_action)) + { action_polygon = polygon; } } else if (at == APPROACH) { // Process APPROACH for the selected polygon - if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processApproach(polygon, sources_collision_points_map, cmd_vel_in, robot_action)) { action_polygon = polygon; } } @@ -506,7 +514,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: bool CollisionMonitor::processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { @@ -514,7 +522,7 @@ bool CollisionMonitor::processStopSlowdownLimit( return false; } - if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) { + if (polygon->getPointsInside(sources_collision_points_map) >= polygon->getMinPoints()) { if (polygon->getActionType() == STOP) { // Setting up zero velocity for STOP model robot_action.polygon_name = polygon->getName(); @@ -561,7 +569,7 @@ bool CollisionMonitor::processStopSlowdownLimit( bool CollisionMonitor::processApproach( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { @@ -570,7 +578,7 @@ bool CollisionMonitor::processApproach( } // Obtain time before a collision - const double collision_time = polygon->getCollisionTime(collision_points, velocity); + const double collision_time = polygon->getCollisionTime(sources_collision_points_map, velocity); if (collision_time >= 0.0) { // If collision will occurr, reduce robot speed const double change_ratio = collision_time / polygon->getTimeBeforeCollision(); diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 523b446b25..19ac5053de 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -157,6 +157,11 @@ double Polygon::getTimeBeforeCollision() const return time_before_collision_; } +std::vector Polygon::getSourcesNames() const +{ + return sources_names_; +} + void Polygon::getPolygon(std::vector & poly) const { poly = poly_; @@ -229,19 +234,48 @@ int Polygon::getPointsInside(const std::vector & points) const return num; } +int Polygon::getPointsInside( + const std::unordered_map> & sources_collision_points_map) const +{ + int num = 0; + std::vector polygon_sources_names = getSourcesNames(); + + // Sum the number of points from all sources associated with current polygon + for (const auto & source_name : polygon_sources_names) { + const auto & iter = sources_collision_points_map.find(source_name); + if (iter != sources_collision_points_map.end()) { + num += getPointsInside(iter->second); + } + } + + return num; +} + double Polygon::getCollisionTime( - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const { // Initial robot pose is {0,0} in base_footprint coordinates Pose pose = {0.0, 0.0, 0.0}; Velocity vel = velocity; + std::vector polygon_sources_names = getSourcesNames(); + std::vector collision_points; + + // Save all points coming from the sources associated with current polygon + for (const auto & source_name : polygon_sources_names) { + const auto & iter = sources_collision_points_map.find(source_name); + if (iter != sources_collision_points_map.end()) { + collision_points.insert(collision_points.end(), iter->second.begin(), iter->second.end()); + } + } + // Array of points transformed to the frame concerned with pose on each simulation step std::vector points_transformed = collision_points; // Check static polygon - if (getPointsInside(points_transformed) >= min_points_) { + if (getPointsInside(collision_points) >= min_points_) { return 0.0; } @@ -392,6 +426,29 @@ bool Polygon::getCommonParameters( node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); } } + + // By default, use all observation sources for polygon + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + const std::vector observation_sources = + node->get_parameter("observation_sources").as_string_array(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(observation_sources)); + sources_names_ = node->get_parameter(polygon_name_ + ".sources_names").as_string_array(); + + // Check the observation sources configured for polygon are defined + for (auto source_name : sources_names_) { + if (std::find(observation_sources.begin(), observation_sources.end(), source_name) == + observation_sources.end()) + { + RCLCPP_ERROR_STREAM( + logger_, + "Observation source [" << source_name << + "] configured for polygon [" << getName() << + "] is not defined as one of the node's observation_source!"); + return false; + } + } } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 40415868f5..f297ca363a 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -143,7 +143,8 @@ class Tester : public ::testing::Test void setCommonParameters(); void addPolygon( const std::string & polygon_name, const PolygonType type, - const double size, const std::string & at); + const double size, const std::string & at, + const std::vector & sources_names = std::vector()); void addPolygonVelocitySubPolygon( const std::string & polygon_name, const std::string & sub_polygon_name, const double linear_min, const double linear_max, @@ -309,7 +310,8 @@ void Tester::setCommonParameters() void Tester::addPolygon( const std::string & polygon_name, const PolygonType type, - const double size, const std::string & at) + const double size, const std::string & at, + const std::vector & sources_names) { if (type == POLYGON) { cm_->declare_parameter( @@ -408,6 +410,13 @@ void Tester::addPolygon( polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name)); cm_->set_parameter( rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name)); + + if (!sources_names.empty()) { + cm_->declare_parameter( + polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".sources_names", sources_names)); + } } void Tester::addPolygonVelocitySubPolygon( @@ -1499,9 +1508,10 @@ TEST_F(Tester, testCollisionPointsMarkers) // Share TF sendTransforms(curr_time); + // No source published, empty marker array published publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCollisionPointsMarker(500ms)); - ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + ASSERT_EQ(collision_points_marker_msg_->markers.size(), 0u); publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); @@ -1580,6 +1590,48 @@ TEST_F(Tester, testVelocityPolygonStop) cm_->stop(); } +TEST_F(Tester, testSourceAssociatedToPolygon) +{ + // Set Collision Monitor parameters: + // - 2 sources (scan and range) + // - 1 stop polygon associated to range source + // - 1 slowdown polygon (associated with all sources by default) + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + addSource(RANGE_NAME, RANGE); + std::vector range_only_sources_names = {RANGE_NAME}; + std::vector all_sources_names = {SCAN_NAME, RANGE_NAME}; + addPolygon("StopOnRangeSource", POLYGON, 1.0, "stop", range_only_sources_names); + addPolygon("SlowdownOnAllSources", POLYGON, 1.0, "slowdown"); + setVectors({"StopOnRangeSource", "SlowdownOnAllSources"}, {SCAN_NAME, RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + + // Publish sources so that : + // - scan obstacle is in polygons + // - range obstacle is far away from polygons + publishScan(0.5, curr_time); + publishRange(4.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + + // Publish cmd vel + publishCmdVel(0.5, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + + // Since the stop polygon is only checking range source, slowdown action should be applied + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, SLOWDOWN); + ASSERT_EQ(action_state_->polygon_name, "SlowdownOnAllSources"); + + // Stop Collision Monitor node + cm_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index c703e12557..a0dbf9fcc9 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -46,6 +46,7 @@ static const char POLYGON_SUB_TOPIC[]{"polygon_sub"}; static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; static const char POLYGON_NAME[]{"TestPolygon"}; static const char CIRCLE_NAME[]{"TestCircle"}; +static const char OBSERVATION_SOURCE_NAME[]{"source"}; static const std::vector SQUARE_POLYGON { 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; static const std::vector ARBITRARY_POLYGON { @@ -235,7 +236,11 @@ class Tester : public ::testing::Test protected: // Working with parameters - void setCommonParameters(const std::string & polygon_name, const std::string & action_type); + void setCommonParameters( + const std::string & polygon_name, const std::string & action_type, + const std::vector & observation_sources = + std::vector({OBSERVATION_SOURCE_NAME}), + const std::vector & sources_names = std::vector()); void setPolygonParameters( const char * points, const bool is_static); @@ -289,7 +294,10 @@ Tester::~Tester() tf_buffer_.reset(); } -void Tester::setCommonParameters(const std::string & polygon_name, const std::string & action_type) +void Tester::setCommonParameters( + const std::string & polygon_name, const std::string & action_type, + const std::vector & observation_sources, + const std::vector & sources_names) { test_node_->declare_parameter( polygon_name + ".action_type", rclcpp::ParameterValue(action_type)); @@ -336,6 +344,18 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC)); test_node_->set_parameter( rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); + + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", observation_sources)); + + if (!sources_names.empty()) { + test_node_->declare_parameter( + polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".sources_names", sources_names)); + } } void Tester::setPolygonParameters( @@ -862,25 +882,26 @@ TEST_F(Tester, testPolygonGetCollisionTime) // Forward movement check nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.2 m ahead the footprint (0.5 m) - std::vector points{{0.7, -0.01}, {0.7, 0.01}}; + std::unordered_map> points_map; + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.7, -0.01}, {0.7, 0.01}}}); // Collision is expected to be ~= 0.2 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); // Backward movement check vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement // Two points 0.2 m behind the footprint (0.5 m) - points.clear(); - points = {{-0.7, -0.01}, {-0.7, 0.01}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.7, -0.01}, {-0.7, 0.01}}}); // Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); // Sideway movement check vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement // Two points 0.1 m ahead the footprint (0.5 m) - points.clear(); - points = {{-0.01, 0.6}, {0.01, 0.6}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.01, 0.6}, {0.01, 0.6}}}); // Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.2, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.2, SIMULATION_TIME_STEP); // Rotation check vel = {0.0, 0.0, 1.0}; // 1.0 rad/s rotation @@ -894,27 +915,27 @@ TEST_F(Tester, testPolygonGetCollisionTime) // | ' | // ----------- // ' - points.clear(); - points = {{0.49, -0.01}, {0.49, 0.01}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.49, -0.01}, {0.49, 0.01}}}); // Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds double exp_res = 45 / 180 * M_PI; - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), exp_res, EPSILON); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), exp_res, EPSILON); // Two points are already inside footprint vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points inside - points.clear(); - points = {{0.1, -0.01}, {0.1, 0.01}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.1, -0.01}, {0.1, 0.01}}}); // Collision already appeared: collision time should be 0 - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.0, EPSILON); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.0, EPSILON); // All points are out of simulation prediction vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.6 m ahead the footprint (0.5 m) - points.clear(); - points = {{1.1, -0.01}, {1.1, 0.01}}; + points_map.clear(); + points_map.insert({OBSERVATION_SOURCE_NAME, {{1.1, -0.01}, {1.1, 0.01}}}); // There is no collision: return value should be negative - EXPECT_LT(polygon_->getCollisionTime(points, vel), 0.0); + EXPECT_LT(polygon_->getCollisionTime(points_map, vel), 0.0); } TEST_F(Tester, testPolygonPublish) @@ -945,6 +966,9 @@ TEST_F(Tester, testPolygonDefaultVisualize) std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); + std::vector observation_sources = {OBSERVATION_SOURCE_NAME}; + test_node_->declare_parameter("observation_sources", rclcpp::ParameterValue(observation_sources)); + test_node_->set_parameter(rclcpp::Parameter("observation_sources", observation_sources)); setPolygonParameters(SQUARE_POLYGON_STR, true); // Create new polygon @@ -977,6 +1001,44 @@ TEST_F(Tester, testPolygonInvalidPointsString) ASSERT_FALSE(polygon_->configure()); } +TEST_F(Tester, testPolygonSourceDefaultAssociation) +{ + // By default, a polygon uses all observation sources + std::vector all_sources = {"source_1", "source_2", "source_3"}; + setCommonParameters(POLYGON_NAME, "stop", all_sources); // no polygon sources names specified + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + ASSERT_EQ(polygon_->getSourcesNames(), all_sources); +} + +TEST_F(Tester, testPolygonSourceInvalidAssociation) +{ + // If a source is not defined as observation source, polygon cannot use it: config should fail + setCommonParameters( + POLYGON_NAME, "stop", {"source_1", "source_2", "source_3"}, {"source_1", "source_4"}); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonSourceAssociation) +{ + // Checks that, if declared, only the specific sources associated to polygon are saved + std::vector poly_sources = {"source_1", "source_3"}; + setCommonParameters(POLYGON_NAME, "stop", {"source_1", "source_2", "source_3"}, poly_sources); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + ASSERT_EQ(polygon_->getSourcesNames(), poly_sources); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 216a85b514..8b9ecec6d7 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -208,6 +208,12 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC)); test_node_->set_parameter( rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); + + std::vector default_observation_sources = {"source"}; + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(default_observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", default_observation_sources)); } void Tester::setVelocityPolygonParameters(const bool is_holonomic) diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 7125a51418..6566784753 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -138,10 +138,10 @@ ament_add_test(test_costmap_subscriber_exec # ament_add_gtest_executable(costmap_tester # costmap_tester.cpp # ) -# ament_target_dependencies(costmap_tester -# ${dependencies} -# ) # target_link_libraries(costmap_tester -# nav2_costmap_2d_core # layers +# nav2_costmap_2d::nav2_costmap_2d_core +# nav2_util::nav2_utils_core +# rclcpp::rclcpp +# tf2_ros::tf2_ros # ) diff --git a/nav2_docking/README.md b/nav2_docking/README.md index c1aa1b10da..fba8ca1139 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -1,6 +1,6 @@ # Open Navigation's Nav2 Docking Framework -This package contains an automatic robot docking framework & auxiliary tools. It uses plugin `dock` implementations for a particular platform to enable the framework to generalize to robots of many different kinematic models, charging methods, sensor modalities, and so on. It can also handle a database of many different docking locations and dock models to handle a heterogeneous environment. This task server is designed be called by an application BT or autonomy application to dock once completed with tasks or battery is low -- _not_ within the navigate-to-pose action itself (though `undock` may be called from inside navigate actions!). +This package contains an automatic robot docking framework & auxiliary tools. It uses plugin `dock` implementations for a particular platform to enable the framework to generalize to robots of many different kinematic models, charging methods, sensor modalities, non-charging dock request needs, and so on. It can also handle a database of many different docking locations and dock models to handle a heterogeneous environment. This task server is designed be called by an application BT or autonomy application to dock once completed with tasks or battery is low -- _not_ within the navigate-to-pose action itself (though `undock` may be called from inside navigate actions!). This work is sponsored by [NVIDIA](https://www.nvidia.com/en-us/) and created by [Open Navigation LLC](https://opennav.org). @@ -24,9 +24,9 @@ The Docking Framework has 5 main components: - `Navigator`: A NavigateToPose action client to navigate the robot to the dock's staging pose if not with the prestaging tolerances - `DockDatabase`: A database of dock instances in an environment and their associated interfaces for transacting with each type. An arbitrary number of types are supported. - `Controller`: A spiral-based graceful controller to use for the vision-control loop for docking -- `ChargingDock`: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find this plugin header in the `opennav_docking_core` package. +- `ChargingDock` and `NonChargingDock`: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find these plugin headers in the `opennav_docking_core` package. -The `ChargingDock` plugins are the heart of the customizability of the framework to support any type of charging dock for any kind of robot. The `DockDatabase` is how you describe where these docks exist in your environment to interact with and any of them may be used in your docking request. +The `ChargingDock` and `NonChargingDock` plugins are the heart of the customizability of the framework to support any type of charging or non-charging dock for any kind of robot. This means you can both dock with charging stations, as well as non-charging infrastructure such as static locations (ex. conveyers) or dynamic locations (ex. pallets). The `DockDatabase` is how you describe where these docks exist in your environment to interact with and any of them may be used in your docking request. For dynamic locations like docking with movable objects, the action request can include an approximate docking location that uses vision-control loops to refine motion into it. The docking procedure is as follows: 1. Take action request and obtain the dock's plugin and its pose @@ -34,7 +34,7 @@ The docking procedure is as follows: 3. Use the dock's plugin to initially detect the dock and return the docking pose 4. Enter a vision-control loop where the robot attempts to reach the docking pose while its actively being refined by the vision system 5. Exit the vision-control loop once contact has been detected or charging has started -6. Wait until charging starts and return success. +6. Wait until charging starts (if applicable) and return success. If anywhere this procedure is unsuccessful, `N` retries may be made by driving back to the dock's staging pose and trying again. If still unsuccessful, it will return a failure code to indicate what kind of failure occurred to the client. @@ -74,7 +74,7 @@ This service exists to potentially reload the dock server's known dock database There are two unique elements to consider in specifying docks: dock _instances_ and dock _plugins_. Dock instances are instances of a particular dock in the map, as the database may contain many known docks (and you can specify which by name you'd like to dock at). Dock plugins are the model of dock that each is an instance of. The plugins contain the capabilities to generically detect and connect to a particular dock model. This separation allows us to efficiently enable many individual docking locations of potentially several different revisions with different attributes. -The **dock plugins** are specified in the parameter file as shown below. If you're familiar with plugins in other Nav2 servers, this should look like a familiar design pattern. Note that there is no specific information about the dock's pose or instances. These are generic attributes about the dock revision (such as staging pose, enable charging command, detection method, etc). You can add additional parameters in the dock's namespace as you choose (for example `timeout`). +The **dock plugins** are specified in the parameter file as shown below. If you're familiar with plugins in other Nav2 servers, this should look like a familiar design pattern. Note that there is no specific information about the dock's pose or instances. These are generic attributes about the dock revision (such as staging pose, enable charging command, detection method, etc). You can add additional parameters in the dock's namespace as you choose (for example `timeout`). These can be of both charging and non-charging types. ``` dock_plugins: ["dockv1", "dockv3"] @@ -145,23 +145,26 @@ There are two functions used during dock approach: * `isCharging`: The approach stops when the robot reports `isDocked`, then we wait for charging to start by calling `isCharging`. * This may be implemented using the `sensor_msgs/BatteryState` message to check the power status or for charging current. + * Used for charging-typed plugins. Similarly, there are two functions used during undocking: * `disableCharging`: This function is called before undocking commences to help prevent wear on the charge contacts. If the charge dock supports turning off the charge current, it should be done here. + * Used for charging-typed plugins. * `hasStoppedCharging`: This function is called while the controller is undocking. Undocking is successful when charging has stopped and the robot has returned to the staging pose. + * Used for charging-typed plugins. Keep in mind that the docking and undocking functions should return quickly as they will be called inside the control loop. Also make sure that `isDocked` should return true if `isCharging` returns true. -### Simple Charging Dock Plugin +### Simple (Non-) Charging Dock Plugins -The `SimpleChargingDock` plugin is an example with many common options which may be fully functional for +The `SimpleChargingDock` and `SimpleNonChargingDock` plugins are examples with many common options which may be fully functional for some robots. `getStagingPose` applys a parameterized translational and rotational offset to the dock pose to obtain the staging pose. @@ -174,7 +177,7 @@ During the docking approach, there are two options for detecting `isDocked`: 1. We can check the joint states of the wheels if the current has spiked above a set threshold to indicate that the robot has made contact with the dock or other physical object. 2. The dock pose is compared with the robot pose and `isDocked` returns true when the distance drops below the specified `docking_threshold`. -The `isCharging` and `hasStoppedCharging` functions have two options: +The `isCharging` and `hasStoppedCharging` functions have two options, when using the charging dock type: 1. Subscribing to a `sensor_msgs/BatteryState` message on topic `battery_state`. The robot is considered charging when the `current` field of the message exceeds the `charging_threshold`. 2. We can return that we are charging is `isDocked() = true`, which is useful for initial testing or low-reliability docking until battery state or similar information is available. diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index d991a25ba9..add25f4754 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -126,9 +126,33 @@ target_link_libraries(simple_charging_dock PRIVATE tf2::tf2 ) +add_library(simple_non_charging_dock SHARED + src/simple_non_charging_dock.cpp +) +target_include_directories(simple_non_charging_dock + PUBLIC + "$" + "$" +) +target_link_libraries(simple_non_charging_dock PUBLIC + ${geometry_msgs_TARGETS} + opennav_docking_core::opennav_docking_core + pose_filter + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros +) +target_link_libraries(simple_non_charging_dock PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib + tf2::tf2 +) + pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml) -install(TARGETS ${library_name} controller pose_filter simple_charging_dock +install(TARGETS ${library_name} controller pose_filter simple_charging_dock simple_non_charging_dock EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index 2b877442f5..49adf4124c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp @@ -142,6 +142,7 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock double charging_threshold_; // If not using an external pose reference, this is the distance threshold double docking_threshold_; + std::string base_frame_id_; // Offset for staging pose relative to dock pose double staging_x_offset_; double staging_yaw_offset_; diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp new file mode 100644 index 0000000000..0f25828944 --- /dev/null +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp @@ -0,0 +1,134 @@ +// Copyright (c) 2024 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. +// 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 OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ +#define OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +#include "opennav_docking_core/non_charging_dock.hpp" +#include "opennav_docking/pose_filter.hpp" + +namespace opennav_docking +{ + +class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock +{ +public: + /** + * @brief Constructor + */ + SimpleNonChargingDock() + : NonChargingDock() + {} + + /** + * @param parent pointer to user's node + * @param name The name of this planner + * @param tf A pointer to a TF buffer + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf); + + /** + * @brief Method to cleanup resources used on shutdown. + */ + virtual void cleanup() {} + + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + virtual void activate() {} + + /** + * @brief Method to deactive Behavior and any threads involved in execution. + */ + virtual void deactivate() {} + + /** + * @brief Method to obtain the dock's staging pose. This method should likely + * be using TF and the dock's pose information to find the staging pose from + * a static or parameterized staging pose relative to the docking pose + * @param pose Dock with pose + * @param frame Dock's frame of pose + * @return PoseStamped of staging pose in the specified frame + */ + virtual geometry_msgs::msg::PoseStamped getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame); + + /** + * @brief Method to obtain the refined pose of the dock, usually based on sensors + * @param pose The initial estimate of the dock pose. + * @param frame The frame of the initial estimate. + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id); + + /** + * @copydoc opennav_docking_core::ChargingDock::isDocked + */ + virtual bool isDocked(); + +protected: + void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); + + // Optionally subscribe to a detected dock pose topic + rclcpp::Subscription::SharedPtr dock_pose_sub_; + rclcpp::Publisher::SharedPtr dock_pose_pub_; + rclcpp::Publisher::SharedPtr filtered_dock_pose_pub_; + rclcpp::Publisher::SharedPtr staging_pose_pub_; + // If subscribed to a detected pose topic, will contain latest message + geometry_msgs::msg::PoseStamped detected_dock_pose_; + // This is the actual dock pose once it has the specified translation/rotation applied + // If not subscribed to a topic, this is simply the database dock pose + geometry_msgs::msg::PoseStamped dock_pose_; + + // Optionally subscribe to joint state message, used to determine if stalled + rclcpp::Subscription::SharedPtr joint_state_sub_; + std::vector stall_joint_names_; + double stall_velocity_threshold_, stall_effort_threshold_; + bool is_stalled_; + + // An external reference (such as image_proc::TrackMarkerNode) can be used to detect dock + bool use_external_detection_pose_; + double external_detection_timeout_; + tf2::Quaternion external_detection_rotation_; + double external_detection_translation_x_; + double external_detection_translation_y_; + + // Filtering of detected poses + std::shared_ptr filter_; + + // If not using an external pose reference, this is the distance threshold + double docking_threshold_; + std::string base_frame_id_; + // Offset for staging pose relative to dock pose + double staging_x_offset_; + double staging_yaw_offset_; + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr tf2_buffer_; +}; + +} // namespace opennav_docking + +#endif // OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ diff --git a/nav2_docking/opennav_docking/plugins.xml b/nav2_docking/opennav_docking/plugins.xml index 2f1da5f283..3a3c91f067 100644 --- a/nav2_docking/opennav_docking/plugins.xml +++ b/nav2_docking/opennav_docking/plugins.xml @@ -4,6 +4,11 @@ A simple charging dock plugin. + + + A simple non-charging dock plugin. + + diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index a8bf62a6b2..0d5034bd60 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -227,19 +227,20 @@ void DockingServer::dockRobot() if (goal->use_dock_id) { RCLCPP_INFO( get_logger(), - "Attempting to dock robot at charger %s.", goal->dock_id.c_str()); + "Attempting to dock robot at %s.", goal->dock_id.c_str()); dock = dock_db_->findDock(goal->dock_id); } else { RCLCPP_INFO( get_logger(), - "Attempting to dock robot at charger at position (%0.2f, %0.2f).", + "Attempting to dock robot at position (%0.2f, %0.2f).", goal->dock_pose.pose.position.x, goal->dock_pose.pose.position.y); dock = generateGoalDock(goal); } - // Check if the robot is docked or charging before proceeding - if (dock->plugin->isDocked() || dock->plugin->isCharging()) { - RCLCPP_INFO(get_logger(), "Robot is already charging, no need to dock"); + // Check if robot is docked or charging before proceeding, only applicable to charging docks + if (dock->plugin->isCharger() && (dock->plugin->isDocked() || dock->plugin->isCharging())) { + RCLCPP_INFO( + get_logger(), "Robot is already docked and/or charging (if applicable), no need to dock"); return; } @@ -280,9 +281,14 @@ void DockingServer::dockRobot() // Approach the dock using control law if (approachDock(dock, dock_pose)) { // We are docked, wait for charging to begin - RCLCPP_INFO(get_logger(), "Made contact with dock, waiting for charge to start"); + RCLCPP_INFO( + get_logger(), "Made contact with dock, waiting for charge to start (if applicable)."); if (waitForCharge(dock)) { - RCLCPP_INFO(get_logger(), "Robot is charging!"); + if (dock->plugin->isCharger()) { + RCLCPP_INFO(get_logger(), "Robot is charging!"); + } else { + RCLCPP_INFO(get_logger(), "Docking was successful!"); + } result->success = true; result->num_retries = num_retries_; stashDockData(goal->use_dock_id, dock, true); @@ -300,7 +306,7 @@ void DockingServer::dockRobot() } catch (opennav_docking_core::DockingException & e) { if (++num_retries_ > max_retries_) { RCLCPP_ERROR(get_logger(), "Failed to dock, all retries have been used"); - throw; + throw e; } RCLCPP_WARN(get_logger(), "Docking failed, will retry: %s", e.what()); } @@ -403,7 +409,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & publishDockingFeedback(DockRobot::Feedback::CONTROLLING); // Stop and report success if connected to dock - if (dock->plugin->isDocked() || dock->plugin->isCharging()) { + if (dock->plugin->isDocked() || (dock->plugin->isCharger() && dock->plugin->isCharging())) { return true; } @@ -450,7 +456,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & if (this->now() - start > timeout) { throw opennav_docking_core::FailedToControl( - "Timed out approaching dock; dock nor charging detected"); + "Timed out approaching dock; dock nor charging (if applicable) detected"); } loop_rate.sleep(); @@ -460,6 +466,11 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & bool DockingServer::waitForCharge(Dock * dock) { + // This is a non-charger docking request + if (!dock->plugin->isCharger()) { + return true; + } + rclcpp::Rate loop_rate(controller_frequency_); auto start = this->now(); auto timeout = rclcpp::Duration::from_seconds(wait_charge_timeout_); @@ -589,11 +600,11 @@ void DockingServer::undockRobot() } RCLCPP_INFO( get_logger(), - "Attempting to undock robot from charger of type %s.", dock->getName().c_str()); + "Attempting to undock robot of dock type %s.", dock->getName().c_str()); // Check if the robot is docked before proceeding - if (!dock->isDocked()) { - RCLCPP_INFO(get_logger(), "Robot is not in the charger, no need to undock"); + if (dock->isCharger() && (!dock->isDocked() && !dock->isCharging())) { + RCLCPP_INFO(get_logger(), "Robot is not in the dock, no need to undock"); return; } @@ -623,7 +634,7 @@ void DockingServer::undockRobot() } // Don't control the robot until charging is disabled - if (!dock->disableCharging()) { + if (dock->isCharger() && !dock->disableCharging()) { loop_rate.sleep(); continue; } @@ -638,7 +649,7 @@ void DockingServer::undockRobot() RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); // Have reached staging_pose vel_publisher_->publish(std::move(command)); - if (dock->hasStoppedCharging()) { + if (!dock->isCharger() || dock->hasStoppedCharging()) { RCLCPP_INFO(get_logger(), "Robot has undocked!"); result->success = true; curr_dock_type_.clear(); @@ -647,7 +658,7 @@ void DockingServer::undockRobot() return; } // Haven't stopped charging? - throw opennav_docking_core::FailedToControl("Failed to control off dock, still charging"); + throw opennav_docking_core::FailedToControl("Failed to control off dock"); } // Publish command and sleep diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index 3ff04db47f..298a3be950 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -94,6 +94,7 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".stall_velocity_threshold", stall_velocity_threshold_); node_->get_parameter(name + ".stall_effort_threshold", stall_effort_threshold_); node_->get_parameter(name + ".docking_threshold", docking_threshold_); + node_->get_parameter("base_frame", base_frame_id_); // Get server base frame ID node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); @@ -250,7 +251,7 @@ bool SimpleChargingDock::isDocked() // Find base pose in target frame geometry_msgs::msg::PoseStamped base_pose; base_pose.header.stamp = rclcpp::Time(0); - base_pose.header.frame_id = "base_link"; + base_pose.header.frame_id = base_frame_id_; base_pose.pose.orientation.w = 1.0; try { tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id); diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp new file mode 100644 index 0000000000..25e67c5eb9 --- /dev/null +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -0,0 +1,274 @@ +// Copyright (c) 2024 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. +// 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 "nav2_util/node_utils.hpp" +#include "opennav_docking/simple_non_charging_dock.hpp" + +namespace opennav_docking +{ + +void SimpleNonChargingDock::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf) +{ + name_ = name; + tf2_buffer_ = tf; + node_ = parent.lock(); + if (!node_) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Parameters for optional external detection of dock pose + nav2_util::declare_parameter_if_not_declared( + node_, name + ".use_external_detection_pose", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_timeout", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_translation_x", rclcpp::ParameterValue(-0.20)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_translation_y", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_yaw", rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_pitch", rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".external_detection_rotation_roll", rclcpp::ParameterValue(-1.57)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".filter_coef", rclcpp::ParameterValue(0.1)); + + // Optionally determine if docked via stall detection using joint_states + nav2_util::declare_parameter_if_not_declared( + node_, name + ".use_stall_detection", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_joint_names", rclcpp::PARAMETER_STRING_ARRAY); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_velocity_threshold", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".stall_effort_threshold", rclcpp::ParameterValue(1.0)); + + // If not using stall detection, this is how close robot should get to pose + nav2_util::declare_parameter_if_not_declared( + node_, name + ".docking_threshold", rclcpp::ParameterValue(0.05)); + + // Staging pose configuration + nav2_util::declare_parameter_if_not_declared( + node_, name + ".staging_x_offset", rclcpp::ParameterValue(-0.7)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); + + node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); + node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_); + node_->get_parameter( + name + ".external_detection_translation_x", external_detection_translation_x_); + node_->get_parameter( + name + ".external_detection_translation_y", external_detection_translation_y_); + double yaw, pitch, roll; + node_->get_parameter(name + ".external_detection_rotation_yaw", yaw); + node_->get_parameter(name + ".external_detection_rotation_pitch", pitch); + node_->get_parameter(name + ".external_detection_rotation_roll", roll); + external_detection_rotation_.setEuler(pitch, roll, yaw); + node_->get_parameter(name + ".stall_velocity_threshold", stall_velocity_threshold_); + node_->get_parameter(name + ".stall_effort_threshold", stall_effort_threshold_); + node_->get_parameter(name + ".docking_threshold", docking_threshold_); + node_->get_parameter("base_frame", base_frame_id_); // Get server base frame ID + node_->get_parameter(name + ".staging_x_offset", staging_x_offset_); + node_->get_parameter(name + ".staging_yaw_offset", staging_yaw_offset_); + + // Setup filter + double filter_coef; + node_->get_parameter(name + ".filter_coef", filter_coef); + filter_ = std::make_unique(filter_coef, external_detection_timeout_); + + if (use_external_detection_pose_) { + dock_pose_.header.stamp = rclcpp::Time(0); + dock_pose_sub_ = node_->create_subscription( + "detected_dock_pose", 1, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { + detected_dock_pose_ = *pose; + }); + } + + bool use_stall_detection; + node_->get_parameter(name + ".use_stall_detection", use_stall_detection); + if (use_stall_detection) { + is_stalled_ = false; + node_->get_parameter(name + ".stall_joint_names", stall_joint_names_); + if (stall_joint_names_.size() < 1) { + RCLCPP_ERROR(node_->get_logger(), "stall_joint_names cannot be empty!"); + } + joint_state_sub_ = node_->create_subscription( + "joint_states", 1, + std::bind(&SimpleNonChargingDock::jointStateCallback, this, std::placeholders::_1)); + } + + dock_pose_pub_ = node_->create_publisher("dock_pose", 1); + filtered_dock_pose_pub_ = node_->create_publisher( + "filtered_dock_pose", 1); + staging_pose_pub_ = node_->create_publisher("staging_pose", 1); +} + +geometry_msgs::msg::PoseStamped SimpleNonChargingDock::getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) +{ + // If not using detection, set the dock pose as the given dock pose estimate + if (!use_external_detection_pose_) { + // This gets called at the start of docking + // Reset our internally tracked dock pose + dock_pose_.header.frame_id = frame; + dock_pose_.pose = pose; + } + + // Compute the staging pose with given offsets + const double yaw = tf2::getYaw(pose.orientation); + geometry_msgs::msg::PoseStamped staging_pose; + staging_pose.header.frame_id = frame; + staging_pose.header.stamp = node_->now(); + staging_pose.pose = pose; + staging_pose.pose.position.x += cos(yaw) * staging_x_offset_; + staging_pose.pose.position.y += sin(yaw) * staging_x_offset_; + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, yaw + staging_yaw_offset_); + staging_pose.pose.orientation = tf2::toMsg(orientation); + + // Publish staging pose for debugging purposes + staging_pose_pub_->publish(staging_pose); + return staging_pose; +} + +bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string) +{ + // If using not detection, set the dock pose to the static fixed-frame version + if (!use_external_detection_pose_) { + dock_pose_pub_->publish(pose); + dock_pose_ = pose; + return true; + } + + // If using detections, get current detections, transform to frame, and apply offsets + geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; + + // Validate that external pose is new enough + auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); + if (node_->now() - detected.header.stamp > timeout) { + RCLCPP_WARN(node_->get_logger(), "Lost detection or did not detect: timeout exceeded"); + return false; + } + + // Transform detected pose into fixed frame. Note that the argument pose + // is the output of detection, but also acts as the initial estimate + // and contains the frame_id of docking + if (detected.header.frame_id != pose.header.frame_id) { + try { + if (!tf2_buffer_->canTransform( + pose.header.frame_id, detected.header.frame_id, + detected.header.stamp, rclcpp::Duration::from_seconds(0.2))) + { + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + return false; + } + tf2_buffer_->transform(detected, detected, pose.header.frame_id); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + return false; + } + } + + // Filter the detected pose + detected = filter_->update(detected); + filtered_dock_pose_pub_->publish(detected); + + // Rotate the just the orientation, then remove roll/pitch + geometry_msgs::msg::PoseStamped just_orientation; + just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation = detected.pose.orientation; + tf2::doTransform(just_orientation, just_orientation, transform); + + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, tf2::getYaw(just_orientation.pose.orientation)); + dock_pose_.pose.orientation = tf2::toMsg(orientation); + + // Construct dock_pose_ by applying translation/rotation + dock_pose_.header = detected.header; + dock_pose_.pose.position = detected.pose.position; + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + dock_pose_.pose.position.x += cos(yaw) * external_detection_translation_x_ - + sin(yaw) * external_detection_translation_y_; + dock_pose_.pose.position.y += sin(yaw) * external_detection_translation_x_ + + cos(yaw) * external_detection_translation_y_; + dock_pose_.pose.position.z = 0.0; + + // Publish & return dock pose for debugging purposes + dock_pose_pub_->publish(dock_pose_); + pose = dock_pose_; + return true; +} + +bool SimpleNonChargingDock::isDocked() +{ + if (joint_state_sub_) { + // Using stall detection + return is_stalled_; + } + + if (dock_pose_.header.frame_id.empty()) { + // Dock pose is not yet valid + return false; + } + + // Find base pose in target frame + geometry_msgs::msg::PoseStamped base_pose; + base_pose.header.stamp = rclcpp::Time(0); + base_pose.header.frame_id = base_frame_id_; + base_pose.pose.orientation.w = 1.0; + try { + tf2_buffer_->transform(base_pose, base_pose, dock_pose_.header.frame_id); + } catch (const tf2::TransformException & ex) { + return false; + } + + // If we are close enough, we are docked + double d = std::hypot( + base_pose.pose.position.x - dock_pose_.pose.position.x, + base_pose.pose.position.y - dock_pose_.pose.position.y); + return d < docking_threshold_; +} + +void SimpleNonChargingDock::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state) +{ + double velocity = 0.0; + double effort = 0.0; + for (size_t i = 0; i < state->name.size(); ++i) { + for (auto & name : stall_joint_names_) { + if (state->name[i] == name) { + // Tracking this joint + velocity += abs(state->velocity[i]); + effort += abs(state->effort[i]); + } + } + } + + // Take average + effort /= stall_joint_names_.size(); + velocity /= stall_joint_names_.size(); + + is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_); +} + +} // namespace opennav_docking + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(opennav_docking::SimpleNonChargingDock, opennav_docking_core::ChargingDock) diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index 8ffbe18740..65d6e1d68a 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -60,6 +60,15 @@ target_link_libraries(test_simple_charging_dock simple_charging_dock tf2_geometry_msgs::tf2_geometry_msgs ) +ament_add_gtest(test_simple_non_charging_dock + test_simple_non_charging_dock.cpp +) +ament_target_dependencies(test_simple_non_charging_dock + ${dependencies} +) +target_link_libraries(test_simple_non_charging_dock + ${library_name} simple_non_charging_dock +) # Test Pose Filter (unit) ament_add_gtest(test_pose_filter diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 6bc7439129..7193bb188b 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -48,6 +48,7 @@ TEST(SimpleChargingDockTests, ObjectLifecycle) EXPECT_FALSE(dock->isCharging()); EXPECT_TRUE(dock->disableCharging()); EXPECT_TRUE(dock->hasStoppedCharging()); + EXPECT_TRUE(dock->isCharger()); dock->deactivate(); dock->cleanup(); @@ -66,6 +67,8 @@ TEST(SimpleChargingDockTests, BatteryState) dock->configure(node, "my_dock", nullptr); dock->activate(); + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); // Below threshold sensor_msgs::msg::BatteryState msg; diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp new file mode 100644 index 0000000000..89c423f5ff --- /dev/null +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -0,0 +1,205 @@ +// Copyright (c) 2024 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. +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "opennav_docking/simple_non_charging_dock.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +// Testing the simple non-charging dock plugin + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +namespace opennav_docking +{ + +TEST(SimpleNonChargingDockTests, ObjectLifecycle) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // Check initial states + EXPECT_THROW(dock->isCharging(), std::runtime_error); + EXPECT_THROW(dock->disableCharging(), std::runtime_error); + EXPECT_THROW(dock->hasStoppedCharging(), std::runtime_error); + EXPECT_FALSE(dock->isCharger()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StallDetection) +{ + auto node = std::make_shared("test"); + auto pub = node->create_publisher( + "joint_states", rclcpp::QoS(1)); + pub->on_activate(); + node->declare_parameter("my_dock.use_stall_detection", rclcpp::ParameterValue(true)); + std::vector names = {"left_motor", "right_motor"}; + node->declare_parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)); + node->declare_parameter("my_dock.stall_velocity_threshold", rclcpp::ParameterValue(0.1)); + node->declare_parameter("my_dock.stall_effort_threshold", rclcpp::ParameterValue(5.0)); + + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + + // Stopped, but below effort threshold + sensor_msgs::msg::JointState msg; + msg.name = {"left_motor", "right_motor", "another_motor"}; + msg.velocity = {0.0, 0.0, 0.0}; + msg.effort = {0.0, 0.0, 0.0}; + pub->publish(msg); + rclcpp::Rate r(2); + r.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isDocked()); + + // Moving, with effort + sensor_msgs::msg::JointState msg2; + msg2.name = {"left_motor", "right_motor", "another_motor"}; + msg2.velocity = {1.0, 1.0, 0.0}; + msg2.effort = {5.1, -5.1, 0.0}; + pub->publish(msg2); + rclcpp::Rate r1(2); + r1.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isDocked()); + + // Stopped, with effort + sensor_msgs::msg::JointState msg3; + msg3.name = {"left_motor", "right_motor", "another_motor"}; + msg3.velocity = {0.0, 0.0, 0.0}; + msg3.effort = {5.1, -5.1, 0.0}; + pub->publish(msg3); + rclcpp::Rate r2(2); + r2.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_TRUE(dock->isDocked()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StagingPose) +{ + auto node = std::make_shared("test"); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 0.0, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StagingPoseWithYawOffset) +{ + // Override the parameter default + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.staging_yaw_offset", 3.14}, + } + ); + + auto node = std::make_shared("test", options); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + // Pose should be the same as default, but pointing in opposite direction + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 3.14, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, RefinedPoseTest) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + auto pub = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + pub->on_activate(); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::PoseStamped pose; + + // Timestamps are outdated; this is after timeout + EXPECT_FALSE(dock->isDocked()); + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 0.1; + detected_pose.pose.position.y = -0.5; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + pose.header.frame_id = "my_frame"; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +} // namespace opennav_docking diff --git a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp index 2f46b8cb76..fb0c93016d 100644 --- a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp @@ -31,7 +31,8 @@ DockRobotAction::DockRobotAction( void DockRobotAction::on_tick() { // Get core inputs about what to perform - if (getInput("use_dock_id", goal_.use_dock_id)) { + [[maybe_unused]] auto res = getInput("use_dock_id", goal_.use_dock_id); + if (goal_.use_dock_id) { getInput("dock_id", goal_.dock_id); } else { getInput("dock_pose", goal_.dock_pose); diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index d4566e5836..886fde2fa7 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -118,6 +118,11 @@ class ChargingDock */ virtual bool hasStoppedCharging() = 0; + /** + * @brief Gets if this is a charging-typed dock + */ + virtual bool isCharger() {return true;} + std::string getName() {return name_;} protected: diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp new file mode 100644 index 0000000000..9951b19dff --- /dev/null +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp @@ -0,0 +1,136 @@ +// Copyright (c) 2024 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. +// 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 OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_ +#define OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_ + +#include +#include + +#include "opennav_docking_core/charging_dock.hpp" + + +namespace opennav_docking_core +{ + +/** + * @class NonChargingDock + * @brief Abstract interface for a non-charging dock for the docking framework + */ +class NonChargingDock : public ChargingDock +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Virtual destructor + */ + virtual ~NonChargingDock() {} + + /** + * @param parent pointer to user's node + * @param name The name of this planner + * @param tf A pointer to a TF buffer + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf) = 0; + + /** + * @brief Method to cleanup resources used on shutdown. + */ + virtual void cleanup() = 0; + + /** + * @brief Method to active Behavior and any threads involved in execution. + */ + virtual void activate() = 0; + + /** + * @brief Method to deactive Behavior and any threads involved in execution. + */ + virtual void deactivate() = 0; + + /** + * @brief Method to obtain the dock's staging pose. This method should likely + * be using TF and the dock's pose information to find the staging pose from + * a static or parameterized staging pose relative to the docking pose + * @param pose Dock pose + * @param frame Dock's frame of pose + * @return PoseStamped of staging pose in the specified frame + */ + virtual geometry_msgs::msg::PoseStamped getStagingPose( + const geometry_msgs::msg::Pose & pose, const std::string & frame) = 0; + + /** + * @brief Method to obtain the refined pose of the dock, usually based on sensors + * @param pose The initial estimate of the dock pose. + * @param frame The frame of the initial estimate. + */ + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id) = 0; + + /** + * @brief Have we made contact with dock? This can be implemented in a variety + * of ways: by establishing communications with the dock, by monitoring the + * the drive motor efforts, etc. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + virtual bool isDocked() = 0; + + /** + * @brief Are we charging? If a charge dock requires any sort of negotiation + * to begin charging, that should happen inside this function as this function + * will be called repeatedly after the docking loop to check if successful. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + bool isCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Undocking while current is still flowing can damage a charge dock + * so some charge docks provide the ability to disable charging before the + * robot physically disconnects. The undocking action will not command the + * robot to move until this returns true. + * + * NOTE: this function is expected to return QUICKLY. Blocking here will block + * the docking controller loop. + */ + bool disableCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Similar to isCharging() but called when undocking. + */ + bool hasStoppedCharging() final + { + throw std::runtime_error("This dock is not a charging dock!"); + } + + /** + * @brief Gets if this is a charging-typed dock + */ + bool isCharger() final {return false;} +}; + +} // namespace opennav_docking_core + +#endif // OPENNAV_DOCKING_CORE__NON_CHARGING_DOCK_HPP_ diff --git a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt index 5099822acb..e900fd292c 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt +++ b/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt @@ -1,10 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(nav2_dwb_controller) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) + +nav2_package() ament_package() diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 77d5f4ec9f..f98c8473a8 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -11,14 +11,15 @@ Apache-2.0 ament_cmake + nav2_common - costmap_queue - dwb_core - dwb_critics - dwb_msgs - dwb_plugins - nav_2d_msgs - nav_2d_utils + costmap_queue + dwb_core + dwb_critics + dwb_msgs + dwb_plugins + nav_2d_msgs + nav_2d_utils ament_cmake diff --git a/nav2_mppi_controller/benchmark/CMakeLists.txt b/nav2_mppi_controller/benchmark/CMakeLists.txt index 97db9f4185..3f92196fcb 100644 --- a/nav2_mppi_controller/benchmark/CMakeLists.txt +++ b/nav2_mppi_controller/benchmark/CMakeLists.txt @@ -9,14 +9,17 @@ foreach(name IN LISTS BENCHMARK_NAMES) add_executable(${name} ${name}.cpp ) - ament_target_dependencies(${name} - ${dependencies_pkgs} - ) target_link_libraries(${name} - mppi_controller mppi_critics benchmark + benchmark + ${geometry_msgs_TARGETS} + mppi_controller + mppi_critics + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} ) -target_include_directories(${name} PRIVATE + target_include_directories(${name} PRIVATE ${PROJECT_SOURCE_DIR}/test/utils -) + ) endforeach() diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index a7216bc80d..0906385c1a 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -13,7 +13,9 @@ // limitations under the License. #include + #include +#include #include "gtest/gtest.h" #include diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 82b850bc46..0816fcc32e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -106,7 +106,9 @@ class MPPIController : public nav2_core::Controller * @brief Visualize trajectories * @param transformed_plan Transformed input plan */ - void visualize(nav_msgs::msg::Path transformed_plan); + void visualize( + nav_msgs::msg::Path transformed_plan, + const builtin_interfaces::msg::Time & cmd_stamp); std::string name_; rclcpp_lifecycle::LifecycleNode::WeakPtr parent_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index d9cdc95ce9..328424317d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -79,7 +79,9 @@ class TrajectoryVisualizer * @brief Add an optimal trajectory to visualize * @param trajectory Optimal trajectory */ - void add(const xt::xtensor & trajectory, const std::string & marker_namespace); + void add( + const xt::xtensor & trajectory, const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp); /** * @brief Add candidate trajectories to visualize @@ -103,7 +105,9 @@ class TrajectoryVisualizer std::shared_ptr> trajectories_publisher_; std::shared_ptr> transformed_path_pub_; + std::shared_ptr> optimal_path_pub_; + std::unique_ptr optimal_path_; std::unique_ptr points_; int marker_id_ = 0; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 6c83131263..54eb1f57a0 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -99,16 +99,18 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( #endif if (visualize_) { - visualize(std::move(transformed_plan)); + visualize(std::move(transformed_plan), cmd.header.stamp); } return cmd; } -void MPPIController::visualize(nav_msgs::msg::Path transformed_plan) +void MPPIController::visualize( + nav_msgs::msg::Path transformed_plan, + const builtin_interfaces::msg::Time & cmd_stamp) { trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); - trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory"); + trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp); trajectory_visualizer_.visualize(std::move(transformed_plan)); } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index a6531b7d1d..7cb07c6858 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -28,6 +28,7 @@ void TrajectoryVisualizer::on_configure( trajectories_publisher_ = node->create_publisher("/trajectories", 1); transformed_path_pub_ = node->create_publisher("transformed_global_plan", 1); + optimal_path_pub_ = node->create_publisher("optimal_trajectory", 1); parameters_handler_ = parameters_handler; auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer"); @@ -42,22 +43,27 @@ void TrajectoryVisualizer::on_cleanup() { trajectories_publisher_.reset(); transformed_path_pub_.reset(); + optimal_path_pub_.reset(); } void TrajectoryVisualizer::on_activate() { trajectories_publisher_->on_activate(); transformed_path_pub_->on_activate(); + optimal_path_pub_->on_activate(); } void TrajectoryVisualizer::on_deactivate() { trajectories_publisher_->on_deactivate(); transformed_path_pub_->on_deactivate(); + optimal_path_pub_->on_deactivate(); } void TrajectoryVisualizer::add( - const xt::xtensor & trajectory, const std::string & marker_namespace) + const xt::xtensor & trajectory, + const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp) { auto & size = trajectory.shape()[0]; if (!size) { @@ -76,8 +82,21 @@ void TrajectoryVisualizer::add( auto marker = utils::createMarker( marker_id_++, pose, scale, color, frame_id_, marker_namespace); points_->markers.push_back(marker); + + // populate optimal path + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.frame_id = frame_id_; + pose_stamped.pose = pose; + + tf2::Quaternion quaternion_tf2; + quaternion_tf2.setRPY(0., 0., trajectory(i, 2)); + pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2); + + optimal_path_->poses.push_back(pose_stamped); }; + optimal_path_->header.stamp = cmd_stamp; + optimal_path_->header.frame_id = frame_id_; for (size_t i = 0; i < size; i++) { add_marker(i); } @@ -111,6 +130,7 @@ void TrajectoryVisualizer::reset() { marker_id_ = 0; points_ = std::make_unique(); + optimal_path_ = std::make_unique(); } void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) @@ -119,6 +139,10 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) trajectories_publisher_->publish(std::move(points_)); } + if (optimal_path_pub_->get_subscription_count() > 0) { + optimal_path_pub_->publish(std::move(optimal_path_)); + } + reset(); if (transformed_path_pub_->get_subscription_count() > 0) { diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 7ebada2a6a..2b7c8e0a90 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -81,7 +81,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); - vis.add(optimal_trajectory, "Optimal Trajectory"); + builtin_interfaces::msg::Time bogus_stamp; + vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); @@ -90,7 +91,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) // Now populated with content, should publish optimal_trajectory = xt::ones({20, 2}); - vis.add(optimal_trajectory, "Optimal Trajectory"); + vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp); vis.visualize(bogus_path); rclcpp::spin_some(node->get_node_base_interface()); @@ -153,3 +154,65 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) // 40 * 4, for 5 trajectory steps + 3 point steps EXPECT_EQ(recieved_msg.markers.size(), 160u); } + +TEST(TrajectoryVisualizerTests, VisOptimalPath) +{ + auto node = std::make_shared("my_node"); + auto parameters_handler = std::make_unique(node); + builtin_interfaces::msg::Time cmd_stamp; + cmd_stamp.sec = 5; + cmd_stamp.nanosec = 10; + + nav_msgs::msg::Path recieved_path; + auto my_sub = node->create_subscription( + "optimal_trajectory", 10, + [&](const nav_msgs::msg::Path msg) {recieved_path = msg;}); + + // optimal_trajectory empty, should fail to publish + xt::xtensor optimal_trajectory; + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + nav_msgs::msg::Path bogus_path; + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_EQ(recieved_path.poses.size(), 0u); + + // Now populated with content, should publish + optimal_trajectory.resize({20, 2}); + for (unsigned int i = 0; i != optimal_trajectory.shape()[0] - 1; i++) { + optimal_trajectory(i, 0) = static_cast(i); + optimal_trajectory(i, 1) = static_cast(i); + } + vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + vis.visualize(bogus_path); + + rclcpp::spin_some(node->get_node_base_interface()); + + // Should have a 20 points path in the map frame and with same stamp than velocity command + EXPECT_EQ(recieved_path.poses.size(), 20u); + EXPECT_EQ(recieved_path.header.frame_id, "fkmap"); + EXPECT_EQ(recieved_path.header.stamp.sec, cmd_stamp.sec); + EXPECT_EQ(recieved_path.header.stamp.nanosec, cmd_stamp.nanosec); + + tf2::Quaternion quat; + for (unsigned int i = 0; i != recieved_path.poses.size() - 1; i++) { + // Poses should be in map frame too + EXPECT_EQ(recieved_path.poses[i].header.frame_id, "fkmap"); + + // Check positions are correct + EXPECT_EQ(recieved_path.poses[i].pose.position.x, static_cast(i)); + EXPECT_EQ(recieved_path.poses[i].pose.position.y, static_cast(i)); + EXPECT_EQ(recieved_path.poses[i].pose.position.z, 0.06); + + // Check orientations are correct + quat.setRPY(0., 0., optimal_trajectory(i, 2)); + auto expected_orientation = tf2::toMsg(quat); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.x, expected_orientation.x); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.y, expected_orientation.y); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.z, expected_orientation.z); + EXPECT_EQ(recieved_path.poses[i].pose.orientation.w, expected_orientation.w); + } +} diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index a8789af62d..4ae1a98ad9 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.5) project(nav2_smac_planner) -set(CMAKE_BUILD_TYPE Release) #Debug, Release - find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(angles REQUIRED) diff --git a/navigation2/CMakeLists.txt b/navigation2/CMakeLists.txt index 763c95c1e9..3b82e6c002 100644 --- a/navigation2/CMakeLists.txt +++ b/navigation2/CMakeLists.txt @@ -1,10 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(navigation2) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) + +nav2_package() ament_package() diff --git a/navigation2/package.xml b/navigation2/package.xml index 4d7fffeb9c..45ad44912b 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -11,6 +11,7 @@ Apache-2.0 ament_cmake + nav2_common nav2_amcl