cl_nav2z::forward_local_planner Namespace Reference

Classes
class	ForwardLocalPlanner

Functions
void	clamp (rclcpp::Node::SharedPtr nh_, geometry_msgs::msg::Twist &cmd_vel, double max_linear_x_speed_, double max_angular_z_speed_)

Function Documentation

clamp()

void cl_nav2z::forward_local_planner::clamp	(	rclcpp::Node::SharedPtr	nh_,
		geometry_msgs::msg::Twist &	cmd_vel,
		double	max_linear_x_speed_,
		double	max_angular_z_speed_ )

Definition at line 273 of file forward_local_planner.cpp.

{
  if (max_angular_z_speed_ == 0 || max_linear_x_speed_ == 0) return;
 
  if (cmd_vel.angular.z == 0)
  {
    cmd_vel.linear.x = max_linear_x_speed_;
  }
  else
  {
    double kurvature = cmd_vel.linear.x / cmd_vel.angular.z;
 
    double linearAuthority = fabs(cmd_vel.linear.x / max_linear_x_speed_);
    double angularAuthority = fabs(cmd_vel.angular.z / max_angular_z_speed_);
    if (linearAuthority < angularAuthority)
    {
      // lets go to maximum linear speed
      cmd_vel.linear.x = max_linear_x_speed_;
      cmd_vel.angular.z = kurvature / max_linear_x_speed_;
      RCLCPP_WARN_STREAM(
        nh_->get_logger(), "k=" << kurvature << "lets go to maximum linear capacity: " << cmd_vel);
    }
    else
    {
      // lets go with maximum angular speed
      cmd_vel.angular.x = max_angular_z_speed_;
      cmd_vel.linear.x = kurvature * max_angular_z_speed_;
      RCLCPP_WARN_STREAM(nh_->get_logger(), "lets go to maximum angular capacity: " << cmd_vel);
    }
  }
}