102 lines
3.5 KiB
C++
102 lines
3.5 KiB
C++
#include "toid_behaviors/simple_translate_x.hpp"
|
|
|
|
#include <cmath>
|
|
|
|
#include "angles/angles.h"
|
|
#include "tf2/convert.hpp"
|
|
|
|
namespace toid
|
|
{
|
|
|
|
SimpleTranslateXBehavior::SimpleTranslateXBehavior() : SimpleMove<TranslateAction>() {}
|
|
SimpleTranslateXBehavior::~SimpleTranslateXBehavior() {}
|
|
|
|
void SimpleTranslateXBehavior::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.007));
|
|
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_);
|
|
}
|
|
|
|
ResultStatus SimpleTranslateXBehavior::onStart(
|
|
const std::shared_ptr<const TranslateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
|
|
const geometry_msgs::msg::Twist & vel)
|
|
{
|
|
target_distance_ = command->distance;
|
|
target_angle_ = tf2::getYaw(pose.orientation);
|
|
target_sign_ = (target_distance_ < 0) ? -1.0 : 1.0;
|
|
max_vel_speed_ = command->max_speed;
|
|
|
|
if(command->max_speed == 0) {
|
|
auto node = node_.lock();
|
|
node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
|
|
}
|
|
|
|
target_distance_ *= target_sign_;
|
|
|
|
initial_pose_ = pose;
|
|
speed_ = vel.angular.x;
|
|
|
|
return ResultStatus{Status::SUCCEEDED};
|
|
}
|
|
|
|
ResultStatus SimpleTranslateXBehavior::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);
|
|
const double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
|
|
|
|
const double dx = pose.position.x - initial_pose_.position.x;
|
|
const double dy = pose.position.y - initial_pose_.position.y;
|
|
const double dist_passed = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));
|
|
|
|
const double lower_bound = speed_ - control_duration_ * max_vel_accel_;
|
|
const double upper_bound = speed_ + control_duration_ * max_vel_accel_;
|
|
|
|
float vel = std::clamp(max_vel_speed_, lower_bound, upper_bound);
|
|
float max_vel_to_stop =
|
|
0.8 * std::sqrt(2.0 * max_vel_decel_ * std::abs(target_distance_ - dist_passed));
|
|
|
|
vel = std::min(vel, max_vel_to_stop);
|
|
const double w = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
|
|
|
|
if (dist_passed >= target_distance_) {
|
|
out_vel.linear.x = 0;
|
|
out_vel.angular.z = 0;
|
|
return ResultStatus{Status::SUCCEEDED};
|
|
}
|
|
|
|
out_vel.linear.x = target_sign_ * vel;
|
|
out_vel.angular.z = w;
|
|
|
|
speed_ = vel;
|
|
|
|
return ResultStatus{Status::RUNNING};
|
|
}
|
|
|
|
} // namespace toid
|
|
|
|
#include "pluginlib/class_list_macros.hpp"
|
|
PLUGINLIB_EXPORT_CLASS(toid::SimpleTranslateXBehavior, nav2_core::Behavior); |