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;}