Inline ActionResult HandleBangBangControl: Precise Position Control for Robots

{ "title": "Inline ActionResult HandleBangBangControl: Precise Position Control for Robots", "description": "This function implements a bang-bang control strategy for precise position control of robots, utilizing longitudinal distance and speed factors to ensure accurate movement and stopping. It incorporates considerations for robot state and path parameters, providing a robust solution for navigating complex environments.", "keywords": "robot control, bang-bang control, position control, longitudinal distance, speed factor, robot state, path parameters, accurate stopping, navigation, environment", "content": "inline ActionResult HandleBangBangControl(const mob::planner::Path &path,\n const RobotStatus &rs,\n mob::control::RegularState &trajectory_follower_state,\n long long int &running_index,\n bool &is_in_bang_bang_control,\n bool &record_first_steering,\n bool &finish_steering,\n const NavPathAction &action,\n bool &stop_sign,\n bool &speed_reverse)\n{ auto current_pose = path.meta.frame_id == kFrameIdOdom ? rs.current_pose_odom.pose : rs.current_pose_map.pose; auto tmp_dist = GetRichPoseDist(current_pose, path.seq.back()); auto d_longitude = std::get<5>(tmp_dist); ROS_WARN_THROTTLE(1.0, "d_longitude: %.4lf", d_longitude);

if (std::abs(d_longitude) < mob_pnc::Config::parameter.gAccurateLongitudinalTolMeters)
{
    // Make the robot absolutely STOP the Motion by Embedded
    pubs.SendToDriver(Config::parameter.U1_WINDUP_VALUE, Config::parameter.U2_WINDUP_VALUE);
    std::string pos_type = "accurate";
    if (path.meta.frame_id == kFrameIdOdom)
    {
        pos_type = "odom_" + pos_type;
        record_final_point(path.seq.back(), rs.current_pose_odom.pose, pos_type);
    }
    else
    {
        pos_type = "map_" + pos_type;
        record_final_point(path.seq.back(), rs.current_pose_map.pose, pos_type);
    }

    trajectory_follower_state.Reset();
    trajectory_follower_state.u2 = rs.steering_drive_report.angle;
    if (!action.path.seq.empty())
    {
        pubs.debug_nav_like_report_pub.publish(
                NavLikeTaskFinishedSummary("Nav2", path.meta.frame_id, rs, path.seq.back()));
    }
    else
    {
        pubs.debug_nav_like_report_pub.publish(
                NavLikeTaskFinishedSummary("Move to point bang-bang", action.frame_id, rs, action.curve_target_pose));
    }
    running_index = 0;
    is_in_bang_bang_control = false;
    record_first_steering = true;
    finish_steering = false;
    stop_sign = false;
    speed_reverse = false;
    return {true, kSuccess};
}
else
{
    if (not is_in_bang_bang_control)
    {
        is_in_bang_bang_control = true;
    }
    // position closed-loop bang-bang control
    double tmp_deadzone_pv = mob_pnc::Config::parameter.DEADZONE_PV;
    if (rs.IsGetPallet())
    {
        double speed_factor = interp(std::abs(d_longitude)/mob_pnc::Config::parameter.gAccurateLongitudinalTolMeters,
                                mob_pnc::Config::parameter.MC_bangbangLonMetersFactor, mob_pnc::Config::parameter.MC_bangbangLonSpeedFactor);
        tmp_deadzone_pv = speed_factor*mob_pnc::Config::parameter.DEADZONE_PV;
        ROS_INFO_THROTTLE(0.5, "bang-bang, d_longitude %.4lf speed_factor %.4lf pv %.4lf %.4lf", d_longitude, speed_factor, tmp_deadzone_pv, mob_pnc::Config::parameter.DEADZONE_PV);
        if(std::abs(rs.steering_drive_report.velocity) > mob_pnc::Config::parameter.DEADZONE_PV
          and d_longitude*rs.steering_drive_report.velocity > 0){
            tmp_deadzone_pv = 0;
            ROS_ERROR("curr_velocity: %.4lf, d_longitude %.4lf, tmp_deadzone_pv %.4lf, make robot stop first", rs.steering_drive_report.velocity, d_longitude, tmp_deadzone_pv);
        }
    }
    ROS_INFO_THROTTLE(5.0, "tmp_deadzone_pv : [%.4lf]", tmp_deadzone_pv);

    double u1 = -copysign(tmp_deadzone_pv, d_longitude);
    if (u1 * rs.steering_drive_report.velocity < 0 and
        not (rs.IsLinearStopped() and rs.IsAngularStopped())){
        ROS_ERROR("In bang-bang the u1 is: %.3f, the current_vel is %.3f", u1, rs.steering_drive_report.velocity);
        u1 = Config::parameter.U1_WINDUP_VALUE;
        speed_reverse = true;
    }
    pubs.SendToDriver(u1, Config::parameter.U2_WINDUP_VALUE);
    ROS_WARN_THROTTLE(1.0, "position closed-loop bang-bang control");
    return {false, ""};
}

}