35 lines
922 B
C++
35 lines
922 B
C++
#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);
|
|
|
|
void step(
|
|
const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
|
|
bool backwards = false);
|
|
};
|
|
} // namespace toid
|