机器人——ROS程序设计学习笔记（chapter7）

川长思鸟来 2022-05-17 00:34 207阅读 0赞

书籍名称：《Learning ROS for RoboticsProgramming - Second Edition》Chapter7

参考链接：[https://www.cnblogs.com/jinee/p/5047021.html][https_www.cnblogs.com_jinee_p_5047021.html]

1. 一定要记住它使用的是右手坐标系：  
x正方向朝左， y正方向向内， z轴正方向朝上  
2. 构建树结构， 即写link和joint

## 1.几何模型 ##

<?xml version ="1.0"?>
    <robot name = "text1">
    
      
      <link name = "base_link">
        <visual>
          <geometry>
            <cylinder length = "0.6" radius = "0.2"/>  
            
          </geometry>
          <material name = "blue">
           <color rgba = "0 0 0.8 1"/>
          </material>
        </visual>
      </link>
    
      <link name = "right_leg">
        <visual>
          <geometry>
            <box size = "0.6 .2 .1"/>
           
          </geometry>
          <material name = "white">
           <color rgba = "1 1 1 1"/>
          </material>
        </visual>  
      </link>
    
      <joint name = "base_to_right_leg" type = "fixed">
        <parent link = "base_link"/>
        <child link = "right_leg"/> 
      </joint>
    
    </robot>

![70][]

<geometry>中添加几何体信息

<material>中添加材质 颜色（rgba格式）

### 2.调整位置，添加另一条腿 ###

<link name = "right_leg">
        <visual>
          <geometry>
            <box size = "0.6 .2 .1"/>
          </geometry>
          <material name = "white">
           <color rgba = "1 1 1 1"/>
          </material>
          <origin rpy = "0 1.5705 0" xyz = "0 0 -0.3"/>
        </visual>  
      </link>
      <joint name = "base_to_right_leg" type = "fixed">
        <parent link = "base_link"/>
        <child link = "right_leg"/>
        <origin xyz = "0.22 0 0.25"> 
      </joint>
    
    
      <link name = "left_leg">
        <visual>
          <geometry>
            <box size = "0.6 .2 .1"/>
          </geometry>
          <material name = "white">
           <color rgba = "1 1 1 1"/>
          </material>
          <origin rpy = "0 1.5705 0" xyz = "-0.22 0 -0.05"/>
        </visual>  
      </link>
      <joint name = "base_to_left_leg" type = "fixed">
        <parent link = "base_link"/>
        <child link = "left_leg"/> 
        <origin xyz = "-0.22 0 0.25">
      </joint>

![70 1][]

每个link的参考坐标系都在它的底部，并与关节的参考坐标系正交，为了添加尺寸，需要指定偏移从一个link到它的关节的子link, 这通过添加origin到每个节点解决。  
origin表示的是关节相对于父关节的距离和旋转， xyz和rpy（1.5705:90°）

### 3.添加剩余部位 ###

调整一下，全部代码：

<?xml version ="1.0"?>
    <robot name = "text1">
    
      
      <link name = "base_link">
        <visual>
          <geometry>
            <cylinder length = "0.6" radius = "0.2"/>
          </geometry>
          <material name = "blue">
           <color rgba = "0 0 0.8 1"/>
          </material>
        </visual>
      </link>
    
    
      <link name = "right_leg">
        <visual>
          <geometry>
            <box size = "0.6 .2 .1"/>
          </geometry>
          <material name = "white">
           <color rgba = "1 1 1 1"/>
          </material>
          <origin rpy = "0 1.5705 0" xyz = "0 0 -0.3"/>
        </visual>  
      </link>
      <joint name = "base_to_right_leg" type = "fixed">
        <parent link = "base_link"/>
        <child link = "right_leg"/> 
        <origin xyz = "0.22 0 .25"/>
      </joint>
    
    
      <link name = "left_leg">
        <visual>
          <geometry>
            <box size = "0.6 .2 .1"/>
          </geometry>
          <material name = "white">
           <color rgba = "1 1 1 1"/>
          </material>
          <origin rpy = "0 1.5705 0" xyz = "0 0 -0.3"/>
        </visual>  
      </link>
      <joint name = "base_to_left_leg" type = "fixed">
        <parent link = "base_link"/>
        <child link = "left_leg"/>
        <origin xyz = "-0.22 0 .25"/> 
      </joint>
    
     <link name = "head">
      <visual>
       <geometry>
        <sphere radius = "0.2"/>
       </geometry>
       <material name  = "white"/>
      </visual>
     </link>
     <joint name="base_to_head" type="fixed">
       <parent link="base_link"/>
       <child link="head"/>
       <origin xyz="0 0 0.3"/>
      </joint>
    
     <link name = "right_foot">
      <visual>
        <geometry>
         <box size = "0.1 .4 .1"/>
        </geometry>
        <material name = "yellow">
         <color rgba = "1 1 0 1"/>
        </material>
      </visual>
     </link>
     <joint name = "leg_to_right_foot" type = "fixed">
      <parent link = "right_leg"/>
      <child link = "right_foot"/>
      <origin xyz = "0 0 -0.65"/>
     </joint>
      
     <link name = "left_foot">
      <visual>
        <geometry>
         <box size = "0.1 .4 .1"/>
        </geometry>
        <material name = "yellow">
         <color rgba = "1 1 0 1"/>
        </material>
      </visual>
     </link>
     <joint name = "leg_to_left_foot" type = "fixed">
      <parent link = "left_leg"/>
      <child link = "left_foot"/>
      <origin xyz = "0 0 -0.65"/>
     </joint>
    
     <link name = "right_front_wheel">
      <visual>
       <geometry>
        <cylinder length= "0.08" radius = "0.05"/>
       </geometry>
       <material name = "black">
        <color rgba = "0 0 0 1"/>
       </material>
      </visual>
     </link>
     <joint name  = "right_foot_to_front_wheel" type = "fixed">
      <parent link ="right_foot"/>
      <child link = "right_front_wheel"/>
      <origin xyz = "0 0.14 -0.05" rpy = "0 1.5705 0"/>
     </joint>
    
     <link name = "right_back_wheel">
      <visual>
       <geometry>
        <cylinder length= "0.08" radius = "0.05"/>
       </geometry>
       <material name = "black">
        <color rgba = "0 0 0 1"/>
       </material>
      </visual>
     </link>
     <joint name  = "right_foot_to_back_wheel" type = "fixed">
      <parent link ="right_foot"/>
      <child link = "right_back_wheel"/>
      <origin xyz = "0 -0.14 -0.05" rpy =  "0 1.5705 0"/>
     </joint>
    
     <link name = "left_front_wheel">
      <visual>
       <geometry>
        <cylinder length= "0.08" radius = "0.05"/>
       </geometry>
       <material name = "black">
        <color rgba = "0 0 0 1"/>
       </material>
      </visual>
     </link>
     <joint name  = "left_foot_to_front_wheel" type = "fixed">
      <parent link ="left_foot"/>
      <child link = "left_front_wheel"/>
      <origin xyz = "0 0.14 -0.05" rpy =  "0 1.5705 0"/>
     </joint>
    
     <link name = "left_back_wheel">
      <visual>
       <geometry>
        <cylinder length= "0.08" radius = "0.05"/>
       </geometry>
       <material name = "black">
        <color rgba = "0 0 0 1"/>
       </material>
      </visual>
     </link>
     <joint name  = "left_foot_to_back_wheel" type = "fixed">
      <parent link ="left_foot"/>
      <child link = "left_back_wheel"/>
      <origin xyz = "0 -0.14 -0.05" rpy =  "0 1.5705 0"/>
     </joint>
    </robot>

![70 2][]

### 4.添加碰撞collision ###

碰撞只要将link包裹住就可以了

<link name = "base_link">
        <visual>
          <geometry>
            <cylinder length = "0.6" radius = "0.2"/>
          </geometry>
          <material name = "blue">
           <color rgba = "0 0 0.8 1"/>
          </material>
        </visual>
        <collision>
         <geometry>
          <cylinder length = "0.6" radius = "0.2"/>
         </geometry>
        </collision>
      </link>

### 5.xacro ###

使用宏，简化代码，顺便添加inertial

这里使用了三个宏：代码如下(中间代码省略)

<?xml version ="1.0"?>
    <robot name = "text1" xmlns:xacro="http://www.ros.org/wiki/xacro">
    
     <xacro:property name="wheel_length" value="0.08" />
     <xacro:property name="wheel_radius" value="0.05" />
     <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>
    .
    .
    .
    
     <link name = "left_back_wheel">
      <visual>
       <geometry>
        <cylinder length= "${wheel_length}" radius = "${wheel_radius}"/>
       </geometry>
       <material name = "black">
        <color rgba = "0 0 0 1"/>
       </material>
      </visual>
      <collision>
       <geometry>
        <cylinder length= "${wheel_length}" radius = "${wheel_radius}"/>
       </geometry>
      </collision>
      <xacro:default_inertial mass = "1"/>
     </link>
     <joint name  = "left_foot_to_back_wheel" type = "continuous">
      <parent link ="left_foot"/>
      <child link = "left_back_wheel"/>
      <axis xyz = "0 0 1"/>
      <origin xyz = "0 -0.14 -0.05" rpy =  "0 1.5705 0"/>
     </joint>
    
    </robot>

### 6.让机器人动起来 ###

让机器人走圆

cpp如下：

#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_text");
        ros::NodeHandle n;
        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;
    
        // robot state
        double inc= 0.005,wheel_1_inc= -0.05,wheel_2_inc= -0.05,wheel_3_inc= -0.07,wheel_4_inc= -0.07;    //轮子向后转，机器人向前移动，所以这里是负
        double angle= 0,base_head = 0,wheel_1= 0,wheel_2= 0,wheel_3= 0,wheel_4= 0;
        // message declarations
        geometry_msgs::TransformStamped odom_trans;
        sensor_msgs::JointState joint_state;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";
    
        while (ros::ok()) {
            //update joint_state
            joint_state.header.stamp = ros::Time::now();
            joint_state.name.resize(9);
            joint_state.position.resize(9);
            joint_state.name[0] ="base_to_head";
            joint_state.position[0] = base_head;
            joint_state.name[1] ="base_to_right_leg";
            joint_state.position[1] = 0;
            joint_state.name[2] ="base_to_left_leg";
            joint_state.position[2] = 0;
    	    joint_state.name[3] ="right_leg_to_right_foot";
            joint_state.position[3] = 0;
    	    joint_state.name[4] ="left_leg_to_left_foot";
            joint_state.position[4] = 0;
    	    joint_state.name[5] ="right_foot_to_front_wheel";
            joint_state.position[5] = wheel_1;
    	    joint_state.name[6] ="right_foot_to_back_wheel";
            joint_state.position[6] = wheel_2;
    	    joint_state.name[7] ="left_foot_to_front_wheel";
            joint_state.position[7] = wheel_3;
    	    joint_state.name[8] ="left_foot_to_back_wheel";
            joint_state.position[8] = wheel_4;
    
            // update transform
            // (moving in a circle with radius)
            //改变二维平面的x,y的坐标，z轴垂直于xOy，故z的坐标是0
            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_state);
            broadcaster.sendTransform(odom_trans);
    
    
    	    // Create new robot state
    	    base_head += inc;
    	    if(base_head< -2.0||base_head> 2.0) base_head *= -1;
    	    wheel_1 += wheel_1_inc;
    	    if(wheel_1< -2.0||wheel_1> 2.0) wheel_1 *= -1;
    	    wheel_2 += wheel_2_inc;
    	    if(wheel_2< -2.0||wheel_2> 2.0) wheel_2 *= -1;
    	    wheel_3 += wheel_3_inc;
    	    if(wheel_3< -2.0||wheel_3> 2.0) wheel_3 *= -1;
    	    wheel_4 += wheel_4_inc;
    	    if(wheel_4< -2.0||wheel_4> 2.0) wheel_4 *= -1;
    
    	    angle += degree/4;
    
            // This will adjust as needed per iteration
            loop_rate.sleep();
        }
    
    
        return 0;
    }

![70 3][]

## Chapter7部分错误与解决方法： ##

**P275：**  
**问题：**执行以下命令时，出现错误提示：

$ check_urdf robot1.urdf
    Error:   Error document empty.
            at line 72 in /build/buildd/urdfdom 0.2.10+dfsg/urdf_parser/src/model.cpp
    ERROR: Model Parsing the xml failed

**解决：**在**.urdf**前添加路径

check_urdf /home/<user_name>/ROS/catkin_ws/src/learning_urdf/urdf/robot1.urdf

**P276：**  
**问题：**执行**$ roslaunch ...**命令后没有3D模型  
**解决：**在**<?xml version="1.0"?>**代码段，在倒数第二行添加，注意结尾是 **urdf.rviz**，否则会报错。

**P277：**  
**问题：**在urdf中添加package：

编译时一直报错，提示没有找到资源  
**解决：**我将含有**.dae**的文件夹（meshes）放在"**robot1\_description**"（功能包）中，相对路径改为

加载成功。另外**urdf**中不能含有中文

[https_www.cnblogs.com_jinee_p_5047021.html]: https://www.cnblogs.com/jinee/p/5047021.html
[70]: /images/20220517/b38eb3e2b5c74cc7aca534c204e7f1a2.png
[70 1]: /images/20220517/1dcbb01e29674e40b5f1deccfcb0ad3c.png
[70 2]: /images/20220517/769dc0c2c846452b9b49887a1bdffe3b.png
[70 3]: /images/20220517/e34f16d1c5c045e78d72868128fa72e8.png