ROS 机器人模型节点的运动控制原理

深藏阁楼爱情的钟 2022-12-31 05:26 140阅读 0赞

一般我们启动机器人模型是通过以下的.launch文件启动，如下所示：

其中机器人模型（.urdf或者.xacro文件）是通过上述launch文件的第一条命令如下所示，这条命令读取文件中描述的机器人模型

机器人模型通过上述命令给robot\_description赋值，可以看到其前面是param， 这个是指定ros 参数服务器中的参数值，而打开rviz之后rviz就是直接从参数服务器中读取机器人描述文件， 然后进行显示，use\_gui也是如此对显示产生的影响.。另外，还发起了两个发布者节点分别发布joint\_states和robot\_state，这也就是会由joint\_states话题的原因，赋的值为：’$ (find xacro)/xacro --inorder’ 在输入参数’为’$ (arg model)’ 情况下的的运行结果。

至于其中启动的两个节点，joint\_state\_publisher 和 robot\_state\_publisher，可以查看[ROS Answer上相关的解释][ROS Answer]。[joint\_state\_publisher][joint_state_publisher]从ROS参数服务器中读取`robot_description`参数, 找到所有non-fixed joint，发布他们的 JointState 消息到 /joint\_states 话题，而robot\_state\_publisher从 /joint\_states 话题中获取机器人joint角度作为输入，使用机器人的运动学树模型计算出机器人link的3D姿态，然后将其发布到话题 /tf 和  /tf\_static 。

为了验证joint\_state\_publisher 和 robot\_state\_publisher他们之间的关系，我们自己编写了 /joint\_states 的话题发布程序，用于发布机器人模型的non-fixed joint节点，然后robot\_state\_publisher从 /joint\_states 话题中获取机器人joint角度作为输入，在将其发布到话题 /tf 和  /tf\_static 。我们可以通过启动rviz观察机器人的关节运动控制是否符合我们自己编写的/joint\_states运动控制信息。 其发布/joint\_states节点的控制程序如下：

#include <string>
    #include <ros/ros.h>
    #include <sensor_msgs/JointState.h>
    #include <tf/transform_broadcaster.h>
    
    int main(int argc, char** argv) {
        ros::init(argc, argv, "state_publisher");
        ros::NodeHandle n;
        //发布joint_states节点
        ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
        tf::TransformBroadcaster broadcaster;
        ros::Rate loop_rate(30);
    
        const double degree = M_PI/180;
    
        // 机器人模型的关节运动变量
        double inc= 0.005, base_arm_inc= 0.005, arm1_armbase_inc= 0.005, arm2_arm1_inc= 0.005;
        double angle= 0 ,base_arm = 0, arm1_armbase = 0, arm2_arm1 = 0;
        // 发布odom坐标到机器人坐标的TF变换
        geometry_msgs::TransformStamped odom_trans;
        sensor_msgs::JointState joint_states;
        odom_trans.header.frame_id = "odom"; //odom名称
        odom_trans.child_frame_id = "base_footprint"; //机器人地盘
    
        while (ros::ok()) {
            //update joint_state
            joint_states.header.stamp = ros::Time::now();
            joint_states.name.resize(7);  //定义7个运动节点
            joint_states.position.resize(7);
            joint_states.name[0] ="base_to_arm_base"; //与机器人地盘相连的旋转转盘
            joint_states.position[0] = base_arm;
            joint_states.name[1] ="arm1_to_arm_base"; //与机器人旋转底座相连的第一节机械臂
            joint_states.position[1] = arm1_armbase;
            joint_states.name[2] ="arm2_to_arm1"; //与第一节机械臂相连的第二节机械臂
            joint_states.position[2] = arm2_arm1;
    
    	    joint_states.name[3] ="joint_wheel_1"; //以下为四个轮子
            joint_states.position[3] = 0;
    	    joint_states.name[4] ="joint_wheel_2";
            joint_states.position[4] = 0;
    	    joint_states.name[5] ="joint_wheel_3";
            joint_states.position[5] = 0;
    	    joint_states.name[6] ="joint_wheel_4";
            joint_states.position[6] = 0;
    
            // update transform
            // (moving in a circle with radius)
            odom_trans.header.stamp = ros::Time::now();
            odom_trans.transform.translation.x = cos(angle);
            odom_trans.transform.translation.y = sin(angle);
            odom_trans.transform.translation.z = 0.0;
            odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);
    
            //send the joint state and transform
            joint_pub.publish(joint_states); //发布joint_states消息
            broadcaster.sendTransform(odom_trans);  //发布tf变换
    
    
    	    //以下为更新机器人可动节点的状态
            arm2_arm1 += arm2_arm1_inc;
            if (arm2_arm1<-1.5 || arm2_arm1>1.5) arm2_arm1_inc *= -1;
            
    		  arm1_armbase += arm1_armbase_inc;            
            if (arm1_armbase>1.2 || arm1_armbase<-1.0) arm1_armbase_inc *= -1;
    
            base_arm += base_arm_inc;
            if (base_arm>1. || base_arm<-1.0) base_arm_inc *= -1;
    		
    		angle += degree/4;
    
            // This will adjust as needed per iteration
            loop_rate.sleep();
        }
        
        return 0;
    }

机器人的模型文件如下所示：

<?xml version="1.0"?>
    <robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
    
       <xacro:include filename="$(find robot_joint)/urdf/robot_body.xacro"/>
    
       <robot_body/>
    </robot>

robot\_body.xacro文件如下所示：

<?xml version="1.0"?>
    <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    
        <xacro:property name="length_wheel" value="0.12" />
        <xacro:property name="radius_wheel" value="0.18" />
        <xacro:macro name="default_inertial" params="mass">
            <inertial>
              <mass value="${mass}"/>
              <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
            </inertial>
        </xacro:macro>
    
       <xacro:macro name="wheel" params="parent number lengthwheel radiuswheel locationX locationY locationZ">
          <joint name="joint_wheel_${number}" type="continuous">
              <axis xyz="0 0 1" />
              <anchor xyz="0 0 0" />
              <limit effort="100" velocity="100" />
              <origin xyz="${locationX} ${locationY} ${locationZ}" rpy="1.5707 0 0" />
              <parent link="${parent}"/>
              <child link="wheel_${number}"/>
          </joint>
          <link name="wheel_${number}">
            <visual>
              <origin xyz="0 0 0" rpy="0 0 0"/>
              <geometry>
                <cylinder length="${lengthwheel}" radius="${radiuswheel}"/>
              </geometry>
              <material name="white1">
                <color rgba="0.7 1 1 0.9"/>
              </material>
            </visual>
            <collision>
              <origin xyz="0 0 0" rpy="0 0 0"/>
              <geometry>
                <cylinder length="${lengthwheel}" radius="${radiuswheel}"/>
              </geometry>
            </collision>
            <xacro:default_inertial mass="2"/>
          </link>
       </xacro:macro>  
    
    <xacro:macro name="robot_body">
      <link name="base_footprint">
        <visual>
          <geometry>
            <cylinder length="0.001" radius="0.1"/>
          </geometry>
          <material name="white">
            <color rgba="1 1 1 1"/>
          </material>
          <origin rpy="0 0 0" xyz="0 0 0"/>
        </visual>
      </link>
    
      <joint name="base_footprint_joint" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <parent link="base_footprint"/>
        <child link="base_link"/>
      </joint>
    
      <link name="base_link">
        <visual>
          <geometry>
            <box size="0.8 1.2 0.3"/>
          </geometry>
          <origin rpy="0 0 1.57079" xyz="0 0 0.15"/>
          <material name="yellow">
            <color rgba="1 0.4 0 1"/>
          </material>
        </visual>
        <collision>
          <geometry>
            <box size="0.8 1.2 0.1"/>
          </geometry>
        </collision>
        <xacro:default_inertial mass="10"/>
      </link>
    
      <link name="arm_base">
          <visual>
            <geometry>
              <box size="0.3 0.3 0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="color1">
              <color rgba="0.3 0.5 0.6 1"/>
            </material>
          </visual>
          <collision>
            <geometry>
              <box size="0.1 0.1 0.1"/>
            </geometry>
          </collision>
        <xacro:default_inertial mass="1"/>
      </link> 
    
      <joint name="base_to_arm_base" type="continuous">
        <parent link="base_link"/>
        <child link="arm_base"/>
        <axis xyz="0 0 1"/>
        <origin xyz="0 0 0.3"/>
      </joint>
    
      <link name="arm1">
        <visual>
          <geometry>
            <box size="0.05 0.05 0.5"/>
          </geometry>
          <origin rpy="0 0 0" xyz="0 0 0.2"/>
          <material name="yellow">
            <color rgba="1 0.4 0 1"/>
          </material>
        </visual>
        <collision>
          <geometry>
            <box size="0.05 0.05 0.5"/>
          </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>
      </link>
    
      <joint name="arm1_to_arm_base" type="revolute">
        <parent link="arm_base"/>
        <child link="arm1"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.15"/>
        <limit effort="1000.0" lower="-1.0" upper="1.0" velocity="0.5"/>
      </joint>
    
      <link name="arm2">
        <visual>
          <geometry>
            <box size="0.05 0.05 0.5"/>
          </geometry>
          <origin rpy="0 0 0" xyz="0 0 0.25"/>
          <material name="color2">
            <color rgba="0.8 0.8 0.5 1"/>
          </material>
        </visual>
        <collision>
          <geometry>
            <box size="0.05 .05 0.5"/>
          </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>
      </link>
    
      <joint name="arm2_to_arm1" type="revolute">
        <parent link="arm1"/>
        <child link="arm2"/>
        <axis xyz="0 1 0"/>
        <origin xyz="0.0 0 0.45"/>
        <limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>
      </joint>  
    
       <wheel parent="base_link" number="1" lengthwheel="${length_wheel}" radiuswheel="${radius_wheel}" locationX="0.4" locationY="0.48" locationZ="0"/>
       <wheel parent="base_link" number="2" lengthwheel="${length_wheel}" radiuswheel="${radius_wheel}" locationX="-0.4" locationY="0.48" locationZ="0"/>
       <wheel parent="base_link" number="3" lengthwheel="${length_wheel}" radiuswheel="${radius_wheel}" locationX="0.4" locationY="-0.48" locationZ="0"/>
       <wheel parent="base_link" number="4" lengthwheel="${length_wheel}" radiuswheel="${radius_wheel}" locationX="-0.4" locationY="-0.48" locationZ="0"/>
    
    </xacro:macro>
    </robot>

launch启动文件：

把上述的文件放在工作空间中编译，然后启动文件命令：

roslaunch robot_joint state_xacro.launch

这时rviz显示如下的模型：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM0OTExNjM2_size_16_color_FFFFFF_t_70][]

其机器人模型的各个可动关节的运动受state\_publisher.cpp程序发布的/joint\_states节点信息控制。这个例子我们验证了joint\_state\_publisher 和 robot\_state\_publisher他们之间的关系，我们通过编写/joint\_states 的话题发布程序，用于发布机器人模型的non-fixed joint节点，然后robot\_state\_publisher从 /joint\_states 话题中获取机器人joint角度作为输入，在将其发布到话题 /tf 和  /tf\_static 。我们可以通过启动rviz观察机器人的关节运动控制是否符合我们自己编写的/joint\_states运动控制信息。

在终端中输入如下命令，可以查看机器人模型的TF框架树

rosrun rqt_tf_tree rqt_tf_tree

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM0OTExNjM2_size_16_color_FFFFFF_t_70 1][]

输入以下命令，可以查看/joint\_states主题的发布者与接收者

rostopic info /joint_states

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM0OTExNjM2_size_16_color_FFFFFF_t_70 2][]

输入以下命令，可以查看/joint\_states携带的信息

rostopic echo /joint_states

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM0OTExNjM2_size_16_color_FFFFFF_t_70 3][]

其中name为控制的关节名称，position为需要设置的关节位置，velocity为需要设置的关节速度，effort为需要设置的最大功率，速度一定时决定机械手停止的最大握力。

其完整功能包的链接：

[https://download.csdn.net/download/qq\_34911636/13977006][https_download.csdn.net_download_qq_34911636_13977006]

[ROS Answer]: http://answers.ros.org/question/207728/why-use-joint_state_publisher/
[joint_state_publisher]: http://wiki.ros.org/joint_state_publisher
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM0OTExNjM2_size_16_color_FFFFFF_t_70]: /images/20221120/05b7730cd5034a66828204b219161fe6.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM0OTExNjM2_size_16_color_FFFFFF_t_70 1]: /images/20221120/4e4bddee2e0f4b9abcf8b409a1ab4b4e.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM0OTExNjM2_size_16_color_FFFFFF_t_70 2]: /images/20221120/4e2649df14824e46a68da0af3d595618.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM0OTExNjM2_size_16_color_FFFFFF_t_70 3]: /images/20221120/a25907b0b721482f98700d955f7d1ef4.png
[https_download.csdn.net_download_qq_34911636_13977006]: https://download.csdn.net/download/qq_34911636/13977006