コード例 #1
0
ファイル: csv_export.cpp プロジェクト: pdrews/ros_kf_control
 void do_pid(const ros::TimerEvent&)
 {
     geometry_msgs::Twist output;
     geometry_msgs::Point pt;
     double x, y, z;
     //Make sure we keep the rest of the output the same.
     output = setpt_;
     // Correct for the sign of the output
     output.angular.z = -commandZ_;
     if(output.angular.z > 0)
         output.angular.z = output.angular.z + motor_zero_;
     else
         output.angular.z = output.angular.z - motor_zero_;
     Pid_.getCurrentPIDErrors(&x, &y, &z);
     pt.x = x;
     pt.y = y;
     pt.z = z;
     debug_pub_.publish(pt);
     if(!manual_override_ && !rc_override_ && 
        (lastCommand_ + timeout_) > ros::Time::now()){
         motor_pub_.publish(output);            
     } else {
         Pid_.reset();
     }
 }
コード例 #2
0
void FreeFloatingPids::InitPID(control_toolbox::Pid &_pid, const ros::NodeHandle&_node, const bool &_use_dynamic_reconfig)
{
    if(_use_dynamic_reconfig)
    {
        // classical PID init
        _pid.init(_node);
    }
    else
    {
        control_toolbox::Pid::Gains gains;

        // Load PID gains from parameter server
        if (!_node.getParam("p", gains.p_gain_))
        {
          ROS_ERROR("No p gain specified for pid.  Namespace: %s", _node.getNamespace().c_str());
          return;
        }
        // Only the P gain is required, the I and D gains are optional and default to 0:
        _node.param("i", gains.i_gain_, 0.0);
        _node.param("d", gains.d_gain_, 0.0);

        // Load integral clamp from param server or default to 0
        double i_clamp;
        _node.param("i_clamp", i_clamp, 0.0);
        gains.i_max_ = std::abs(i_clamp);
        gains.i_min_ = -std::abs(i_clamp);
        _pid.setGains(gains);
    }
}
コード例 #3
0
ファイル: csv_export.cpp プロジェクト: pdrews/ros_kf_control
        AngularRateControl() : nh_("~"),lastIMU_(0.0),
        lastCommand_(0.0), timeout_(1.5),
        joy_control_("/teleop/twistCommand"), auto_control_("/mux/safeCommand"),
        manual_override_(false), rc_override_(true)
         {
            nh_.param("kp",kp_,1.0);
            nh_.param("ki",ki_,0.0);
            nh_.param("kd",kd_,0.0);
            nh_.param("imax",imax_,1E9);
            nh_.param("period",period_,0.05);
            nh_.param("motor_zero",motor_zero_,0.0);
            nh_.param("input_scale",input_scale_,1.0);
            double dTimeout;
            nh_.param("timeout",dTimeout,1.5);
            timeout_ = ros::Duration(dTimeout);

            Pid_.initPid(kp_,ki_,kd_,imax_,-imax_);

            // Make sure TF is ready
            ros::Duration(0.5).sleep();

            // Subscribe to the velocity command and setup the motor publisher
            imu_sub_ = nh_.subscribe("imu",1,&AngularRateControl::imu_callback,this);
            cmd_sub_ = nh_.subscribe("command",1,&AngularRateControl::cmd_callback,this);
            mux_sub_ = nh_.subscribe("/mux/selected",1,&AngularRateControl::mux_callback,this);
            rc_sub_ = nh_.subscribe("/sense",1,&AngularRateControl::rc_callback,this);
            motor_pub_ = nh_.advertise<geometry_msgs::Twist>("motors",1);
            debug_pub_ = nh_.advertise<geometry_msgs::Point>("debug",1);
            //Create a callback for the PID loop
            pid_loop_ = nh_.createTimer(ros::Duration(period_), &AngularRateControl::do_pid, this);

            f_ = boost::bind(&AngularRateControl::config_callback, this, _1, _2);
            server_.setCallback(f_);

        }
コード例 #4
0
ファイル: csv_export.cpp プロジェクト: pdrews/ros_kf_control
 void imu_callback(const sensor_msgs::Imu& msg) {
    if(lastIMU_ == ros::Time(0.0))
        lastIMU_ = ros::Time::now() - ros::Duration(0.01);
    // setpt_ and msg have opposite senses of Z, so we add
    double error = setpt_.angular.z + msg.angular_velocity.z;
    ros::Duration deltaT = msg.header.stamp - lastIMU_;
    lastIMU_ = msg.header.stamp;
    commandZ_ = Pid_.updatePid(error, deltaT);
 
 }
コード例 #5
0
void PositionCommand::pidGainLoad (control_toolbox::Pid &pid , control_toolbox::Pid::Gains & G, const ros::NodeHandle &n )
{
    double Kp,Ki,Kd,IMax,IMin;
    n.param<double>("Kp"     ,G.p_gain_  , 0.0);
    n.param<double>("Ki"     ,G.i_gain_  , 0.0);
    n.param<double>("Kd"     ,G.d_gain_  , 0.0);
    n.param<double>("IMax"   ,G.i_max_   , 0.0);
    n.param<double>("IMin"   ,G.i_min_   , 0.0);

    std::cout   << "Kp = "      << G.p_gain_
                << " Ki = "     << G.i_gain_
                << " Kd = "     << G.d_gain_
                << " IMax = "   << G.i_max_
                << " IMin = "   << G.i_min_
                << std::endl;

    pid.setGains(Kp,Ki,Kd,IMax,IMin);
}
コード例 #6
0
ファイル: csv_export.cpp プロジェクト: pdrews/ros_kf_control
 void config_callback(rate_control::RateControlConfig &config, uint32_t level)
 {
     Pid_.setGains(config.kp, config.ki, config.kd, config.imax, -config.imax);
     motor_zero_ = config.motor_zero;
     input_scale_ = config.input_scale;
 }