ROS中rviz添加urdf文件显示机器人模型

下面为启动rviz并加载urdf文件格式机器人模型的代码：
虽然有一些不懂得，但是是可以启动并正确加载机器人的。

<launch>

  <arg name="model" default="/home/XXX/3D_model/src/robot1_description/urdf/robot1.urdf"/>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
  <param name="use_gui" value="$(arg gui)"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> 

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>

如下，为机器人的urdf文件内容：

<?xml version="1.0"?>
<robot name="origins">
    <link name="base_link">
        <visual>
                <geometry>
                    <box size="0.2 .3 .1"/>
                </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="white">
                <color rgba="1 1 1 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                    <box size="0.2 .3 0.1"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="100"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>

    </link>

    <link name="wheel_1">
        <visual>
                <geometry>
                    <cylinder length="0.05" radius="0.05"/>
                </geometry>
            <origin rpy="0 1.5 0" xyz="0 0 0"/>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="0.05" radius="0.05"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="10"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>
    </link>

    <link name="wheel_2">
        <visual>
                <geometry>
                    <cylinder length="0.05" radius="0.05"/>
                </geometry>
            <origin rpy="0 1.5 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="0.05" radius="0.05"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="10"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>

    </link>

    <link name="wheel_3">
        <visual>
                <geometry>
                    <cylinder length="0.05" radius="0.05"/>
                </geometry>
            <origin rpy="0 1.5 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="0.05" radius="0.05"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="10"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>
    </link>

    <link name="wheel_4">
        <visual>
                <geometry>
                    <cylinder length="0.05" radius="0.05"/>
                </geometry>
            <origin rpy="0 1.5 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="0.05" radius="0.05"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="10"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>

    </link>

    <joint name="base_to_wheel1" type="continuous">
        <parent link="base_link"/>
        <child link="wheel_1"/>
        <origin xyz="0.1 0.1 0"/>
                <axis xyz="1 0 0"/>
    </joint>

    <joint name="base_to_wheel2" type="continuous">
        <parent link="base_link"/>
        <child link="wheel_2"/>
        <origin xyz="-0.1 0.1 0"/>
                <axis xyz="1 0 0"/>
        </joint>

    <joint name="base_to_wheel3" type="continuous">
        <parent link="base_link"/>
        <child link="wheel_3"/>
        <origin xyz="0.1 -0.1 0"/>
                <axis xyz="1 0 0"/>

    </joint>

    <joint name="base_to_wheel4" type="continuous">
        <parent link="base_link"/>
        <child link="wheel_4"/>
        <origin xyz="-0.1 -0.1 0"/>
                <axis xyz="1 0 0"/> 
    </joint>
    <link name="arm_base">
<visual>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</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"/>
</joint>
<link name="arm_1">
<visual>
<geometry>
<box size="0.05 .05 0.5"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.05 .05 0.5"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="arm_1_to_arm_base" type="revolute">
<parent link="arm_base"/>
<child link="arm_1"/>
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.15"/>
<limit effort ="1000.0" lower="-2.0" upper="1.0" velocity="0.5"/>
</joint>
<link name="arm_2">
<visual>
<geometry>
<box size="0.05 0.05 0.5"/>
</geometry>
<origin rpy="0 0 0" xyz="0.06 0 0.15"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.05 .05 0.5"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="arm_2_to_arm_1" type="revolute">
<parent link="arm_1"/>
<child link="arm_2"/>
<axis xyz="1 0 0"/>
<origin xyz="0.0 0 0.45"/>
<limit effort ="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>
</joint>
<joint name="left_gripper_joint" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 -1.57 0" xyz="0.06 0 0.4"/>
<parent link="arm_2"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_gripper_joint" type="revolute">
<axis xyz="0 0 -1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 -1.57 0" xyz="0.06 0 0.4"/>
<parent link="arm_2"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
</robot>

发布消息的应用程序，可以控制机械臂的移动和*的转动，但是车还没有动………

#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;
    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 height=0;
    // robot state
    double inc= 0.005, base_arm_inc= 0.005, arm1_armbase_inc= 0.005, arm2_arm1_inc= 0.005, gripper_inc= 0.005, tip_inc= 0.005;
    double angle= 0 ,base_arm = 0, arm1_armbase = 0, arm2_arm1 = 0, gripper = 0, tip = 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(11);
        joint_state.position.resize(11);
        joint_state.name[0] ="base_to_arm_base";
        joint_state.position[0] = base_arm;
        joint_state.name[1] ="arm_1_to_arm_base";
        joint_state.position[1] = arm1_armbase;
        joint_state.name[2] ="arm_2_to_arm_1";
        joint_state.position[2] = arm2_arm1;
    joint_state.name[3] ="left_gripper_joint";
        joint_state.position[3] = gripper;
    joint_state.name[4] ="left_tip_joint";
        joint_state.position[4] = tip;
    joint_state.name[5] ="right_gripper_joint";
        joint_state.position[5] = gripper;
    joint_state.name[6] ="right_tip_joint";
        joint_state.position[6] = tip;
    joint_state.name[7] ="base_to_wheel1";
        joint_state.position[7] =height ;
    joint_state.name[8] ="base_to_wheel2";
        joint_state.position[8] = height;
    joint_state.name[9] ="base_to_wheel3";
        joint_state.position[9] = height;
    joint_state.name[10] ="base_to_wheel4";
        joint_state.position[10] = height;

        height+=0.005;





        // 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_state);
        broadcaster.sendTransform(odom_trans);


    // Create new robot state
        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;
        gripper += gripper_inc;
        if (gripper<0 || gripper>1) gripper_inc *= -1;

          angle += degree/4;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }


    return 0;
}