diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 97508a0173a..fea0c7e2b87 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -296,13 +296,15 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + recovery_plugins: ["spin", "backup", "wait", "assisted_teleop"] spin: plugin: "nav2_recoveries/Spin" backup: plugin: "nav2_recoveries/BackUp" wait: plugin: "nav2_recoveries/Wait" + assisted_teleop: + plugin: "nav2_recoveries/AssistedTeleop" global_frame: odom robot_base_frame: base_link transform_timeout: 0.1 diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index b436f98cbd7..711683a9086 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -69,6 +69,10 @@ class CostmapTopicCollisionChecker * @brief Returns if a pose is collision free */ bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose); + /** + * @brief Returns costmap resolution + */ + double getCostmapResolution(); protected: /** diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index 225e6ac3fb8..6d8e10fbc47 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -89,6 +89,11 @@ double CostmapTopicCollisionChecker::scorePose( return collision_checker_.footprintCost(getFootprint(pose)); } +double CostmapTopicCollisionChecker::getCostmapResolution() +{ + return costmap_sub_.getCostmap()->getResolution(); +} + Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose) { Footprint footprint; diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 80986c1a515..cea5c2dcc1f 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/LoadMap.srv" "srv/SaveMap.srv" "action/BackUp.action" + "action/AssistedTeleop.action" "action/ComputePathToPose.action" "action/ComputePathThroughPoses.action" "action/FollowPath.action" diff --git a/nav2_msgs/action/AssistedTeleop.action b/nav2_msgs/action/AssistedTeleop.action new file mode 100644 index 00000000000..b029380632c --- /dev/null +++ b/nav2_msgs/action/AssistedTeleop.action @@ -0,0 +1,7 @@ +#goal definition +builtin_interfaces/Duration time +--- +#result definition +builtin_interfaces/Duration total_elapsed_time +--- +#feedback diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_recoveries/CMakeLists.txt index 8af7c5b2f03..94f883ab1d9 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/nav2_recoveries/CMakeLists.txt @@ -71,6 +71,14 @@ ament_target_dependencies(nav2_wait_recovery ${dependencies} ) +add_library(nav2_assisted_teleop_recovery SHARED + plugins/assisted_teleop.cpp +) + +ament_target_dependencies(nav2_assisted_teleop_recovery + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml) # Library @@ -99,6 +107,7 @@ install(TARGETS ${library_name} nav2_backup_recovery nav2_spin_recovery nav2_wait_recovery + nav2_assisted_teleop_recovery ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -128,6 +137,7 @@ ament_export_libraries(${library_name} nav2_backup_recovery nav2_spin_recovery nav2_wait_recovery + nav2_assisted_teleop_recovery ) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_recoveries/plugins/assisted_teleop.cpp b/nav2_recoveries/plugins/assisted_teleop.cpp new file mode 100644 index 00000000000..e1872cca98e --- /dev/null +++ b/nav2_recoveries/plugins/assisted_teleop.cpp @@ -0,0 +1,277 @@ +// Copyright (c) 2021 Anushree Sabnis +// +// 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 +#include +#include +#include +#include + +#include "assisted_teleop.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" +#include "nav2_util/node_utils.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" + +namespace nav2_recoveries +{ +using namespace std::chrono_literals; //NOLINT + +AssistedTeleop::AssistedTeleop() +: action_server_(nullptr), + cycle_frequency_(10.0) +{ +} + +void +AssistedTeleop::cleanup() +{ + action_server_.reset(); + vel_pub_.reset(); +} + +void +AssistedTeleop::activate() +{ + RCLCPP_INFO(logger_, "Activating %s", recovery_name_.c_str()); + + vel_pub_->on_activate(); + action_server_->activate(); +} + +void +AssistedTeleop::deactivate() +{ + vel_pub_->on_deactivate(); + action_server_->deactivate(); +} + +void +AssistedTeleop::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf, + std::shared_ptr collision_checker) +{ + node_ = parent; + auto node = node_.lock(); + logger_ = node->get_logger(); + + RCLCPP_INFO(logger_, "Configuring %s", name.c_str()); + + recovery_name_ = name; + tf_ = tf; + collision_checker_ = collision_checker; + + nav2_util::declare_parameter_if_not_declared( + node, + "projection_time", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, + "linear_velocity_threshold_", rclcpp::ParameterValue(0.06)); + + nav2_util::declare_parameter_if_not_declared( + node, + "cmd_vel_topic", rclcpp::ParameterValue(std::string("cmd_vel"))); + + nav2_util::declare_parameter_if_not_declared( + node, + "input_vel_topic", rclcpp::ParameterValue(std::string("cmd_vel_input"))); + + node->get_parameter("global_frame", global_frame_); + node->get_parameter("robot_base_frame", robot_base_frame_); + node->get_parameter("transform_tolerance", transform_tolerance_); + node->get_parameter("projection_time", projection_time_); + node->get_parameter("linear_velocity_threshold", linear_velocity_threshold_); + node->get_parameter("cmd_vel_topic", cmd_vel_topic_); + node->get_parameter("input_vel_topic", input_vel_topic_); + + vel_pub_ = node->create_publisher(cmd_vel_topic_, 1); + + action_server_ = std::make_shared( + node, recovery_name_, + std::bind(&AssistedTeleop::execute, this)); +} + +void +AssistedTeleop::velCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + double angular_vel = msg->angular.z; + geometry_msgs::msg::Vector3Stamped twist_speed; + twist_speed.vector.x = msg->linear.x; + twist_speed.vector.y = msg->linear.y; + twist_speed.vector.z = 0; + moveRobot(computeVelocity(twist_speed, angular_vel)); +} + +void +AssistedTeleop::projectPose( + geometry_msgs::msg::Vector3Stamped & twist_speed, + double angular_vel, double projection_time, geometry_msgs::msg::Pose2D & projected_pose) +{ + // Project Pose by time increment + projected_pose.x += projection_time * ( + twist_speed.vector.x * cos(projected_pose.theta) + + twist_speed.vector.y * sin(projected_pose.theta)); + projected_pose.y += projection_time * ( + twist_speed.vector.x * sin(projected_pose.theta) + + twist_speed.vector.y * cos(projected_pose.theta)); + projected_pose.theta += projection_time * + angular_vel; +} + +geometry_msgs::msg::Twist::UniquePtr +AssistedTeleop::computeVelocity( + geometry_msgs::msg::Vector3Stamped & twist_speed, double angular_vel) +{ + auto cmd_vel = std::make_unique(); + geometry_msgs::msg::Pose2D projected_pose; + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_ERROR(logger_, "Current robot pose is not available."); + } + projected_pose.x = current_pose.pose.position.x; + projected_pose.y = current_pose.pose.position.y; + projected_pose.theta = tf2::getYaw( + current_pose.pose.orientation); + + cmd_vel->linear.x = twist_speed.vector.x; + cmd_vel->linear.y = twist_speed.vector.y; + cmd_vel->angular.z = angular_vel; + + double linear_vel = std::fabs(std::hypot(cmd_vel->linear.x, cmd_vel->linear.y)); + const double dt = collision_checker_->getCostmapResolution() / linear_vel; + int loopcount = 1; + double scaling_factor = 1; + + while (true) { + double time_to_collision = loopcount * dt; + if (time_to_collision >= projection_time_) { + break; + } else { + scaling_factor = projection_time_ / time_to_collision; + + projectPose(twist_speed, angular_vel, time_to_collision, projected_pose); + + if (!collision_checker_->isCollisionFree(projected_pose)) { + RCLCPP_WARN(logger_, "Collision approaching in %.2f seconds", time_to_collision); + cmd_vel->linear.x /= scaling_factor; + cmd_vel->linear.y /= scaling_factor; + cmd_vel->angular.z /= scaling_factor; + break; + } + } + loopcount++; + } + return cmd_vel; +} + +void +AssistedTeleop::moveRobot(geometry_msgs::msg::Twist::UniquePtr cmd_vel) +{ + if (std::fabs(std::hypot(cmd_vel->linear.x, cmd_vel->linear.y)) < linear_velocity_threshold_) { + stopRobot(); + } else { + vel_pub_->publish(std::move(cmd_vel)); + } +} + +void AssistedTeleop::execute() +{ + RCLCPP_INFO(logger_, "Attempting %s", recovery_name_.c_str()); + + auto node = node_.lock(); + // Log a message every second + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + auto timer = node->create_wall_timer( + 1s, + [&]() + {RCLCPP_INFO(logger_, "%s running...", recovery_name_.c_str());}); + + + auto start_time = node->now(); + + // Initialize the ActionT goal and result + auto at_goal = action_server_->get_current_goal(); + auto result = std::make_shared(); + + rclcpp::WallRate loop_rate(cycle_frequency_); + auto assisted_teleop_end_ = (node->now() + at_goal->time).nanoseconds(); + + vel_sub_ = node->create_subscription( + input_vel_topic_, rclcpp::SystemDefaultsQoS(), + std::bind( + &AssistedTeleop::velCallback, + this, std::placeholders::_1)); + + while (rclcpp::ok()) { + if (action_server_->is_cancel_requested()) { + RCLCPP_INFO(logger_, "Canceling %s", recovery_name_.c_str()); + stopRobot(); + vel_sub_.reset(); + result->total_elapsed_time = node->now() - start_time; + action_server_->terminate_all(result); + return; + } + + if (action_server_->is_preempt_requested()) { + RCLCPP_ERROR( + logger_, "Received a preemption request for %s," + " however feature is currently not implemented. Aborting and stopping.", + recovery_name_.c_str()); + stopRobot(); + vel_sub_.reset(); + result->total_elapsed_time = node->now() - start_time; + action_server_->terminate_current(result); + return; + } + + auto current_point = node->now().nanoseconds(); + + auto time_left = + assisted_teleop_end_ - current_point; + + // Enable recovery behavior if we haven't run out of time + if (time_left < 0) { + vel_sub_.reset(); + result->total_elapsed_time = node->now() - start_time; + action_server_->succeeded_current(result); + RCLCPP_INFO( + logger_, + "%s completed successfully", recovery_name_.c_str()); + return; + } + } + loop_rate.sleep(); +} + +void AssistedTeleop::stopRobot() +{ + auto cmd_vel = std::make_unique(); + cmd_vel->linear.x = 0.0; + cmd_vel->linear.y = 0.0; + cmd_vel->angular.z = 0.0; + + vel_pub_->publish(std::move(cmd_vel)); +} +} // namespace nav2_recoveries + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_recoveries::AssistedTeleop, nav2_core::Recovery) diff --git a/nav2_recoveries/plugins/assisted_teleop.hpp b/nav2_recoveries/plugins/assisted_teleop.hpp new file mode 100644 index 00000000000..73c82d2e2bc --- /dev/null +++ b/nav2_recoveries/plugins/assisted_teleop.hpp @@ -0,0 +1,150 @@ +// Copyright (c) 2021 Anushree Sabnis +// +// 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_RECOVERIES__PLUGINS__ASSISTED_TELEOP_HPP_ +#define NAV2_RECOVERIES__PLUGINS__ASSISTED_TELEOP_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/create_timer_ros.h" +#include "geometry_msgs/msg/twist.hpp" +#include "nav2_util/simple_action_server.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_core/recovery.hpp" + +#include "nav2_msgs/action/assisted_teleop.hpp" + +namespace nav2_recoveries +{ + +using namespace std::chrono_literals; //NOLINT + +/** + * @class nav2_recoveries::AssistedTeleop + * @brief An action server recovery base class implementing the action server and basic factory. + */ +class AssistedTeleop : public nav2_core::Recovery +{ +public: + using AssistedTeleopAction = nav2_msgs::action::AssistedTeleop; + using ActionServer = nav2_util::SimpleActionServer; + + /** + * @brief AsistedTeleop constructor + */ + AssistedTeleop(); + + /** + * @brief AsistedTeleop destructor + */ + virtual ~AssistedTeleop() = default; + + /** + * @brief configure the server and Assisted Teleop Action on lifecycle setup + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & name, std::shared_ptr tf, + std::shared_ptr collision_checker) override; + + // Activate server on lifecycle transition + void activate() override; + + // Deactivate server on lifecycle transition + void deactivate() override; + + // Cleanup server on lifecycle transition + void cleanup() override; + +protected: + // Logger + rclcpp::Logger logger_{rclcpp::get_logger("nav2_recoveries")}; + + + /** + * @brief Utility function to project robot pose + * @param twist_speed linear speed of robot + * @param angular_vel angular speed about Z + * @param projection_time time increment to project the pose + * @param projected_pose Pose projected by given linear and angular speeds + */ + void projectPose( + geometry_msgs::msg::Vector3Stamped & twist_speed, double angular_vel, + double projection_time, geometry_msgs::msg::Pose2D & projected_pose); + + /** + * @brief Callback function for velocity subscriber + * @param msg received Twist message + */ + void velCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + /** + * @brief Performs velocity reduction if a collision is imminent + * @param twist_speed linear speed of robot + * @param angular_vel angular speed about Z + */ + geometry_msgs::msg::Twist::UniquePtr computeVelocity( + geometry_msgs::msg::Vector3Stamped & twist_speed, double angular_vel); + + /** + * @brief Move robot by specified velocity + * @param cmd_vel Velocity with which to move the robot + */ + void moveRobot(geometry_msgs::msg::Twist::UniquePtr cmd_vel); + + /** + * @brief Main execution callbacks for the action server, enable's recovery's specific behavior as it waits for timeout + */ + void execute(); + + /** + * @brief Stops the robot with a commanded velocity + */ + void stopRobot(); + + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + // Action Server + std::shared_ptr action_server_; + + // Publishers and Subscribers + std::shared_ptr collision_checker_; + std::shared_ptr tf_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; + rclcpp::Subscription::SharedPtr vel_sub_; + + // User defined parameters + double projection_time_; + double cycle_frequency_; + double linear_velocity_threshold_; + double transform_tolerance_; + std::string global_frame_; + std::string robot_base_frame_; + std::string cmd_vel_topic_; + std::string input_vel_topic_; + + std::string recovery_name_; +}; + +} // namespace nav2_recoveries + +#endif // NAV2_RECOVERIES__PLUGINS__ASSISTED_TELEOP_HPP_ diff --git a/nav2_recoveries/recovery_plugin.xml b/nav2_recoveries/recovery_plugin.xml index b8ccaca0ad5..32fa221b2e7 100644 --- a/nav2_recoveries/recovery_plugin.xml +++ b/nav2_recoveries/recovery_plugin.xml @@ -16,4 +16,10 @@ + + + + + + \ No newline at end of file