cl_move_base_z::forward_local_planner Namespace Reference

Classes
class	ForwardLocalPlanner

Functions
tf::Stamped< tf::Pose >	optionalRobotPose (costmap_2d::Costmap2DROS *costmapRos)
void	clamp (geometry_msgs::Twist &cmd_vel, double max_linear_x_speed_, double max_angular_z_speed_)

Function Documentation

clamp()

void cl_move_base_z::forward_local_planner::clamp	(	geometry_msgs::Twist &	cmd_vel,
		double	max_linear_x_speed_,
		double	max_angular_z_speed_
	)

Definition at line 224 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_;
            ROS_WARN_STREAM("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_;
            ROS_WARN_STREAM("lets go to maximum angular capacity: " << cmd_vel);
        }
    }
}

optionalRobotPose()

tf::Stamped< tf::Pose > cl_move_base_z::forward_local_planner::optionalRobotPose

(

costmap_2d::Costmap2DROS *

costmapRos

)

Definition at line 216 of file forward_local_planner.cpp.

{
    tf::Stamped<tf::Pose> tfpose;
    costmapRos->getRobotPose(tfpose);
    return tfpose;
}

Referenced by cl_move_base_z::forward_local_planner::ForwardLocalPlanner::computeVelocityCommands().

Here is the caller graph for this function: