diff --git a/toid_behaviors/CMakeLists.txt b/toid_behaviors/CMakeLists.txt index dec5493..b1652f5 100644 --- a/toid_behaviors/CMakeLists.txt +++ b/toid_behaviors/CMakeLists.txt @@ -27,6 +27,8 @@ set( set( SOURCES + src/scl.cpp + src/move_coords.cpp src/simple_move.cpp src/simple_rotate.cpp src/simple_translate_x.cpp diff --git a/toid_behaviors/include/toid_behaviors/move_coords.hpp b/toid_behaviors/include/toid_behaviors/move_coords.hpp new file mode 100644 index 0000000..0bf0170 --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/move_coords.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include "toid_behaviors/simple_move.hpp" +#include "toid_behaviors/scl.hpp" +#include "toid_msgs/action/simple_move_coords.hpp" + +#include "geometry_msgs/msg/pose.hpp" + +namespace toid +{ +using MoveAction = toid_msgs::action::SimpleMoveCoords; +using namespace nav2_behaviors; + +class MoveCoords : public SimpleMove +{ +public: + MoveCoords(); + ~MoveCoords(); + + void configureCB() override; + + ResultStatus onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & vel) override; + + ResultStatus updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel, + geometry_msgs::msg::Twist & out_vel) override; + + virtual nav2_core::CostmapInfoType getResourceInfo() override { + return nav2_core::CostmapInfoType::NONE; + } + +protected: + + SmoothControlLaw scl; + + //Goal + geometry_msgs::msg::Pose target_pose_; + double target_angle_; + double target_sign_; + bool backwards_; + + //State + double last_speed_; + + //Config + double max_vel_accel_; + double max_vel_decel_; + double max_vel_speed_; + double min_vel_speed_; + + double max_angular_speed_; + + double kp_; + double k_phi_; + double k_delta_; + double beta_; + double lambda_; + double slowdown_radius_; +}; + +} // namespace toid + diff --git a/toid_behaviors/include/toid_behaviors/scl.hpp b/toid_behaviors/include/toid_behaviors/scl.hpp new file mode 100644 index 0000000..af8194f --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/scl.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/twist.hpp" + +namespace toid +{ +class SmoothControlLaw +{ +public: + double k_phi = 5.0; + double k_delta = 2.0; + double bbeta = 0.4; + double lam = 2.0; + + double slowdown_radius = 0.20; + double v_linear_min = 0.05; + double v_linear_max = 0.1; + double v_angular_max = 2.0; + + void egocentric_polar( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, + double & r, double & phi, double & delta); + + double curvature(double r, double phi, double delta); + + void calculate_vel( + const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current, + geometry_msgs::msg::Twist & out_speed, bool backwards = false); +}; +} // namespace toid \ No newline at end of file diff --git a/toid_behaviors/src/move_coords.cpp b/toid_behaviors/src/move_coords.cpp new file mode 100644 index 0000000..4a2ebef --- /dev/null +++ b/toid_behaviors/src/move_coords.cpp @@ -0,0 +1,136 @@ +#include "toid_behaviors/move_coords.hpp" + +#include "angles/angles.h" + +namespace toid +{ + +MoveCoords::MoveCoords() : SimpleMove() {} +MoveCoords::~MoveCoords() {} + +void MoveCoords::configureCB() +{ + auto node = node_.lock(); + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_accel", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".max_vel_accel", max_vel_accel_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_decel", rclcpp::ParameterValue(1.0)); + node->get_parameter(behavior_name_ + ".max_vel_decel", max_vel_decel_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_vel_speed", rclcpp::ParameterValue(0.4)); + node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".min_vel_speed", rclcpp::ParameterValue(0.10)); + node->get_parameter(behavior_name_ + ".min_vel_speed", min_vel_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0)); + node->get_parameter(behavior_name_ + ".max_angular_speed", max_angular_speed_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kp", rclcpp::ParameterValue(5.0)); + node->get_parameter(behavior_name_ + ".kp", kp_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kphi", rclcpp::ParameterValue(5.0)); + node->get_parameter(behavior_name_ + ".kphi", k_phi_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".kdelta", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".kdelta", k_delta_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".beta", rclcpp::ParameterValue(0.4)); + node->get_parameter(behavior_name_ + ".beta", beta_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".lambda", rclcpp::ParameterValue(2.0)); + node->get_parameter(behavior_name_ + ".lambda", lambda_); + + nav2_util::declare_parameter_if_not_declared( + node, behavior_name_ + ".slow_down_radius", rclcpp::ParameterValue(0.20)); + node->get_parameter(behavior_name_ + ".slow_down_radius", slowdown_radius_); +} + +ResultStatus MoveCoords::onStart( + const std::shared_ptr command, const geometry_msgs::msg::Pose &, + const geometry_msgs::msg::Twist & vel) +{ + target_pose_.position.x = command->x; + target_pose_.position.y = command->y; + backwards_ = command->backwards; + + target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta)); + target_angle_ = command->theta; + target_sign_ = backwards_ ? -1.0 : 1.0; + + scl.k_phi = k_phi_; + scl.k_delta = k_delta_; + scl.bbeta = beta_; + scl.lam = lambda_; + scl.slowdown_radius = slowdown_radius_; + scl.v_angular_max = max_angular_speed_; + scl.v_linear_min = min_vel_speed_; + scl.v_linear_max = max_vel_speed_; + + last_speed_ = vel.angular.x; + + return ResultStatus{Status::SUCCEEDED}; +} + +ResultStatus MoveCoords::updateVel( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &, + geometry_msgs::msg::Twist & out_vel) +{ + const double current_yaw = tf2::getYaw(pose.orientation); + double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_); + + if (backwards_) { + angle_dist = angles::two_pi_complement(angle_dist); + } + + const double dx = target_pose_.position.x - pose.position.x; + const double dy = target_pose_.position.y - pose.position.y; + + const double dist_left = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_)); + + const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_; + const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_; + + if (dist_left <= 0.001) { + out_vel.linear.x = 0; + out_vel.angular.z = 0; + return ResultStatus{Status::SUCCEEDED}; + } + + double vel = max_vel_speed_; + double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left); + vel = std::min(vel, max_vel_to_stop); + + + if (dist_left <= 0.02) { + out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound); + out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_); + last_speed_ = out_vel.linear.x; + return ResultStatus{Status::RUNNING}; + } + + scl.v_linear_max = target_sign_ * std::clamp(target_sign_ * vel, lower_bound, upper_bound); + scl.calculate_vel(target_pose_, pose, out_vel, backwards_); + last_speed_ = out_vel.linear.x; + + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); + + return ResultStatus{Status::RUNNING}; +} + +} // namespace toid + + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(toid::MoveCoords, nav2_core::Behavior); \ No newline at end of file diff --git a/toid_behaviors/src/scl.cpp b/toid_behaviors/src/scl.cpp new file mode 100644 index 0000000..12ad53f --- /dev/null +++ b/toid_behaviors/src/scl.cpp @@ -0,0 +1,62 @@ +#include "toid_behaviors/scl.hpp" + +#include + +#include "angles/angles.h" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + + +namespace toid { +void SmoothControlLaw::calculate_vel( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, + geometry_msgs::msg::Twist &out_speed, bool backwards) +{ + double r, phi, delta; + egocentric_polar(target, current, backwards, r, phi, delta); + + double curvature = this->curvature(r, phi, delta); + curvature = backwards? -curvature : curvature; + + double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam)); + + v = std::min(v_linear_max * (r / slowdown_radius), v); + v = std::clamp(v, v_linear_min, v_linear_max); + v = backwards ? -v : v; + + const double w = curvature * v; + const double w_bound = std::clamp(w, -v_angular_max, v_angular_max); + + v = (curvature != 0.0) ? (w_bound / curvature) : v; + out_speed.linear.x = v; + out_speed.angular.z = w_bound; +} + +double SmoothControlLaw::curvature(double r, double phi, double delta) +{ + if (r < 1e-6) { + return 0.0; + } + const float prop = k_delta * (delta - std::atan(-k_phi * phi)); + const float feedback = (1 + k_phi / (1 + (k_phi * phi) * (k_phi * phi))) * std::sin(delta); + + return -1.0 / r * (prop + feedback); +} + +void SmoothControlLaw::egocentric_polar( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, double & r, + double & phi, double & delta) +{ + const double dx = target.position.x - current.position.x; + const double dy = target.position.y - current.position.y; + const double los = backwards ? (std::atan2(-dy, dx) + M_PI) : std::atan2(-dy, dx); + + const double ttheta = tf2::getYaw(target.orientation); + const double ctheta = tf2::getYaw(current.orientation); + + r = std::hypot(dx, dy); + phi = angles::normalize_angle(ttheta + los); + delta = angles::normalize_angle(ctheta + los); +} + +} \ No newline at end of file diff --git a/toid_behaviors/toid_behaviors.xml b/toid_behaviors/toid_behaviors.xml index 48af7a1..10cc958 100644 --- a/toid_behaviors/toid_behaviors.xml +++ b/toid_behaviors/toid_behaviors.xml @@ -6,5 +6,8 @@ + + + \ No newline at end of file diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index 1b60254..a0a4761 100644 --- a/toid_msgs/CMakeLists.txt +++ b/toid_msgs/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/SendDouble.srv" + "action/SimpleMoveCoords.action" "action/SimpleRotate.action" "action/SimpleTranslateX.action" ) diff --git a/toid_msgs/action/SimpleMoveCoords.action b/toid_msgs/action/SimpleMoveCoords.action new file mode 100644 index 0000000..65df8ac --- /dev/null +++ b/toid_msgs/action/SimpleMoveCoords.action @@ -0,0 +1,15 @@ +uint8 IGNORE_OBSTACLES=1 + +float64 x +float64 y +float64 theta +uint8 backwards 0 +uint8 mode 0 +--- +uint16 NONE=0 +uint16 TF_ERROR=1 + +builtin_interfaces/Duration total_elapsed_time +uint16 error_code +string error_msg +--- diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 54152e7..54dd3bf 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -22,7 +22,7 @@ behavior_server: local_footprint_topic: local_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint cycle_frequency: 50.0 - behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX"] + behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX", "moveCoords"] spin: plugin: "nav2_behaviors::Spin" backup: @@ -39,6 +39,8 @@ behavior_server: max_angular_decel: 1.0 translateX: plugin: "toid::SimpleTranslateXBehavior" + moveCoords: + plugin: "toid::MoveCoords" local_frame: map global_frame: map robot_base_frame: base_footprint