PIBOT的ros中driver

目標

• 串口數(shù)據(jù)發(fā)送與接收
• 訂閱cmd_vel topic下發(fā)至下位機
• 根據(jù)下位機的反饋發(fā)布odom topic和odom tf

串口數(shù)據(jù)發(fā)送與接收

  boost::shared_ptr<Transport> trans;
  boost::shared_ptr<Dataframe> frame;

    trans = boost::make_shared<Serial_transport>(bdg.port, bdg.buadrate);
    frame = boost::make_shared<Simple_dataframe>(trans.get());

這里實現(xiàn)一個上位機的Serial_transport和一個Simple_dataframe即可完成

subscribe cmd_vel

cmd_vel_sub = nh.subscribe(bdg.cmd_vel_topic, 1000, &BaseDriver::cmd_vel_callback, this);

void BaseDriver::cmd_vel_callback(const geometry_msgs::Twist& vel_cmd)
{
    ROS_INFO_STREAM("cmd_vel:[" << vel_cmd.linear.x << " " << vel_cmd.linear.y << " " << vel_cmd.angular.z << "]");

    Data_holder::get()->velocity.v_liner_x = vel_cmd.linear.x*100;
    Data_holder::get()->velocity.v_liner_y = vel_cmd.linear.y*100;
    Data_holder::get()->velocity.v_angular_z = vel_cmd.angular.z*100;

    need_update_speed = true;
}

void BaseDriver::update_speed()
{
    if (need_update_speed)
    {
        ROS_INFO_STREAM("update_speed");
        need_update_speed = !(frame->interact(ID_SET_VELOCITY));
    }
}

代碼容易理解段誊，訂閱消息泌类，回調(diào)函數(shù)中設(shè)置標識健田，循環(huán)中根據(jù)標識下發(fā)設(shè)置指令

publish odom

ROS機器人底盤(9)-base_link聋丝、odom凉当、map關(guān)系中有個odom publisher的例子，基本拿過來就可以

void BaseDriver::update_odom()
{
    frame->interact(ID_GET_ODOM);
    
    ros::Time current_time = ros::Time::now(); 

    float x = Data_holder::get()->odom.x*0.01;
    float y = Data_holder::get()->odom.y*0.01;
    float th = Data_holder::get()->odom.yaw*0.01;

    float vxy = Data_holder::get()->odom.v_liner_x*0.01;
    float vth = Data_holder::get()->odom.v_angular_z*0.01;

    //ROS_INFO("odom: x=%.2f y=%.2f th=%.2f vxy=%.2f vth=%.2f", x, y ,th, vxy,vth);
    
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

    //send the transform  
    odom_trans.header.stamp = current_time;  
    odom_trans.transform.translation.x = x;  
    odom_trans.transform.translation.y = y;  
    odom_trans.transform.rotation = odom_quat; 
    odom_broadcaster.sendTransform(odom_trans);  

    //publish the message  
    odom.header.stamp = current_time;  
    odom.pose.pose.position.x = x;  
    odom.pose.pose.position.y = y;  
    odom.pose.pose.orientation = odom_quat;  
    odom.twist.twist.linear.x = vxy;  
    odom.twist.twist.angular.z = vth;  
    odom_pub.publish(odom);
}

動態(tài)PID調(diào)節(jié)

概述

底層提供了各個電機輸入輸出的宾巍，參見協(xié)議ROS機器人底盤(3)-通訊協(xié)議

配置

params.yaml打開out_pid_debug_enable

port: /dev/pibot
buadrate: 115200

base_frame: base_link

# topic
cmd_vel_topic: cmd_vel

#pid debug
out_pid_debug_enable: true

PID曲線

運行

roslaunch pibot_bringup bringup.launch
rosrun rqt_plot rqt_plot /motor1_input /motor1_output

即可展示出實時曲線

plot

控制之前最好先架起小車

為了穩(wěn)定給出輸入直接向cmd_vel發(fā)消息

  x: 0.15
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" -r 20

觀察曲線變化輸入輸出基本一致輸出振幅較小即可剩拢；否則打開配置頁面
rosrun rqt_reconfigure rqt_reconfigure

config