Class KinematicParameters
Defined in File kinematic_parameters.hpp
Class Documentation
-
class KinematicParameters
One representation of the robot’s kinematics.
Public Types
-
using Ptr = std::shared_ptr<KinematicParameters>
Public Functions
-
KinematicParameters()
-
void initialize(const base2d_kinematics_msgs::msg::Base2DKinematics &msg)
-
inline double getMinX()
-
inline double getMaxX()
-
inline double getAccX()
-
inline double getDecelX()
-
inline double getMinY()
-
inline double getMaxY()
-
inline double getAccY()
-
inline double getDecelY()
-
inline double getMinSpeedXY()
-
inline double getMinTheta()
-
inline double getMaxTheta()
-
inline double getAccTheta()
-
inline double getDecelTheta()
-
inline double getMinSpeedTheta()
-
base2d_kinematics_msgs::msg::Base2DKinematics toMsg() const
-
void clamp(double &x, double &y, double &theta)
-
bool isValidSpeed(double x, double y, double theta)
Check to see whether the combined x/y/theta velocities are valid.
This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta. The speed is valid if 1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative) AND 2) min_speed_xy is negative or min_speed_theta is negative or hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta.
In English, it makes sure the diagonal motion is not too fast, and that the velocity is moving in some meaningful direction.
In Latin, quod si motus sit signum quaerit et movere ieiunium et significantissime comprehendite.
- Returns:
True if the magnitude hypot(x,y) and theta are within the robot’s absolute limits
Calculate the velocity after a set period of time, given the desired velocity and kinematics.
- Parameters:
cmd_vel – Desired velocity
start_vel – starting velocity
dt – amount of time in seconds
- Returns:
new velocity after dt seconds
Use the robot’s kinematic model to calculate new positions for the robot.
- Parameters:
start_pose – Starting pose
vel – Actual robot velocity (assumed to be within acceleration limits)
dt – amount of time in seconds
- Returns:
New pose after dt seconds
Protected Functions
Protected Attributes
-
double min_vel_x_
-
double min_vel_y_
-
double max_vel_x_
-
double max_vel_y_
-
double max_vel_theta_
-
double min_speed_xy_
-
double max_speed_xy_
-
double min_speed_theta_
-
double acc_lim_x_
-
double acc_lim_y_
-
double acc_lim_theta_
-
double decel_lim_x_
-
double decel_lim_y_
-
double decel_lim_theta_
-
double min_speed_xy_sq_
-
double max_speed_xy_sq_
-
rclcpp::Subscription<base2d_kinematics_msgs::msg::Base2DKinematics>::SharedPtr kinematics_sub_ = {nullptr}
-
rclcpp::Publisher<base2d_kinematics_msgs::msg::Base2DKinematics>::SharedPtr kinematics_pub_ = {nullptr}
-
rclcpp::Node::SharedPtr node_ = {nullptr}
-
using Ptr = std::shared_ptr<KinematicParameters>